Kinect V2开发（7）测量骨骼点高度以及骨骼角度

Kinect得到的是镜面图像

骨骼角度测量
通过Kinect获取到关节的三维坐标点后可以根据向量点积或叉积公式计算出关节角度
在DirectXMath数学库中也有现成的计算向量夹角的函数XMVector3AngleBetweenVectors：
Namespace Use DirectX
Header DirectXMath.h

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

DOUBLE CBodyBasics::Angle(const DirectX::XMVECTOR* vec, JointType jointA, JointType jointB, JointType jointC)
{
    double 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;
}

在获取关节信息后调用，这里测量的是 右肘和右肩连线 与右肘和右腕连线之间的角度

const DirectX::XMVECTOR* vec = filterKinect.GetFilteredJoints();    // Retrive Filtered Joints

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

高度测量
地面检测的难度比较大，在IBodyFrame类里面有一个get_FloorClipPlane方法，函数的参数是vector4 *floorClipPlane，返回值是HRESULT类型。vector4结构体里有4个float类型的数据成员，分别是x,y,z,w，这4个参数即为地面方程的系数（x,y,z）和常数项(w)，将这四个数分别赋值给A，B，C，D，则可得地面方程为
A·x+B·y+C·z+D=0
此处的地面方程是在Kinect的深度相机坐标系下得到的，常数项表示Kinect的深度相机的中心点到地面的距离。知道骨骼点的三维坐标和地面方程，则可计算关节点离地面的距离
height =

                    Vector4 vFloorClipPlane;
                    float A, B, C, D;
                    float x, y, z;
                    float baseH;
                    myBodyFrame->get_FloorClipPlane(&vFloorClipPlane);
                    A = vFloorClipPlane.x;
                    B = vFloorClipPlane.y;
                    C = vFloorClipPlane.z;
                    D = vFloorClipPlane.w;
                    x = joints[JointType_SpineBase].Position.X;
                    y = joints[JointType_SpineBase].Position.Y;
                    z = joints[JointType_SpineBase].Position.Z;
                    baseH = (A*x + B*y + C*z + D) / sqrt(pow(A, 2) + pow(B, 2) + pow(C, 2));
                    cout << "JointType_SpineBase的高度是  " << baseH << "\tm" << endl;

Bones Hierarchy：
我们使用骨骼跟踪系统定义的关节来定义骨骼的层次结构。 层次结构以髋关节中心关节作为根，并延伸到脚、头和手：

通过IBody::GetJointOrientations函数可以获取到关节的姿态：

HRESULT = pBody -> GetJointOrientations(_countof(joints), jointOrientations); 

关节姿态是一个结构体

typedef struct _JointOrientation {
    JointType JointType;
    Vector4 Orientation;  // quaternion
} JointOrientation;

1. 末端节点（Head、HandTip、Thumb、Feet）不含有姿态信息，这些关节返回的四元数各分量都为0：
   
2. 所有的关节姿态描述都以摄像机坐标系为参考，当人体站正朝向Kinect时SpineBase关节处的坐标系如下图所示。此时返回的关节四元数理论上应为(w,x,y,z)=(0,0,1,0)，对应的Z-Y-X欧拉角为(180°,0,180°)或(0,180°,0)
   
3. 1. 关节坐标系的Y轴沿着骨骼的方向，Z轴为骨骼转动轴（ Z-axis points to the direction that makes sense for the joint to rotate），X轴与Y轴和Z轴垂直，构成右手坐标系
      Bone direction(Y green) - always matches the skeleton.
      Normal(Z blue) - joint roll, perpendicular to the bone
      Binormal(X red) - perpendicular to the bone and normal
      

---

参考
Joint Orientation
Adventures in Motion Capture: Using Kinect Data (Part 1)
Meaning of Rotation Data of K4W v2