Kinect V2开发（5）读关节数据

Kinect能取得Depth（物体与传感器的距离信息）和BodyIndex(人物索引)，基于这些数据可以侦测到人体的骨骼信息并追踪，在Kinect V2的SDK 2.0中，它最多可以同时获取到6个人、每个人25个关节点的信息，并且通过深度摄像头，可以同时获取到这些关节点的坐标。此时的坐标使用的是Camera Space，也就是摄像机空间坐标系,代表了物体距离深度摄像头的距离。
Kinect的人体姿势，是向学习了基于庞大数量的姿势信息的识别器里，输入人体区域的信息来推定的（注：因为男女老少高矮胖瘦体形各不相同，所以必须基于神经网络的数据库才能准确识别人体）。这个技术出自微软在IEEE CVPR 2011（计算机视觉及模式认识领域的首位会议）发表，获奖Best Paper。
Microsoft ResearchReal-Time Human Pose Recognition in Parts from a Single Depth Image
文章我正在读，之后有时间会传一篇阅读笔记。

要读取骨骼数据，前面的步骤和之前一样，要先通过IKinectSensor来取得 IBodyFrameSource，然后开启 IBodyFrameReader，之后再在主循环里取得 IBodyFrame里面的数据，但是在IBodyFrame里面实际上包括了所有人的数据，需要通过GetAndRefreshBodyData()这个函数写入IBody这个类里面再进行个别读取。可以另外设置一个变量代表一个IBody阵列，写入数据后即可以读取每个人的骨架资料。
通过IBodyFrameSource 的 get_BodyCount() 可以取得iBodyCount 代表可以读取到追踪的人数，目前来说最多就是6个人。

通过IBody中的get_IsTracked()这个函数可以判断某个人是否正在被追踪

通过IBody中的GetJoints()这个函数可以得到所有关节点的位置信息。

位置信息被定义成Joint这个类别，里面包含三个参数，第一个是JointType，代表是哪个关节点，我们可以在JointType Enumeration中看到具体的列举；第二个是Position，是用CameraSpacePoint 来记录这个关节点在摄像头空间坐标系里的位置（如果要用来在2D图像中显示，需要做坐标转换）；第三个是TrackingState，用来记录这个关节的追踪状态。

通过IBody中的GetJointOrientations()这个函数可以得到关节点的方向

IBody中还有两个函数get_HandRightState() 和 get_HandLeftState() 可以用来获取两手的状态数据。
我做了一个上半身骨骼信息读取，效果图如下：

用户编号是看起来是随机的，我自己测试的时候每一次编号都不太一样，但是在0-5范围，位置变化的时候不一定能读取到几个关节，离Kinect比较近的话只能读到一个关节。然后这个位置和方向的数据是读出来了，但是是否正确还不知道怎么验证。
代码如下：

#include <iostream>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <Kinect.h>

using namespace std;
using namespace cv;

const   string  get_name(int n);    //此函数判断出关节点的名字

int main(void)
{
    // 1a.获取感应器
    IKinectSensor* pSensor = nullptr;
    GetDefaultKinectSensor(&pSensor);
    // 1b. 打开感应器
    pSensor->Open();

    /****************2.打开深度图像阅读器************************/
    // 取得深度数据
    IDepthFrameSource* pDepthSource = nullptr;
    pSensor->get_DepthFrameSource(&pDepthSource);
    // 取得深度数据的描述信息（宽、高）
    int        iDepthWidth = 0;
    int        iDepthHeight = 0;
    IFrameDescription* pDepthDescription = nullptr;
    pDepthSource->get_FrameDescription(&pDepthDescription);
    pDepthDescription->get_Width(&iDepthWidth);
    pDepthDescription->get_Height(&iDepthHeight);
    // 打开深度数据阅读器
    IDepthFrameReader* pDepthReader = nullptr;
    pDepthSource->OpenReader(&pDepthReader);
    pDepthDescription->Release();
    pDepthDescription = nullptr;
    // 释放变量pDepthSource
    pDepthSource->Release();
    pDepthSource = nullptr;

    /*******************3.打开Body数据的阅读器*******************/
    // 取得Body数据
    IBodyFrameSource* pBodySource = nullptr;
    pSensor->get_BodyFrameSource(&pBodySource);
    // 取得Body数据的描述信息（数量）
    int        iBodyCount = 0;
    pBodySource->get_BodyCount(&iBodyCount);
    IBody** aBody = new IBody*[iBodyCount];
    for (int i = 0; i < iBodyCount; ++i)
        aBody[i] = nullptr;
    // 打开Body数据阅读器
    IBodyFrameReader* pBodyReader = nullptr;
    pBodySource->OpenReader(&pBodyReader);
    // 释放变量pBodySource
    pBodySource->Release();
    pBodySource = nullptr;

    /*******************4.为显示深度图像做准备******************/
    Mat img16(iDepthHeight, iDepthWidth, CV_16UC1);
    Mat img8(iDepthHeight, iDepthWidth, CV_8UC1);


    while (1)
    {
        // 4a. 深度图像的转化以及显示
        IDepthFrame * pDepthFrame = nullptr;
        while (pDepthReader->AcquireLatestFrame(&pDepthFrame) != S_OK);
        pDepthFrame->CopyFrameDataToArray(iDepthWidth * iDepthHeight, (UINT16 *)img16.data);
        img16.convertTo(img8, CV_8UC1, 255.0 / 4500);
        imshow("Depth Img", img8);

        // 4b. 获取Body数据
        IBodyFrame* pBodyFrame = nullptr;
        while (pBodyReader->AcquireLatestFrame(&pBodyFrame) != S_OK);
        if (pBodyFrame->GetAndRefreshBodyData(iBodyCount, aBody) == S_OK)
        {
            int iTrackedBodyCount = 0;

            // 4c. 遍历每个人
            for (int i = 0; i < iBodyCount; ++i)
            {
                IBody* pBody = aBody[i];

                // 判断这个人是不是正在被追踪
                BOOLEAN bTracked = false;
                if ((pBody->get_IsTracked(&bTracked) == S_OK) && bTracked)
                {
                    ++iTrackedBodyCount;
                    cout << "User " << i << " is under tracking!" << endl;

                    // 获取关节位置
                    int count = 0;
                    Joint aJoints[JointType::JointType_Count];
                    if (pBody->GetJoints(JointType::JointType_Count, aJoints) != S_OK)
                    {
                        cerr << "Get joints fail" << endl;
                    }

                    // 获取关节方向
                    JointOrientation aOrientations[JointType::JointType_Count];
                    if (pBody->GetJointOrientations(JointType::JointType_Count, aOrientations) != S_OK)
                    {
                        cerr << "Get joints fail" << endl;
                    }

                    // 输出信息
                    for (int j = 0; j < JointType_Count; j++)
                    {
                        //判断该点是否被追踪
                        if (aJoints[j].TrackingState == TrackingState_Tracked)
                            continue;
                        //获取关节的名字
                        string rt = get_name(aJoints[j].JointType);
                        //输出关节信息
                        if (rt != "NULL")
                        {
                            count++;
                            cout << "   " << rt << "  tracked" << endl;
                            cout << "\n\t position: " << aJoints[j].Position.X <<"," << aJoints[j].Position.Y << "," << aJoints[j].Position.Z
                                << "\n\t orientation: " << aOrientations[j].Orientation.w<< ","<< aOrientations[j].Orientation.x << "," << aOrientations[j].Orientation.y << "," << aOrientations[j].Orientation.z <<endl;
                        }
                    }
                        cout << count << "joints tracked" << endl << endl;
                }
            }
            //判断这一时刻有几个人在被追踪
            if (iTrackedBodyCount > 0)
                cout << "Total " << iTrackedBodyCount << " bodies in this time\n" << endl;
            else
            {
                cerr << "Can't read body data" << endl;
            }
        }
        // 4d. release frame
        pDepthFrame->Release();
        pBodyFrame->Release();

        if (waitKey(30) == VK_ESCAPE)
            break;
        //为避免数据刷太快，每秒钟更新一次
        Sleep(1000);    
    }
    delete[] aBody;
    // 4e. release frame
    pDepthReader->Release();
    pBodyReader->Release();

    // 1c.关闭感应器
    pSensor->Close();
    // 1d.释放感应器
    pSensor->Release();
    pSensor = nullptr;

    return 0;
}

const   string  get_name(int n)
{
    switch (n)
    {
    case    2:return    "Neck"; break;
    case    3:return    "Head"; break;
    case    4:return    "Left shoulder"; break;
    case    8:return    "Right shoulder"; break;
    case    7:return    "Left hand"; break;
    case    11:return   "Right hand"; break;
    case    22:return   "Left thumb"; break;
    case    24:return   "Right thumb"; break;
    default :return "NULL";
    }
}