Kinect2.0骨骼跟蹤與數據平滑

　　Kinect v1和Kinect v2傳感器的配置比較:

		Kinect v1	Kinect v2
顏色（Color）	分辨率（Resolution）	640×480	1920×1080
	fps	30fps	30fps
深度（Depth）	分辨率（Resolution）	320×240	512×424
	fps	30fps	30fps
人物數量（Player）		6人	6人
人物姿勢（Skeleton）		2人	6人
関節（Joint）		20関節／人	25関節／人
手的開閉狀態（Hand State）		△（Developer Toolkit）	○（SDK）
檢測範囲（Range of Detection）		0.8～4.0m	0.5～4.5m
角度（Angle）（Depth）	水平（Horizontal）	57度	70度
	垂直（Vertical）	43度	60度
（Tilt Motor）		○	×（手動）
複數的App		×（単一的App）	○

　　Kinect2.0的數據獲取流程如下圖所示：

　　參考Kinect for Windows v2.0 SDK中的Body Basics-D2D（C++ Direct2D sample）示例程序，可以自己構建一個基本的骨骼追蹤程序。

　　被”骨骼跟蹤”的用戶位置由攝像機坐標系下的X、Y、Z坐標表示，不同於彩色圖像空間坐標系，該坐標系是三維的，以米為單位。Z軸表示紅外攝像頭光軸，與圖像平面垂直。Kinect傳感器的位置會影響骨骼坐標。如果Kinect被放置在非水平面上或者有可能通過傳動馬達調整有效視角范圍。在這種情況下，Y軸就不是垂直於水平面的，或者說與重力方向不平行，那么計算得到的坐標系可能並非是標准形式。因此，在最終的圖像中，盡管人筆直地站立，但卻顯示為傾斜的。

 　　為了在屏幕上顯示關節點，需要進行Kinect空間坐標轉換，下面代碼將攝像機空間坐標轉換為深度圖像坐標：

CameraSpacePoint cameraSpacePoint = { x, y, z };   // 獲取到的關節坐標
m_pCoordinateMapper -> MapCameraPointToDepthSpace(cameraSpacePoint, &depthSpacePosition[i]); // 轉換到深度圖像坐標系

• Camera space

　　Camera space refers to the 3D coordinate system used by Kinect. The coordinate system is defined as follows:

• The origin (x=0, y=0, z=0) is located at the center of the IR sensor on Kinect
• X grows to the sensor’s left
• Y grows up (note that this direction is based on the sensor’s tilt)
• Z grows out in the direction the sensor is facing
• 1 unit = 1 meter

[The camera space coordinate system]

　　Any Kinect Tracking algorithm that operates in 3D (e.g. Skeletal Tracking) stores its results in camera space. In some cases, you might want to project one of these points to a row/column location on the depth image for further processing. In that case, you would be mapping from camera space to depth space.

• Depth space

　　Depth space is the term used to describe a 2D location on the depth image. Think of this as a row/column location of a pixel where x is the column and y is the row. So x=0, y=0 corresponds to the top left corner of the image and x=511, y=423 (width-1, height-1) is the bottom right corner of the image. In some cases, a z value is needed to map out of depth space. For these cases, simply sample the depth image at the row/column in question, and use that value (which is depth in millimeters) directly as z.

• Color space

　　The color sensor on Kinect is offset a bit from the sensor that generates depth and infrared. As a result, the depth sensor and the color sensor see a slightly different view of the world. If you want to find the color that corresponds to given pixel on the depth image, you’ll have to convert its position to color space. To color space describes a 2D point on the color image, just like depth space does for the depth image. So a position in color space is a row/column location of a pixel on the image, where x=0, y=0 is the pixel at the top left of the color image, and x=1919, y=1079 (width-1, height-1) corresponds to the bottom right.

 

---

　　與任何測量系統一樣，Kinect獲取到的關節坐標數據包含了許多噪聲。影響噪聲特性和大小的因素有很多（room lighting; a person’s body size; the person’s distance from the sensor array; the person’s pose (for example, for hand data, if the person’s hand is open or fisted); location of the sensor array; quantization noise; rounding effects introduced by computations; and so on）。誤差從來源看可分為系統誤差和偶然誤差，如下圖b所示為系統誤差（由於儀器本身不精確，或實驗方法粗略，或實驗原理不完善而產生的），要減小系統誤差，必須校准測量儀器，改進實驗方法，設計在原理上更為完善的實驗。圖d為偶然誤差（由各種偶然因素而產生的），偶然誤差總是有時偏大，有時偏小，並且偏大偏小的概率相同。因此，可以多進行幾次測量，求出幾次測得的數值的平均值，這個平均值比一次測得的數值更接近於真實值。由於噪聲的性質各不相同，適用的濾波方法也不一樣。因此，Kinect開發者需要結合多種濾波方法來獲得滿意的效果。

　　濾波會帶來一定的延遲，The joint filtering latency is how much time it takes for filter output to catch up to the actual joint position when there is a movement in a joint. User research shows that 72% of people start noticing this delay when latency is more than 100 msec, and therefore, it is suggested that developers aim for an overall latency of 100 msec 

[latency added by joint filtering is the lag between output and input when there is movement in input data]

　　A useful technique to reduce latency is to tweak the joint filter to predict the future joint positions. That is, the filter output would be a smoothed estimate of joint position in subsequent frames. If forecasting is used, then joint filtering would reduce the overall latency. However, since the forecasted outputs are estimated from previous data, the forecasted data may not always be accurate, especially when a movement is suddenly started or stopped. Forecasting may propagate and magnify the noise in previous data to future data, and hence, may increase the noise.

　　One should understand how latency and smoothness affect the user experience, and identify which one is more important to create a good experience. Then, carefully choose a filtering method and fine-tune its parameters to match the specific needs of the application.  In most Kinect applications, data output from the ST system is used for a variety of purposes. Joints have different characteristics from one another in terms of how fast they can move, or how they are used to create the overall experience. For example, in some applications, a person’s hands can move much faster than the spine joint, and therefore, one needs to use different filtering techniques for hands than the spine and other joints. So, there is no filtering solution that fits the needs of all use cases for all joints. 

　　The joint filter implementation in a typical application receives joint positions from ST as input in each frame, and returns the filtered joint positions as output. The filter treats each joint’s x, y, and z coordinates independently from other joints or other dimensions. That is, a filter is independently applied to the x, y, and z coordinate of each joint separately—and potentially each with different filtering parameters. Note that, though it is typical to directly filter the Cartesian position data returned by ST, one can apply the same filtering techniques to any data calculated from joint positions.

 　　為了能更好地理解各種濾波方法和設置濾波參數，我們可以研究濾波器的階躍信號和正弦信號的時間響應：

[Typical response of a filter to step function input]

　　Rise time shows how quickly the filter catches up with sudden changes in input, while overshoot, ringing, and settling time are indications of how well a filter can settle down after it has responded to a sudden change in input. A filter’s response to a step function does not reveal all of the useful characteristics of a filtering technique, because it only shows the filter’s response to sudden changes. It is also useful to study a filter’s response to sine waveform input, both in time and frequency domains. 

[Typical response of a filter to sine waveform input]

 骨骼數據濾波方法有多種：

• ARMA模型(auto regressive moving average model)自回歸滑動平均模型

• 一次移動平均平滑

• 二次移動平均平滑

• 一次指數平滑

• 二次指數平滑

• Adaptive Double Exponential Smoothing Filter

• 中值濾波（Median Filter）

• Taylor Series Filter

• Savitzky-Golay 濾波器

• Jitter Removal Filter

　　最簡單的就是一次移動平均濾波，但一次移動平滑法只適用於水平型歷史數據的預測，而對有明顯趨勢的數據會產生較大的系統誤差。例如，某零售企業食品部1988-1998年的銷售額資料如下表所示：

　　首先根據已知資料繪制散點圖。由圖中可看到銷售額逐年增加，若用簡單的一次移動平均法預測，就會出現預測值與實際值的滯后現象：

　　可以采用趨勢修正移動平均法進行預測來消除滯后，趨勢修正移動平均法（二次移動平均法）是指在簡單移動平均法或加權移動平均法的基礎上，計算變動趨勢值，並對變動趨勢值進行移動平均，求出若干期的變動趨勢平均值，再利用此趨勢平均值修正簡單移動平均或加權移動平均預測值，以消除原預測值的滯后影響的一種計算方法。

　　變動趨勢的計算公式為：$b_t=S_t-S_{t-1}$

　　式中:$b_t$--第t期的變動趨勢值；$S_t$--第t期的移動平均值（平滑值）；$S_{t-1}$--第t-1期的移動平均值。

　　利用變動趨勢值進行預測時，可按下述模型：

$$F_{t+T}=S_t+T \cdot \bar{b_t}$$

　　式中：$F_{t+T}$--預測值；T--間隔期數；$\bar{b_t}$--平均變動趨勢值。

　　選擇N=3進行移動平均，計算出移動平均值，趨勢值以及平均趨勢值。根據表中的數據，要預測1999年的銷售額，計算公式為：

$$F_{t+T}=S_t+T \cdot \bar{b_t}=191.3+2 \times 22.833=236.967$$

　　由此可看出，對於總體有上升或下降趨勢的時間序列，由於采用了變動趨勢修正移動平均法進行預測，消除了滯后或超前的偏差，能夠較真實地反映出事物發展的規律。

 

---

 　　在時間序列數據呈現線性趨勢時，移動平均值總是落后於觀察值數據的變化。二次移動平均法，正是要糾正這一滯后偏差，建立預測目標的線性時間關系數學模型，求得預測值。二次移動平均預測法解決了預測值滯后於實際觀察值的矛盾，適用於有明顯趨勢變動的市場現象時間序列的預測， 同時它還保留了一次移動平均法的優點。

　　二次指數平滑法的基本思想與二次移動平均法一致，Kinect for Windows 1.5, 1.6, 1.7, 1.8 SDK中關節數據平滑函數NuiTransformSmooth就是利用了Holt雙參數線性指數平滑法。Kinect SDK2.0中不再包含現成的平滑方法，需要自己去實現，可以參考How to use Joint Smoothing和Skeletal Joint Smoothing White Paper

Parameters

TRANSFORM_SMOOTH_PARAMETERS

 

Correction：修正參數，值越小，修正越多；fSmoothing：平滑參數，設置處理骨骼數據幀時的平滑量。值越大，平滑的越多，0表示不進行平滑；

JitterRadius：抖動半徑參數，設置修正的半徑。如果關節點“抖動”超過了設置的這個半徑，將會被糾正到這個半徑之內。（Jitter removal limits changes of each frame in order to dampen the spikes）；

Prediction: 預測超前期數，增大該值能減小濾波的延遲，但是會對突然的運動更敏感，容易造成過沖（可以設置合理的最大偏離半徑減輕該問題）；

MaxDeviationRadius：最大偏離半徑參數，用來和抖動半徑一起來設置抖動半徑的最大邊界。

 　　使用霍爾特雙參數指數平滑法來平滑關節數據減小抖動，可以參考下面的代碼（Jitter filter和初始化那幾行沒看懂，好像根上面的描述有沖突，代碼中把與上次平滑值偏差較大超過JitterRadius的點當作有效點）

　　KinectJointFilter.h

//--------------------------------------------------------------------------------------
// KinectJointFilter.h
//
// This file contains Holt Double Exponential Smoothing filter for filtering Joints
//
// Copyright (C) Microsoft Corporation. All rights reserved.
//--------------------------------------------------------------------------------------

#pragma once

#include <Windows.h>
#include <Kinect.h>
#include <DirectXMath.h>
#include <queue>


typedef struct _TRANSFORM_SMOOTH_PARAMETERS
{
    FLOAT   fSmoothing;             // [0..1], lower values closer to raw data
    FLOAT   fCorrection;            // [0..1], lower values slower to correct towards the raw data
    FLOAT   fPrediction;            // [0..n], the number of frames to predict into the future
    FLOAT   fJitterRadius;          // The radius in meters for jitter reduction
    FLOAT   fMaxDeviationRadius;    // The maximum radius in meters that filtered positions are allowed to deviate from raw data
} TRANSFORM_SMOOTH_PARAMETERS;



// Holt Double Exponential Smoothing filter
class FilterDoubleExponentialData
{
    public:
    DirectX::XMVECTOR m_vRawPosition;
    DirectX::XMVECTOR m_vFilteredPosition;
    DirectX::XMVECTOR m_vTrend;
    DWORD    m_dwFrameCount;
};



class FilterDoubleExponential
{
    public:
    FilterDoubleExponential() { Init(); }
    ~FilterDoubleExponential() { Shutdown(); }

    void Init( FLOAT fSmoothing = 0.25f, FLOAT fCorrection = 0.25f, FLOAT fPrediction = 0.25f, FLOAT fJitterRadius = 0.03f, FLOAT fMaxDeviationRadius = 0.05f )
    {
        Reset( fSmoothing, fCorrection, fPrediction, fJitterRadius, fMaxDeviationRadius );
    }

    void Shutdown()
    {
    }

    // Reset the filter when a skeleton is lost
    void Reset( FLOAT fSmoothing = 0.25f, FLOAT fCorrection = 0.25f, FLOAT fPrediction = 0.25f, FLOAT fJitterRadius = 0.03f, FLOAT fMaxDeviationRadius = 0.05f )
    {
        assert( m_pFilteredJoints );
        assert( m_pHistory );

        m_fMaxDeviationRadius = fMaxDeviationRadius; // Size of the max prediction radius Can snap back to noisy data when too high
        m_fSmoothing = fSmoothing;                   // How much smothing will occur.  Will lag when too high
        m_fCorrection = fCorrection;                 // How much to correct back from prediction.  Can make things springy
        m_fPrediction = fPrediction;                 // Amount of prediction into the future to use. Can over shoot when too high
        m_fJitterRadius = fJitterRadius;             // Size of the radius where jitter is removed. Can do too much smoothing when too high

        memset( m_pFilteredJoints, 0, sizeof( DirectX::XMVECTOR ) * JointType_Count );
        memset( m_pHistory, 0, sizeof( FilterDoubleExponentialData ) * JointType_Count );
    }

    void Update( IBody* const pBody );
    void Update( Joint joints[] );

    inline const DirectX::XMVECTOR* GetFilteredJoints() const { return &m_pFilteredJoints[0]; }

    private:
    DirectX::XMVECTOR m_pFilteredJoints[JointType_Count];
    FilterDoubleExponentialData m_pHistory[JointType_Count];
    FLOAT m_fSmoothing;
    FLOAT m_fCorrection;
    FLOAT m_fPrediction; 

    FLOAT m_fJitterRadius;
    FLOAT m_fMaxDeviationRadius;

    void Update( Joint joints[], UINT JointID, TRANSFORM_SMOOTH_PARAMETERS smoothingParams );
};

　　KinectJointFilter.cpp

//--------------------------------------------------------------------------------------
// KinectJointFilter.cpp
//
// This file contains Holt Double Exponential Smoothing filter for filtering Joints
//
// Copyright (C) Microsoft Corporation. All rights reserved.
//--------------------------------------------------------------------------------------

//#include "stdafx.h"
#include "KinectJointFilter.h"

using namespace DirectX;

//-------------------------------------------------------------------------------------
// Name: Lerp()
// Desc: Linear interpolation between two floats
//-------------------------------------------------------------------------------------
inline FLOAT Lerp( FLOAT f1, FLOAT f2, FLOAT fBlend )
{
    return f1 + ( f2 - f1 ) * fBlend;
}

//--------------------------------------------------------------------------------------
// if joint is 0 it is not valid.
//--------------------------------------------------------------------------------------
inline BOOL JointPositionIsValid( XMVECTOR vJointPosition )
{
    return ( XMVectorGetX( vJointPosition ) != 0.0f ||
        XMVectorGetY( vJointPosition ) != 0.0f ||
        XMVectorGetZ( vJointPosition ) != 0.0f );
}

//--------------------------------------------------------------------------------------
// Implementation of a Holt Double Exponential Smoothing filter. The double exponential
// smooths the curve and predicts.  There is also noise jitter removal. And maximum
// prediction bounds.  The paramaters are commented in the init function.
//--------------------------------------------------------------------------------------
void FilterDoubleExponential::Update( IBody* const pBody )
{
    assert( pBody );

    // Check for divide by zero. Use an epsilon of a 10th of a millimeter
    m_fJitterRadius = XMMax( 0.0001f, m_fJitterRadius ); //Returns the larger of the two input objects

    TRANSFORM_SMOOTH_PARAMETERS SmoothingParams;

    Joint joints[JointType_Count];

    pBody->GetJoints( _countof(joints), joints );
    for( INT i = 0; i < JointType_Count; i++ )
    {
        SmoothingParams.fSmoothing = m_fSmoothing;
        SmoothingParams.fCorrection = m_fCorrection;
        SmoothingParams.fPrediction = m_fPrediction;
        SmoothingParams.fJitterRadius = m_fJitterRadius;
        SmoothingParams.fMaxDeviationRadius = m_fMaxDeviationRadius;

        // If inferred, we smooth a bit more by using a bigger jitter radius
        Joint joint = joints[i];
        if( joint.TrackingState == TrackingState::TrackingState_Inferred )
        {
            SmoothingParams.fJitterRadius *= 2.0f;
            SmoothingParams.fMaxDeviationRadius *= 2.0f;
        }

        Update( joints, i, SmoothingParams );
    }
}

void FilterDoubleExponential::Update( Joint joints[] )
{
    // Check for divide by zero. Use an epsilon of a 10th of a millimeter
    m_fJitterRadius = XMMax( 0.0001f, m_fJitterRadius );

    TRANSFORM_SMOOTH_PARAMETERS SmoothingParams;
    for( INT i = 0; i < JointType_Count; i++ )
    {
        SmoothingParams.fSmoothing = m_fSmoothing;
        SmoothingParams.fCorrection = m_fCorrection;
        SmoothingParams.fPrediction = m_fPrediction;
        SmoothingParams.fJitterRadius = m_fJitterRadius;
        SmoothingParams.fMaxDeviationRadius = m_fMaxDeviationRadius;

        // If inferred, we smooth a bit more by using a bigger jitter radius
        Joint joint = joints[i];
        if( joint.TrackingState == TrackingState::TrackingState_Inferred )
        {
            SmoothingParams.fJitterRadius *= 2.0f;
            SmoothingParams.fMaxDeviationRadius *= 2.0f;
        }

        Update( joints, i, SmoothingParams ); // 對每個關節數據分別進行平滑濾波
    }

}

void FilterDoubleExponential::Update( Joint joints[], UINT JointID, TRANSFORM_SMOOTH_PARAMETERS smoothingParams )
{
    XMVECTOR vPrevRawPosition;       // x(t-1)
    XMVECTOR vPrevFilteredPosition;  // 前一期平滑值S(t-1)
    XMVECTOR vPrevTrend;             // 前一期趨勢值b(t-1)

    XMVECTOR vRawPosition;            // 實際值x(t)
    XMVECTOR vFilteredPosition;        // 平滑值S(t)
    XMVECTOR vPredictedPosition;    // 預測值F(t+T)
    XMVECTOR vTrend;                // 趨勢值b(t)
    XMVECTOR vDiff;                    
    XMVECTOR vLength;
    FLOAT fDiff;    
    BOOL bJointIsValid;

    const Joint joint = joints[JointID];

    vRawPosition = XMVectorSet( joint.Position.X, joint.Position.Y, joint.Position.Z, 0.0f );

    vPrevFilteredPosition = m_pHistory[JointID].m_vFilteredPosition;    // 前一期平滑值S(t-1)
    vPrevTrend = m_pHistory[JointID].m_vTrend;                          // 前一期趨勢值b(t-1)
    vPrevRawPosition = m_pHistory[JointID].m_vRawPosition;              // x(t-1)

    bJointIsValid = JointPositionIsValid( vRawPosition );

    // If joint is invalid, reset the filter
    if( !bJointIsValid )
    {
        m_pHistory[JointID].m_dwFrameCount = 0;
    }

    // Initial start values
    if( m_pHistory[JointID].m_dwFrameCount == 0 )
    {
        vFilteredPosition = vRawPosition;
        vTrend = XMVectorZero();
        m_pHistory[JointID].m_dwFrameCount++;
    }
    else if( m_pHistory[JointID].m_dwFrameCount == 1 )
    {
        vFilteredPosition = XMVectorScale( XMVectorAdd( vRawPosition, vPrevRawPosition ), 0.5f );  //XMVectorScale: Scalar multiplies a vector by a floating-point value
        vDiff = XMVectorSubtract( vFilteredPosition, vPrevFilteredPosition );
        vTrend = XMVectorAdd( XMVectorScale( vDiff, smoothingParams.fCorrection ), XMVectorScale( vPrevTrend, 1.0f - smoothingParams.fCorrection ) );
        m_pHistory[JointID].m_dwFrameCount++;
    }
    else
    {
        
        // A good filtering solution is usually a combination of various filtering techniques, which may include applying 
        // a jitter removal filter to remove spike noise, a smoothing filter, and a forecasting filter to reduce latency,
        // and then adjusting the outputs based on person kinematics and anatomy to avoid awkward cases caused by overshoot.
    
        // First apply jitter filter
        vDiff = XMVectorSubtract( vRawPosition, vPrevFilteredPosition );
        vLength = XMVector3Length( vDiff ); //Returns a vector. The length of vDiff is replicated into each component
        fDiff = fabs( XMVectorGetX( vLength ) );

        if( fDiff <= smoothingParams.fJitterRadius )
        {
            vFilteredPosition = XMVectorAdd( XMVectorScale( vRawPosition, fDiff / smoothingParams.fJitterRadius ),
                                             XMVectorScale( vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius ) );
        }
        else // It should be determined not to be a jitter when the diff value is exceed radius threshold of parameter. In this case, It adopt raw value.
        {
            vFilteredPosition = vRawPosition;
        }


        //////////////////////////////////////////// Now the double exponential smoothing filter:

        // 1. S(t) = α*x(t) + (1-α)*(S(t-1)+b(t-1))     0≤α≤1
        // The first smoothing equation adjusts St. This helps to eliminate the lag and brings St to the appropriate base of the current value.
        vFilteredPosition = XMVectorAdd( XMVectorScale( vFilteredPosition, 1.0f - smoothingParams.fSmoothing ),
                                         XMVectorScale( XMVectorAdd( vPrevFilteredPosition, vPrevTrend ), smoothingParams.fSmoothing ) );

        vDiff = XMVectorSubtract( vFilteredPosition, vPrevFilteredPosition );  // S(t)-S(t-1)

        // 2. b(t)= γ * (S(t)-S(t-1)) + (1-γ) * b(t-1)   0≤γ≤1
        // The second smoothing equation then updates the trend, which is expressed as the difference between the last two values.
        vTrend = XMVectorAdd( XMVectorScale( vDiff, smoothingParams.fCorrection ), XMVectorScale( vPrevTrend, 1.0f - smoothingParams.fCorrection ) ); // 修正趨勢值
    }

    // 3. F(t+T) = S(t) + b(t)*T    
    vPredictedPosition = XMVectorAdd( vFilteredPosition, XMVectorScale( vTrend, smoothingParams.fPrediction ) ); // Predict into the future to reduce latency


    // Check that we are not too far away from raw data
    vDiff = XMVectorSubtract( vPredictedPosition, vRawPosition );
    vLength = XMVector3Length( vDiff );
    fDiff = fabs( XMVectorGetX( vLength ) );

    if( fDiff > smoothingParams.fMaxDeviationRadius )
    {
        vPredictedPosition = XMVectorAdd( XMVectorScale( vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff ),
                                          XMVectorScale( vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff ) );
    }

    // Save the data from this frame
    m_pHistory[JointID].m_vRawPosition = vRawPosition;
    m_pHistory[JointID].m_vFilteredPosition = vFilteredPosition;
    m_pHistory[JointID].m_vTrend = vTrend;

    // Output the data
    m_pFilteredJoints[JointID] = vPredictedPosition;
    m_pFilteredJoints[JointID] = XMVectorSetW( m_pFilteredJoints[JointID], 1.0f );
}

　　myKinect.h

#pragma once
#include <Kinect.h>
#include <opencv2\opencv.hpp>
#include "KinectJointFilter.h"


// Safe release for interfaces
template<class Interface>
inline void SafeRelease(Interface *& pInterfaceToRelease) // 參數為指針的引用
{
    if (pInterfaceToRelease != NULL)
    {
        pInterfaceToRelease->Release();
        pInterfaceToRelease = NULL;
    }
}



class CBodyBasics
{
    // kinect 2.0 的深度圖像分辨率是424 * 512
    static const int        cDepthWidth = 512;
    static const int        cDepthHeight = 424;

public:
    CBodyBasics();        // Constructor
    ~CBodyBasics();        // Destructor
    void                    Update();// 獲得骨架、背景二值圖和深度信息
    HRESULT                 InitializeDefaultSensor();// 用於初始化kinect

private:
    // Current Kinect
    IKinectSensor*          m_pKinectSensor;     // kinect源
    ICoordinateMapper*      m_pCoordinateMapper; // 用於坐標變換

    // Body reader
    IBodyFrameReader*       m_pBodyFrameReader;  // 用於骨架數據讀取

    
    void ProcessBody(IBody* pBody); // 處理指定的骨架，並且在屏幕上繪制出來
    
    void DrawBone(const Joint* pJoints, const DepthSpacePoint* depthSpacePosition, JointType joint0, JointType joint1); // 畫骨架函數
    void DrawHandState(const DepthSpacePoint depthSpacePosition, HandState handState);  // 畫手的狀態函數
    void DrawBody(const Joint* pJoints, const DepthSpacePoint *depthSpacePosition);

    
    FilterDoubleExponential filter;          // Holt Double Exponential Smoothing Filter
    IBody* GetActiveBody(IBody** ppBodies);     // 獲取最近的body
    FLOAT Angle(const DirectX::XMVECTOR* vec, JointType jointA, JointType jointB, JointType jointC);    // Get joint angle ∠ABC in degree

    cv::Mat skeletonImg;  // 顯示圖像的Mat
};

　　myKinect.cpp

#include "myKinect.h"
#include <iostream>
using namespace DirectX;


/// Initializes the default Kinect sensor
HRESULT CBodyBasics::InitializeDefaultSensor()
{

    // 搜索kinect sensor
    HRESULT hr = GetDefaultKinectSensor(&m_pKinectSensor);
    if (FAILED(hr)){
        std::cerr << "Error : GetDefaultKinectSensor" << std::endl;
        return -1;
    }

    // 打開kinect
    hr = m_pKinectSensor->Open();
    if (FAILED(hr)){
        std::cerr << "Error : IKinectSensor::Open()" << std::endl;
        return -1;
    }


    // 從Sensor取得Source
    IBodyFrameSource* pBodyFrameSource = NULL; // 骨架數據源
    hr = m_pKinectSensor->get_BodyFrameSource(&pBodyFrameSource);
    if (FAILED(hr)){
        std::cerr << "Error : IKinectSensor::get_BodyFrameSource()" << std::endl;
        return -1;
    }

    // 從Source打開Reader
    hr = pBodyFrameSource->OpenReader(&m_pBodyFrameReader);
    if (FAILED(hr)){
        std::cerr << "Error : IBodyFrameSource::OpenReader()" << std::endl;
        return -1;
    }

    SafeRelease(pBodyFrameSource);

    // coordinatemapper
    hr = m_pKinectSensor->get_CoordinateMapper(&m_pCoordinateMapper);


    if (!m_pKinectSensor || FAILED(hr))
    {
        std::cout << "Kinect initialization failed!" << std::endl;
        return E_FAIL;
    }

    // skeletonImg,用於畫骨架的MAT
    skeletonImg.create(cDepthHeight, cDepthWidth, CV_8UC3);
    skeletonImg.setTo(0);

    return hr;  // indicates success or failure
}


/// Main processing function
void CBodyBasics::Update()
{
    // 每次先清空skeletonImg
    skeletonImg.setTo(0);

    // 如果丟失了kinect，則不繼續操作
    if (!m_pBodyFrameReader)
    {
        return;
    }

    IBodyFrame* pBodyFrame = NULL; 

    HRESULT hr = m_pBodyFrameReader->AcquireLatestFrame(&pBodyFrame); // 獲取骨架信息

    if (SUCCEEDED(hr))
    {
        IBody* ppBodies[BODY_COUNT] = { 0 }; // 每一個IBody可以追蹤一個人，總共可以追蹤六個人

        // 把kinect追蹤到的人的信息，分別存到每一個IBody中
        hr = pBodyFrame->GetAndRefreshBodyData(_countof(ppBodies), ppBodies);


        IBody *bodyToTrack = NULL;
        bodyToTrack = GetActiveBody(ppBodies); // 獲取最近的body

        // 處理最近的骨架，並且畫出來
        if (bodyToTrack)
        {
            ProcessBody(bodyToTrack);
        }

        for (int i = 0; i < _countof(ppBodies); ++i)
        {
            SafeRelease(ppBodies[i]);
        }

    }

    SafeRelease(pBodyFrame); // 必須要釋放，否則之后無法獲得新的frame數據
}



// Finds the closest body from the sensor if any
IBody* CBodyBasics::GetActiveBody(IBody** ppBodies)
{
    IBody* bodyToTrack = NULL;
    float closestDistance = 10000.0f; // Start with a far enough distance
    BOOLEAN bTracked = false;
    for (int index = 0; index < BODY_COUNT; ++index)
    {
        Joint joint[25];
        ppBodies[index]->GetJoints(25, joint); // 獲得第一個關節數據（JointType_SpineBase = 0）
        float newDistance = joint[JointType_SpineBase].Position.Z;
        if (newDistance != 0 && newDistance <= closestDistance)
        {
            closestDistance = newDistance;
            bodyToTrack = ppBodies[index];
        }
    }

    return bodyToTrack;
}




/// 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))
        {
            /************************************************************************************************
            // Raw Joint
            for (int j = 0; j < _countof(joints); ++j)
            {
            // 將關節點坐標從攝像機坐標系轉到深度坐標系以顯示
            m_pCoordinateMapper->MapCameraPointToDepthSpace(joints[j].Position, &depthSpacePosition[j]);
            }
            *************************************************************************************************/

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



// 畫手的狀態
void CBodyBasics::DrawHandState(const DepthSpacePoint depthSpacePosition, HandState handState)
{
    const int radius = 20;
    const cv::Vec3b blue = cv::Vec3b(128, 0, 0), green = cv::Vec3b(0, 128, 0), red = cv::Vec3b(0, 0, 128);

    // 給不同的手勢分配不同顏色
    CvScalar color;
    switch (handState){
    case HandState_Open:
        color = green;
        break;
    case HandState_Closed:
        color = red;
        break;
    case HandState_Lasso:
        color = blue;
        break;
    default: // 如果沒有確定的手勢，就不要畫
        return;
    }

    circle(skeletonImg, cvPoint(depthSpacePosition.X, depthSpacePosition.Y), radius, color, 4);
}


/// Draws one bone of a body (joint to joint)
void CBodyBasics::DrawBone(const Joint* pJoints, const DepthSpacePoint* depthSpacePosition, JointType joint0, JointType joint1)
{
    TrackingState joint0State = pJoints[joint0].TrackingState;
    TrackingState joint1State = pJoints[joint1].TrackingState;

    // If we can't find either of these joints, exit
    if ((joint0State == TrackingState_NotTracked) || (joint1State == TrackingState_NotTracked))
    {
        return;
    }

    // Don't draw if both points are inferred
    if ((joint0State == TrackingState_Inferred) && (joint1State == TrackingState_Inferred))
    {
        return;
    }

    CvPoint p1 = cvPoint(depthSpacePosition[joint0].X, depthSpacePosition[joint0].Y),
            p2 = cvPoint(depthSpacePosition[joint1].X, depthSpacePosition[joint1].Y);

    // We assume all drawn bones are inferred unless BOTH joints are tracked
    if ((joint0State == TrackingState_Tracked) && (joint1State == TrackingState_Tracked))
    {
        line(skeletonImg, p1, p2, cvScalar(255, 128, 0), 3);    // 線寬為3
    }
    else
    {
        line(skeletonImg, p1, p2, cvScalar(100, 100, 100), 1);     // 線寬為1
    }
}



/// Draws a body 
void CBodyBasics::DrawBody(const Joint* pJoints, const DepthSpacePoint *depthSpacePosition)
{
    //---------------------------Torso-------------------------------
    DrawBone(pJoints, depthSpacePosition, JointType_Head, JointType_Neck);
    DrawBone(pJoints, depthSpacePosition, JointType_Neck, JointType_SpineShoulder);
    DrawBone(pJoints, depthSpacePosition, JointType_SpineShoulder, JointType_SpineMid);
    DrawBone(pJoints, depthSpacePosition, JointType_SpineMid, JointType_SpineBase);
    DrawBone(pJoints, depthSpacePosition, JointType_SpineShoulder, JointType_ShoulderRight);
    DrawBone(pJoints, depthSpacePosition, JointType_SpineShoulder, JointType_ShoulderLeft);
    DrawBone(pJoints, depthSpacePosition, JointType_SpineBase, JointType_HipRight);
    DrawBone(pJoints, depthSpacePosition, JointType_SpineBase, JointType_HipLeft);

    // -----------------------Right Arm ------------------------------------ 
    DrawBone(pJoints, depthSpacePosition, JointType_ShoulderRight, JointType_ElbowRight);
    DrawBone(pJoints, depthSpacePosition, JointType_ElbowRight, JointType_WristRight);
    DrawBone(pJoints, depthSpacePosition, JointType_WristRight, JointType_HandRight);
    //DrawBone(pJoints, depthSpacePosition, JointType_HandRight, JointType_HandTipRight);
    //DrawBone(pJoints, depthSpacePosition, JointType_WristRight, JointType_ThumbRight);

    //----------------------------------- Left Arm--------------------------
    DrawBone(pJoints, depthSpacePosition, JointType_ShoulderLeft, JointType_ElbowLeft);
    DrawBone(pJoints, depthSpacePosition, JointType_ElbowLeft, JointType_WristLeft);
    DrawBone(pJoints, depthSpacePosition, JointType_WristLeft, JointType_HandLeft);
    //DrawBone(pJoints, depthSpacePosition, JointType_HandLeft, JointType_HandTipLeft);
    //DrawBone(pJoints, depthSpacePosition, JointType_WristLeft, JointType_ThumbLeft);

    // ----------------------------------Right Leg--------------------------------
    DrawBone(pJoints, depthSpacePosition, JointType_HipRight, JointType_KneeRight);
    DrawBone(pJoints, depthSpacePosition, JointType_KneeRight, JointType_AnkleRight);
    DrawBone(pJoints, depthSpacePosition, JointType_AnkleRight, JointType_FootRight);

    // -----------------------------------Left Leg---------------------------------
    DrawBone(pJoints, depthSpacePosition, JointType_HipLeft, JointType_KneeLeft);
    DrawBone(pJoints, depthSpacePosition, JointType_KneeLeft, JointType_AnkleLeft);
    DrawBone(pJoints, depthSpacePosition, JointType_AnkleLeft, JointType_FootLeft);


    // Draw the joints except the last four（HandTipLeft,ThumbLeft,HandTipRight,ThumbRight）
    for (int i = 0; i < JointType_Count-4; ++i)
    {
        if (pJoints[i].TrackingState == TrackingState_Inferred)
        {
            circle(skeletonImg, cvPoint(depthSpacePosition[i].X, depthSpacePosition[i].Y), 3, cvScalar(64, 255, 0), -1);  
        }
        else if (pJoints[i].TrackingState == TrackingState_Tracked)
        {
            circle(skeletonImg, cvPoint(depthSpacePosition[i].X, depthSpacePosition[i].Y), 3, cvScalar(255, 255, 0), -1);  
        }
    }
}




/// Constructor
CBodyBasics::CBodyBasics() :
m_pKinectSensor(NULL),
m_pCoordinateMapper(NULL),
m_pBodyFrameReader(NULL)
{
    // Some smoothing with little latency (defaults).
    // Only filters out small jitters.
    // Good for gesture recognition in games.
    //defaultParams = { 0.5f, 0.5f, 0.5f, 0.05f, 0.04f };

    // Smoothed with some latency.
    // Filters out medium jitters.
    // Good for a menu system that needs to be smooth but doesn't need the reduced latency as much as gesture recognition does.
    //somewhatLatentParams = { 0.5f, 0.1f, 0.5f, 0.1f, 0.1f };

    // Very smooth, but with a lot of latency.
    // Filters out large jitters.
    // Good for situations where smooth data is absolutely required and latency is not an issue.
    //verySmoothParams = { 0.7f, 0.3f, 1.0f, 1.0f, 1.0f };

    // Setting Smoothing Parameter
    float smoothing = 0.5f;          // [0..1], lower values closer to raw data, passing 0 causes the raw data to be returned
    float correction = 0.1f;         // [0..1], a lower value corrects more slowly and appears smoother
    float prediction = 0.5f;         // [0..n], the number of frames to predict into the future(A value greater than 0.5 will likely lead to overshoot when the data changes quickly)
    float jitterRadius = 0.1f;       // The radius in meters for jitter reduction
    float maxDeviationRadius = 0.1f; // The maximum radius in meters that filtered positions are allowed to deviate from raw data

    filter.Init(smoothing, correction, prediction, jitterRadius, maxDeviationRadius);
}


/// Destructor
CBodyBasics::~CBodyBasics()
{
    SafeRelease(m_pBodyFrameReader);
    SafeRelease(m_pCoordinateMapper);

    if (m_pKinectSensor)
    {
        m_pKinectSensor->Close();
    }
    SafeRelease(m_pKinectSensor);
}

　　main.cpp

#include "myKinect.h"
#include <iostream>
using namespace std;


int main()
{


    CBodyBasics myKinect;
    HRESULT hr = myKinect.InitializeDefaultSensor();
    if (SUCCEEDED(hr)){
        while (1){
            myKinect.Update();
        }
    }
    else{
        cout << "kinect initialization failed!" << endl;
        system("pause");
    }

    return 0;
}

 

 

參考：

Kinect 2.0 + OpenCV 顯示深度數據、骨架信息、手勢狀態和人物二值圖

Kinect for Windows SDK v2 Sample Program

Kinect v2程序設計(C++) Body篇

Kinect v2程序設計(C++) Color篇

Kinect v2程序設計(C++) Depth篇

Skeletal Joint Smoothing White Paper

Kinect for Windows 2.0入門介紹

Kinect開發學習筆記之（一）Kinect介紹和應用

Kinect V2.0 的調試步驟(多圖)

Kinect v1和Kinect v2的徹底比較

如何平滑處理Kinect采集的骨骼數據 | KinectAPI編程

Kinect1.0的安裝和使用