kinect v2获取人体骨骼数据

#include <iostream>
#include <string>
#include <WS2tcpip.h>
#pragma comment (lib, "ws2_32.lib")
#include <chrono>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>        // 核心功能，包括矩阵和数组操作
#include <opencv2/imgproc.hpp>     // 图像处理功能
#include <opencv2/highgui.hpp>     // 图形用户界面相关功能，用于显示图像和交互
#include <opencv2/video.hpp>       // 视频处理功能
#include <Kinect.h>
#include <fstream>
#include<chrono>
#include<thread>using namespace cv;void DrawLine(cv::Mat& Img, const Joint& r1, const Joint& r2, ICoordinateMapper* pMapper)
{if (r1.TrackingState == TrackingState_Tracked && r2.TrackingState == TrackingState_Tracked){ColorSpacePoint p1, p2;pMapper->MapCameraPointToColorSpace(r1.Position, &p1);pMapper->MapCameraPointToColorSpace(r2.Position, &p2);line(Img, Point(p1.X, p1.Y), Point(p2.X, p2.Y), Vec3b(0, 0, 255), 5);circle(Img, Point(p1.X, p1.Y), 10, (0, 0, 255), -1);circle(Img, Point(p2.X, p2.Y), 10, (0, 0, 255), -1);}
}int main()
{IKinectSensor* pSensor = nullptr;GetDefaultKinectSensor(&pSensor); pSensor->Open(); IColorFrameSource* pFrameSource = nullptr;pSensor->get_ColorFrameSource(&pFrameSource);int iWidth = 0, iHeight = 0;IFrameDescription* pFrameDescription = nullptr;pFrameSource->get_FrameDescription(&pFrameDescription);pFrameDescription->get_Width(&iWidth);pFrameDescription->get_Height(&iHeight);IColorFrameReader* pColorFrameReader = nullptr;pFrameSource->OpenReader(&pColorFrameReader);pFrameDescription->Release();pFrameDescription = nullptr;pFrameSource->Release();pFrameSource = nullptr;UINT uBufferSize = 0;cv::Mat mColorImg(iHeight, iWidth, CV_8UC4);uBufferSize = iHeight * iWidth * 4 * sizeof(BYTE);IBodyFrameReader* pBodyFrameReader = nullptr;IBody** aBodyData = nullptr;INT32 iBodyCount = 0;IBodyFrameSource* pBodySource = nullptr;pSensor->get_BodyFrameSource(&pBodySource);pBodySource->get_BodyCount(&iBodyCount);aBodyData = new IBody*[iBodyCount];for (int i = 0; i < iBodyCount; ++i)aBodyData[i] = nullptr;		pBodySource->OpenReader(&pBodyFrameReader);pBodySource->Release();pBodySource = nullptr;ICoordinateMapper* pCoordinateMapper = nullptr;pSensor->get_CoordinateMapper(&pCoordinateMapper);cv::namedWindow("Body Image", cv::WINDOW_NORMAL);cv::resizeWindow("Body Image", 560, 360);	while (true){IColorFrame* pColorFrame = nullptr;if (pColorFrameReader->AcquireLatestFrame(&pColorFrame) == S_OK){pColorFrame->CopyConvertedFrameDataToArray(uBufferSize, mColorImg.data, ColorImageFormat_Bgra);pColorFrame->Release();}cv::Mat mImg = mColorImg.clone();IBodyFrame* pBodyFrame = nullptr;if (pBodyFrameReader->AcquireLatestFrame(&pBodyFrame) == S_OK){// std::cout << "读取人体帧数据成功" << std::endl;if (pBodyFrame->GetAndRefreshBodyData(iBodyCount, aBodyData) == S_OK){// std::cout << "获取骨骼帧中的骨骼数据成功" << std::endl;for (int i = 0; i < iBodyCount; ++i){IBody* pBody = aBodyData[i];BOOLEAN bTracked = false;if ((pBody->get_IsTracked(&bTracked) == S_OK) && bTracked){// std::cout << "骨骼已被跟踪" << std::endl;// std::cout << "跟踪序号：" << i << std::endl;Joint aJoints[JointType_Count];if (pBody->GetJoints(JointType_Count, aJoints) == S_OK){//脊柱DrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_SpineMid], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_SpineMid], aJoints[JointType_SpineShoulder], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_Neck], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_Neck], aJoints[JointType_Head], pCoordinateMapper);//左 armDrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderLeft], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_ShoulderLeft], aJoints[JointType_ElbowLeft], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_ElbowLeft], aJoints[JointType_WristLeft], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_WristLeft], aJoints[JointType_HandLeft], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_HandLeft], aJoints[JointType_HandTipLeft], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_WristLeft], aJoints[JointType_ThumbLeft], pCoordinateMapper);//右armDrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderRight], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_ShoulderRight], aJoints[JointType_ElbowRight], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_ElbowRight], aJoints[JointType_WristRight], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_WristRight], aJoints[JointType_HandRight], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_HandRight], aJoints[JointType_HandTipRight], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_WristRight], aJoints[JointType_ThumbRight], pCoordinateMapper);//左legDrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_HipLeft], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_HipLeft], aJoints[JointType_KneeLeft], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_KneeLeft], aJoints[JointType_AnkleLeft], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_AnkleLeft], aJoints[JointType_FootLeft], pCoordinateMapper);//右legDrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_HipRight], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_HipRight], aJoints[JointType_KneeRight], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_KneeRight], aJoints[JointType_AnkleRight], pCoordinateMapper);DrawLine(mImg, aJoints[JointType_AnkleRight], aJoints[JointType_FootRight], pCoordinateMapper);// *************************保存TXT数据*************************//创建一个名为"data.txt"的文本文件，并清空其中的内容std::ofstream outfile("data_test139.txt", std::ios::trunc);for (int s = 0; s < JointType_Count; s++){outfile << aJoints[s].Position.X << "\t" << aJoints[s].Position.Y << "\t" << aJoints[s].Position.Z << std::endl;}outfile << std::endl;std::cout << "骨骼关节点数据写入成功" << std::endl;}}//追踪失败else {if (pBody->get_IsTracked(&bTracked) != S_OK) {std::cerr << "骨骼追踪失败,Failed to read body tracking state. Error code: " << pBody->get_IsTracked(&bTracked) << std::endl;}//未被追踪else if (!bTracked){std::cerr << "骨骼未被追踪,Body is not being tracked." << std::endl;}}}}else{std::cerr << "Can't read body data" << std::endl;}pBodyFrame->Release();}cv::imshow("Body Image", mImg);if (cv::waitKey(5) == VK_ESCAPE){break;}}pSensor->Close();pSensor->Release();pSensor = nullptr;return 0;
}