Kinect2.0关节角度获取

 

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

$$vec{a}cdot vec{b} = |vec{a}||vec{b}|cos heta$$

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;
}

　　国外有个公司vitruvius已经将关节角度获取、背景去除、手势识别、Avateering等功能做的简单易用，可以在其网站上下载免费的版本试用：

参考：

https://vitruviuskinect.com/

DirectXMath Library 3D Vector Geometric Functions

Using the Kinect Sensor to Calculate Body Segment Angles

Find the angle between two line segments

LightBuzz.Vitruvius/Core/Vector3.cs

How to select the users to track in C++

【D3D11游戏编程】学习笔记二：XNAMath之XMVECTOR