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

　　國外有個公司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