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);
							//左 arm
							DrawLine(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);
							//右arm
							DrawLine(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);
							//左leg
							DrawLine(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);
							//右leg
							DrawLine(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;
}