Kinect2.0关节角度获取

　　通过Kinect获取到关节的三维坐标点后可以根据向量点积或叉积公式计算出关节角度：

$\vec{a}\cdot \vec{b} = |\vec{a}||\vec{b}|cos\theta$

vector1.Normalize();
vector2.Normalize();
double cosinus = DotProduct(vector1, vector2);

double angle = (Math.Acos(cosinus) * (180.0 / Math.PI));

　　在DirectXMath数学库中也有现成的计算向量夹角的函数XMVector3AngleBetweenVectors：

XMVECTOR  XMVector3AngleBetweenVectors( XMVECTOR V1, XMVECTOR V2 ); //返回向量V1、V2间的夹角[angle, angle,angle, angle]，单位为弧度

　　下面的部分代码将获取到的骨骼数据进行平滑，然后计算出关节角度：

/// Handle new body data
void CBodyBasics::ProcessBody(IBody* pBody)
{
    HRESULT hr;
    BOOLEAN bTracked = false;
    hr = pBody->get_IsTracked(&bTracked);  // Retrieves a boolean value that indicates if the body is tracked

    if (SUCCEEDED(hr) && bTracked)  // 判断是否追踪到骨骼
    {
        Joint joints[JointType_Count];
        HandState leftHandState = HandState_Unknown;
        HandState rightHandState = HandState_Unknown;

        DepthSpacePoint *depthSpacePosition = new DepthSpacePoint[_countof(joints)]; // 存储深度坐标系中的关节点位置

        pBody->get_HandLeftState(&leftHandState);  // 获取左右手状态
        pBody->get_HandRightState(&rightHandState);

        hr = pBody->GetJoints(_countof(joints), joints); // 获得25个关节点
        if (SUCCEEDED(hr))
        {
            // Filtered Joint
            filter.Update(joints);
            const DirectX::XMVECTOR* vec = filter.GetFilteredJoints();    // Retrive Filtered Joints
            

            float angle = Angle(vec, JointType_WristRight, JointType_ElbowRight, JointType_ShoulderRight); // Get ElbowRight joint angle
            char s[10];
            sprintf_s(s, "%.0f", angle);
            std::string strAngleInfo = s;
            putText(skeletonImg, strAngleInfo, cvPoint(0, 50), CV_FONT_HERSHEY_COMPLEX, 0.5, cvScalar(0, 0, 255)); // 屏幕上显示角度信息

            for (int type = 0; type < JointType_Count; type++)
            {
                if (joints[type].TrackingState != TrackingState::TrackingState_NotTracked)
                {
                    float x = 0.0f, y = 0.0f, z = 0.0f;
                    // Retrieve the x/y/z component of an XMVECTOR Data and storing that component's value in an instance of float referred to by a pointer
                    DirectX::XMVectorGetXPtr(&x, vec[type]);
                    DirectX::XMVectorGetYPtr(&y, vec[type]);
                    DirectX::XMVectorGetZPtr(&z, vec[type]);

                    CameraSpacePoint cameraSpacePoint = { x, y, z };        
                    m_pCoordinateMapper->MapCameraPointToDepthSpace(cameraSpacePoint, &depthSpacePosition[type]); //将关节点坐标从摄像机坐标系转到深度坐标系以显示
                }
            }

            DrawBody(joints, depthSpacePosition);
            DrawHandState(depthSpacePosition[JointType_HandLeft], leftHandState);
            DrawHandState(depthSpacePosition[JointType_HandRight], rightHandState);
        }

        delete[] depthSpacePosition;
    }

    cv::imshow("skeletonImg", skeletonImg);
    cv::waitKey(5); // 延时5ms
}




FLOAT CBodyBasics::Angle(const DirectX::XMVECTOR* vec, JointType jointA, JointType jointB, JointType jointC)
{
    float angle = 0.0;
    
    XMVECTOR vBA = XMVectorSubtract(vec[jointB], vec[jointA]);
    XMVECTOR vBC = XMVectorSubtract(vec[jointB], vec[jointC]);

    XMVECTOR vAngle = XMVector3AngleBetweenVectors(vBA, vBC);

    angle = XMVectorGetX(vAngle) * 180.0 * XM_1DIVPI;    // XM_1DIVPI: An optimal representation of 1 / π

    return angle;
}