（轉）Kinect體感機器人—— 空間向量法計算關節角度

Kinect體感機器人（三）—— 空間向量法計算關節角度

By 馬冬亮(凝霜  Loki)

一個人的戰爭(http://blog.csdn.net/MDL13412)

        終於寫到體感機器人的核心代碼了，如何過濾、計算骨骼點是機器人控制的關鍵。經過摸索、評估、測試，最終得出了一個使用空間座標進行計算的算法，下面我將進行詳細講解。

爲什麼是空間向量

        說到角度計算，那麼我們首先想到的就是解析幾何，因爲這是我們人類思考的方式。但是解析幾何帶來了一個問題——邊界條件，使用其進行計算時，需要考慮各種特殊情況：平行、重疊、垂直、相交。。。這直接導致了代碼量的爆炸性增長，而我們又知道，代碼的BUG是與其長度呈指數級增長的，這給我們帶來了沉重的心智負擔，編碼和調試都變得異常困難。

        說到這，有人要說了，解析幾何的邊界條件無非就那麼幾種，我分模塊進行編碼就可以減少複雜度了，並不會損失太多。那麼，請設想如下情況，如何計算手臂平面與地面的夾角？如下圖：

        空間解析幾何帶來了更多的邊界條件，而Kinect在採集的過程中是不能下斷點進行聯機調試的，證明算法的正確性變得異常困難，這麼多的邊界條件，很難一一驗證。

        下面我們來看一組公式：

        從上面這組公式可以看出，通過向量，我們可以完全擺脫邊界條件的繁瑣（對於骨骼點的重疊，可以通過濾波解決，見後文），只需編寫通用的公式即可完成所有情況的計算，簡單與優雅並存。

座標映射

        向量法使用的是常規的數學座標系，而Kinect的座標系與常規數學座標系稍有不同，如下圖所示：

        由此可知，要使用向量，首先就要將Kinect的座標系映射到數學座標系上，其方法非常簡單：由向量的可平移性質及方向性，可以推導出Kinect座標系中任意兩個不重合的座標點A(x1, y1, z1)，B(x2, y2, z2)經過變換，可轉化到數學座標系中，對其組成的向量AB，可以認爲是從座標軸零點引出，轉化公式如下：

        根據上述性質，可以將人體關節角度計算簡化爲對空間向量夾角的計算。

空間向量法計算關節角度

        由於所選用機器人的關節處舵機存在諸多限制，對於大臂保持靜止，小臂與大臂垂直的旋轉動作，需要藉助於肩膀上的舵機進行聯合調節。這就要求不能簡單的只計算兩空間向量的夾角，爲此特提出了一種漸進算法，即求空間平面xOz與肩膀、肘關節、手所組成平面的夾角，並以其夾角完成對肩膀舵機的調速工作。

        下面是實際人體左臂動作的計算過程，實拍人體動作照片見圖A，左臂提取出的關節點在Kinect空間座標系中的向量表示見圖B，經過變換後轉化爲普通座標系中的向量見圖C。

圖A 人體動作

圖B 左臂關節點在Kinect困難關鍵座標系中的向量表示

        上圖中：各個關節點（肩膀，肘，手）是處在空間平面中，對應z軸從裏到外分別爲：肩膀，肘，手，且三點在向量圖中均處於z軸負半軸。

圖C 經過變換後轉化爲數學座標系中的向量

        對於肘關節角度的計算，可以直接使用空間向量ES和EH的夾角得出，計算過程如下:

        對於大臂的上下襬動角度，可以將向量ES投影到xOy平面上，並求其與y座標軸的夾角得出，計算過程及公式類似於肘關節角度的計算過程。
        對於協助小臂轉動的肩膀舵機的角度計算，其向量轉化關係下所示：

        爲了求取空間平面夾角，需要首先求取兩平面的法向量，再根據法向量計算出兩平面夾角。計算過程如下：

        式（3-5）和式（3-6）分別計算出向量ES和向量EH，分別對應肘關節指向肩膀和肘關節指向手腕的兩條向量；式（3-7）通過叉乘計算出肩膀、肘、手所構成空間平面的法向量n1；式（3-8）代表空間平面xOz的法向量；式（3-9）求取法向量n1與法向量n2的夾角，從而完成對協助小臂轉動的肩膀舵機的角度計算。
        對於腿部的識別，由於人體小腿無法旋轉，故只需採用兩向量夾角及投影到平面的方式進行求取，與手臂部分相似，不再詳述。

腿部姿態檢測

        首先，由於機器人模仿人體腿部動作時會遇到平衡問題，爲了解決此問題，需要給機器人加裝陀螺儀和及加速度傳感器，實時調整機器人重心，保持機器人站立的穩定性。但是在機器人調整穩定性同時，會導致機器人上肢的晃動，在機器人實際工作時，會造成手臂動作發生異常，可能導致意外發生。其次，機器人腿部動作大多侷限於行走、轉向、下蹲、站立等幾個固定動作，讓機器人完全模仿人體腿部動作，會給用戶帶來非常多的冗餘操作，使用戶不能專注於業務細節而需要專注於控制機器人腿部動作；最後，由於本文使用的人形機器人關節並不與人體關節一一對應，勢必會造成控制上的誤差，這可能帶來災難性的後果。
        綜上所述，通過識別人體腿部特定動作，支持機器人前進、後退、左轉、右轉、下蹲、站立，即可滿足絕大多數情況下對機器人腿部動作的要求，並且有效的減少了用戶的操作複雜度，讓用戶可以專注於業務細節。
        爲了支持機器人前進、後退、左轉、右轉、下蹲、站立這幾個固定動作，需要對人體腿部姿態進行檢測，從而控制機器人完成相應動作。檢測算法首先檢測左腿髖關節是否達到確認度閥值，若達到，則先檢測是否爲下蹲姿勢，若不爲下蹲，則檢測左側髖關節指向膝關節的向量相對於前、左、後三個方向哪個方向的權值更大，並取其權值最大的作爲機器人控制信號，其分別對應與機器人的前進、左轉、後退動作；若未達到閥值，則檢測右髖關節是否達到確認度閥值，若達到，則檢測右側髖關節指向膝關節的向量相對於前、右、後三個方向哪個方向的權值更大，並取其權值最大的作爲機器人控制信號，其分別對應與機器人的前進、右轉、後退動作；若未達到閥值，則判定爲機器人站立動作。
        腿部姿態詳細檢測流程如下圖所示：

濾波算法

        由於Kinect傳感器採集到的數據會有擾動，從而造成機器人控制的不穩定性，因此必須對識別出來的骨骼點進行濾波處理，以保證機器人動作穩定、連貫。
        對於濾波算法的選擇，要綜合考慮運算速度、內存佔用、準確性、抗隨機干擾能力等技術指標。這就要求對採樣數據進行分析，從而選取濾波效果最好的算法。
        本識別程序運行於EPCM-505C開發平臺，在只進行關節識別的情況下，每秒能識別6-8幀圖像，加上空間座標向量運算及腿部姿勢識別後，每秒能處理4-5幀圖像。由於期望儘可能快的向機器人發送控制數據，以提高機器人的響應速度。因此，所選擇的濾波算法應儘可能快速。
        經過對OpenNI識別出的關節點空間座標分析可知，其擾動一般是在人體實際關節座標的四周做小幅度波動，另外存在一些識別死區，此時無法檢測到關節點。因此，所選用的濾波算法要保證機器人的正確運行，對無法識別的關節點做相應處理，對小幅度波動的關節點保持上一狀態不變。
        綜上所述，本文提出了一種改進型的限幅濾波算法，此濾波算法採用了動態規劃的思想，保證每次濾波後的結果都是最優解，從而從整體上得出最優解。濾波算法的詳細流程下圖所示：

        經過與常用濾波算法對比實驗證明，此算法濾波效果良好，能滿足對機器人控制的需求。詳細對比結果如下表所示：

算法名稱 技術指標	改進型限幅濾波算法	限幅濾波 算法	算術平均值 濾波算法	滑動平均值 濾波算法
速度	快	快	慢	較快
內存佔用	少	少	多	多
能識別細微幅度動作	否	否	能	能
對小幅度擾動過濾效果	優	優	優	優
濾波結果是否正確	是	是	不一定	不一定

arccos哈系表

        由於有16個關節角度需要計算，在PC上每秒可以運行30幀，即16 * 30 = 480次三角函數運算，這很明顯是需要用打表進行優化的，下面是哈系表的代碼，如果不明白，請繪製cos函數曲線，再進行對比閱讀：

/**
 * @brief   性能分析代碼.
 * @ingroup ProgramHelper
 *
 * @details 用於分析查詢哈希表和直接使用C庫的三角函數計算角度值的性能.
 *
 * @code
 *
#include <cstdlib>
#include <iostream>
#include <fstream>
#include <cmath>
#include <iomanip>
#include <ctime>

using namespace std;

int main(int argc, char** argv)
{
    clock_t start,finish;
    volatile double dummy;

    start=clock();

//    for (int i = 0; i < 1000; ++i)
//        for (int j = 0; j < 100000; ++j)
//            dummy = cos((i % 3) * M_PI / 180);

    for (int i = 0; i < 1000; ++i)
        for (int j = 0; j < 100000; ++j)
            dummy = (int)(((i % 3) * 1000)) % 100000;

//    for (int i = 0; i < 1000; ++i)
//        for (int j = 0; j < 100000; ++j)
//            dummy = i;

    finish = clock();

    cout << (double)(finish-start)/CLOCKS_PER_SEC << endl;

    return 0;
}
 *
 * @endcode
 */

/**
 * @brief   性能分析代碼.
 * @ingroup ProgramHelper
 * 
 * @details 用於分析查詢哈希表和直接使用C庫的三角函數計算角度值的性能.
 * 
 * @code
 * 

include <cstdlib>

include <iostream>

include <fstream>

include <cmath>

include <iomanip>

include <ctime>

using namespace std;

int main(int argc, char** argv)
{
clock_t start,finish;
volatile double dummy;

start=clock();

// for (int i = 0; i < 1000; ++i)
// for (int j = 0; j < 100000; ++j)
// dummy = cos((i % 3) * M_PI / 180);

for (int i = 0; i < 1000; ++i)
    for (int j = 0; j < 100000; ++j)
        dummy = (int)(((i % 3) * 1000)) % 100000;

// for (int i = 0; i < 1000; ++i)
// for (int j = 0; j < 100000; ++j)
// dummy = i;

finish = clock();

cout << (double)(finish-start)/CLOCKS_PER_SEC << endl;

return 0;

}
*
* @endcode
*/

/
  @brief   生成cos哈希表的索引範圍.
  @ingroup ProgramHelper

  @details 將1-90度的cos值經過Hash函數變換, 得出一個哈希範圍.
  @see     CosHashTable

  @code

#include <cstdlib>
#include <iostream>
#include <fstream>
#include <cmath>
#include <iomanip>

using namespace std;

int main(int argc, char argv)
{
    ofstream fout(“a.txt”);

    for (int i = 90; i > 0; –i)
    {
        fout << setw(6) << (((int)(cos((double)i  M_PI / 180)  100000)) % 100001);

        if (0 == i % 10)
            fout << ”,” << endl;
        else
            fout << ”,”;
    }

    fout << “100000”;

    fout.flush();
    fout.close();

    return 0;
}

  @endcode
 /
static int s_initArccosHash[] =
{
  1745,  3489,  5233,  6975,  8715, 10452, 12186, 13917, 15643, 17364,
 19080, 20791, 22495, 24192, 25881, 27563, 29237, 30901, 32556, 34202,
 35836, 37460, 39073, 40673, 42261, 43837, 45399, 46947, 48480, 50000,
 51503, 52991, 54463, 55919, 57357, 58778, 60181, 61566, 62932, 64278,
 65605, 66913, 68199, 69465, 70710, 71933, 73135, 74314, 75470, 76604,
 77714, 78801, 79863, 80901, 81915, 82903, 83867, 84804, 85716, 86602,
 87461, 88294, 89100, 89879, 90630, 91354, 92050, 92718, 93358, 93969,
 94551, 95105, 95630, 96126, 96592, 97029, 97437, 97814, 98162, 98480,
 98768, 99026, 99254, 99452, 99619, 99756, 99862, 99939, 99984,100000
};

/* 

 * @brief   生成cos哈希表的索引範圍. 

 * @ingroup ProgramHelper 

 *  

 * @details 將1-90度的cos值經過Hash函數變換, 得出一個哈希範圍. 

 * @see     CosHashTable 

 *  

 * @code 

 * 

include <cstdlib>

include <iostream>

include <fstream>

include <cmath>

include <iomanip>

using namespace std;

int main(int argc, char** argv)
{
ofstream fout(“a.txt”);

for (int i = 90; i > 0; --i)
{
    fout << setw(6) << (((int)(cos((double)i * M_PI / 180) * 100000)) % 100001);

    if (0 == i % 10)
        fout << "," << endl;
    else
        fout << ",";
}

fout << "100000";

fout.flush();
fout.close();

return 0;

}
*
* @endcode
*/
static int s_initArccosHash[] =
{
1745, 3489, 5233, 6975, 8715, 10452, 12186, 13917, 15643, 17364,
19080, 20791, 22495, 24192, 25881, 27563, 29237, 30901, 32556, 34202,
35836, 37460, 39073, 40673, 42261, 43837, 45399, 46947, 48480, 50000,
51503, 52991, 54463, 55919, 57357, 58778, 60181, 61566, 62932, 64278,
65605, 66913, 68199, 69465, 70710, 71933, 73135, 74314, 75470, 76604,
77714, 78801, 79863, 80901, 81915, 82903, 83867, 84804, 85716, 86602,
87461, 88294, 89100, 89879, 90630, 91354, 92050, 92718, 93358, 93969,
94551, 95105, 95630, 96126, 96592, 97029, 97437, 97814, 98162, 98480,
98768, 99026, 99254, 99452, 99619, 99756, 99862, 99939, 99984,100000
};

/
  @brief   acrcos哈希表
  @ingroup ProgramHelper

  @details 哈希函數:
           角度 = s_arccosHash[((int)(cos(degree)  100000) % 100001]
 */
static int s_arccosHash[100001];

/ 

 * @brief   acrcos哈希表 

 * @ingroup ProgramHelper 

 *  

 * @details 哈希函數: 

 *          角度 = s_arccosHash[((int)(cos(degree) * 100000) % 100001] 

 */ 

static int s_arccosHash[100001];

關節角度計算核心代碼

        這裏只給出左臂的關鍵代碼，右臂及腿部類似，不再類述：

////////////////////////////////////////////////////////////////////////////////
// 計算機器人關節角度
////////////////////////////////////////////////////////////////////////////////

/**
 * @brief   計算機器人的左臂上3個舵機的角度.
 * @ingroup SceneDrawer
 *
 * @param   shoulder 肩膀的座標
 * @param   elbow 肘關節的座標
 * @param   hand 手(腕)的座標
 *
 * @details 所有座標均採用向量法進行計算.
 */
inline void CalculateLeftArm(const XnSkeletonJointPosition &shoulder,
                             const XnSkeletonJointPosition &elbow,
                             const XnSkeletonJointPosition &hand)
{
    MyVector3D vector1;
    MyVector3D vector2;
    MyVector3D normal1;
    MyVector3D normal2;
    double deltaNormal1;
    double deltaNormal2;
    double deltaVector1;
    double deltaVector2;
    double cosAngle;

    if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
    {
        // vector1      -> shoulder - elbow
        // vector2      -> hand - elbow
        vector1.X = shoulder.position.X - elbow.position.X;
        vector1.Y = shoulder.position.Y - elbow.position.Y;
        vector1.Z = elbow.position.Z - shoulder.position.Z;

        vector2.X = hand.position.X - elbow.position.X;
        vector2.Y = hand.position.Y - elbow.position.Y;
        vector2.Z = elbow.position.Z - hand.position.Z;

        // normal1 = vector1 x vector2

        normal1.X = vector1.Y * vector2.Z - vector1.Z * vector2.Y;
        normal1.Y = vector1.Z * vector2.X - vector1.X * vector2.Z;
        normal1.Z = vector1.X * vector2.Y - vector1.Y * vector2.X;

        normal2.X = 0.0;
        normal2.Y = -150.0;
        normal2.Z = 0.0;

        deltaNormal1 = sqrt(normal1.X * normal1.X + normal1.Y * normal1.Y + normal1.Z * normal1.Z);
        deltaNormal2 = sqrt(normal2.X * normal2.X + normal2.Y * normal2.Y + normal2.Z * normal2.Z);

        if (deltaNormal1 * deltaNormal2 > 0.0)
        {
            cosAngle = (normal1.X * normal2.X + normal1.Y * normal2.Y + normal1.Z * normal2.Z) / (deltaNormal1 * deltaNormal2);

            if (cosAngle < 0.0)
                g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
            else
            {
                if (shoulder.position.Y < hand.position.Y)
                    g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 + s_arccosHash[(int)(cosAngle * 100000) % 100001];
                else
                    g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 - s_arccosHash[(int)(cosAngle * 100000) % 100001];
            }
        }
        else
            g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;

        vector1.X = elbow.position.X - shoulder.position.X;
        vector1.Y = elbow.position.Y - shoulder.position.Y;
        vector1.Z = 0.0;

        vector2.X = 0.0;
        vector2.Y = 100;
        vector2.Z = 0.0;

        deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
        deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);

        if (deltaVector1 * deltaVector2 > 0.0)
        {
            cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);

            if (cosAngle < 0.0)
                g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
            else
                g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
        }
        else
            g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;

        vector1.X = shoulder.position.X - elbow.position.X;
        vector1.Y = shoulder.position.Y - elbow.position.Y;
        vector1.Z = elbow.position.Z - shoulder.position.Z;

        vector2.X = hand.position.X - elbow.position.X;
        vector2.Y = hand.position.Y - elbow.position.Y;
        vector2.Z = elbow.position.Z - hand.position.Z;

        deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
        deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);

        if (deltaVector1 * deltaVector2 > 0.0)
        {
            cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);

            if (cosAngle < 0.0)
                g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
            else
                g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
        }
        else
            g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
    }   // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
    else
    {
        g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
        g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;
        g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
    }   // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)

#ifdef DEBUG_MSG_LEFT_ARM
    char bufferLeftArm[200];
    snprintf(bufferLeftArm, sizeof(bufferLeftArm),
             ”LEFT_SHOULDER_VERTICAL = %4d  LEFT_SHOULDER_HORIZEN = %4d  LEFT_ELBOW = %4d”,
             g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL],
             g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN],
             g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW]);
    std::cout << bufferLeftArm << std::endl;
    NsLog()->info(bufferLeftArm);
#endif
}

////////////////////////////////////////////////////////////////////////////////
// 計算機器人關節角度
////////////////////////////////////////////////////////////////////////////////

/**
 * @brief   計算機器人的左臂上3個舵機的角度.
 * @ingroup SceneDrawer
 * 
 * @param   shoulder 肩膀的座標
 * @param   elbow 肘關節的座標
 * @param   hand 手(腕)的座標
 * 
 * @details 所有座標均採用向量法進行計算.
 */
inline void CalculateLeftArm(const XnSkeletonJointPosition &shoulder,
                             const XnSkeletonJointPosition &elbow,
                             const XnSkeletonJointPosition &hand)
{
    MyVector3D vector1;
    MyVector3D vector2;
    MyVector3D normal1;
    MyVector3D normal2;
    double deltaNormal1;
    double deltaNormal2;
    double deltaVector1;
    double deltaVector2;
    double cosAngle;

    if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
    {
        // vector1      -> shoulder - elbow
        // vector2      -> hand - elbow
        vector1.X = shoulder.position.X - elbow.position.X;
        vector1.Y = shoulder.position.Y - elbow.position.Y;
        vector1.Z = elbow.position.Z - shoulder.position.Z;

        vector2.X = hand.position.X - elbow.position.X;
        vector2.Y = hand.position.Y - elbow.position.Y;
        vector2.Z = elbow.position.Z - hand.position.Z;   

        // normal1 = vector1 x vector2

        normal1.X = vector1.Y * vector2.Z - vector1.Z * vector2.Y;
        normal1.Y = vector1.Z * vector2.X - vector1.X * vector2.Z;
        normal1.Z = vector1.X * vector2.Y - vector1.Y * vector2.X;

        normal2.X = 0.0;
        normal2.Y = -150.0;
        normal2.Z = 0.0;        

        deltaNormal1 = sqrt(normal1.X * normal1.X + normal1.Y * normal1.Y + normal1.Z * normal1.Z);
        deltaNormal2 = sqrt(normal2.X * normal2.X + normal2.Y * normal2.Y + normal2.Z * normal2.Z);

        if (deltaNormal1 * deltaNormal2 > 0.0)
        {
            cosAngle = (normal1.X * normal2.X + normal1.Y * normal2.Y + normal1.Z * normal2.Z) / (deltaNormal1 * deltaNormal2);

            if (cosAngle < 0.0)
                g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
            else
            {
                if (shoulder.position.Y < hand.position.Y)
                    g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 + s_arccosHash[(int)(cosAngle * 100000) % 100001];
                else
                    g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = 90 - s_arccosHash[(int)(cosAngle * 100000) % 100001];        
            } 
        }
        else
            g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;

        vector1.X = elbow.position.X - shoulder.position.X;
        vector1.Y = elbow.position.Y - shoulder.position.Y;
        vector1.Z = 0.0;

        vector2.X = 0.0;
        vector2.Y = 100;
        vector2.Z = 0.0;

        deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
        deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);

        if (deltaVector1 * deltaVector2 > 0.0)
        {
            cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);

            if (cosAngle < 0.0)
                g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
            else
                g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
        }
        else
            g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;

        vector1.X = shoulder.position.X - elbow.position.X;
        vector1.Y = shoulder.position.Y - elbow.position.Y;
        vector1.Z = elbow.position.Z - shoulder.position.Z;

        vector2.X = hand.position.X - elbow.position.X;
        vector2.Y = hand.position.Y - elbow.position.Y;
        vector2.Z = elbow.position.Z - hand.position.Z;   

        deltaVector1 = sqrt(vector1.X * vector1.X + vector1.Y * vector1.Y + vector1.Z * vector1.Z);
        deltaVector2 = sqrt(vector2.X * vector2.X + vector2.Y * vector2.Y + vector2.Z * vector2.Z);

        if (deltaVector1 * deltaVector2 > 0.0)
        {
            cosAngle = (vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z) / (deltaVector1 * deltaVector2);

            if (cosAngle < 0.0)
                g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = 90 + (90 - s_arccosHash[(int)((-cosAngle) * 100000) % 100001]);
            else
                g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = s_arccosHash[(int)(cosAngle * 100000) % 100001];
        }
        else
            g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
    }   // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)
    else
    {
        g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL] = INVALID_JOINT_VALUE;
        g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN] = INVALID_JOINT_VALUE;
        g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW] = INVALID_JOINT_VALUE;
    }   // if (shoulder.fConfidence > JOINT_CONFIDENCE && elbow.fConfidence > JOINT_CONFIDENCE && hand.fConfidence > JOINT_CONFIDENCE)

ifdef DEBUG_MSG_LEFT_ARM

char bufferLeftArm[200];
snprintf(bufferLeftArm, sizeof(bufferLeftArm), 
         "LEFT_SHOULDER_VERTICAL = %4d  LEFT_SHOULDER_HORIZEN = %4d  LEFT_ELBOW = %4d", 
         g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_VERTICAL], 
         g_controlRobot.RobotJointAngle[ROBOT_LEFT_SHOULDER_HORIZEN], 
         g_controlRobot.RobotJointAngle[ROBOT_LEFT_ELBOW]);
std::cout << bufferLeftArm << std::endl;
NsLog()->info(bufferLeftArm);

endif

}

腿部姿態檢測核心代碼

/**
 * @brief   判別機器人行走及下蹲.
 * @ingroup SceneDrawer
 *
 * @details 前後左右行走,下蹲.
 */
inline void PoseDetect()
{
    // 首先判斷左腿
    if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN])
    {
        // 判斷是否爲蹲
        if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD &&
            g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD)
        {
            g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_SQUAT;
            return;
        }

        // 需要判斷向左踢腿的情況
        if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL])
        {
            // 判斷是否達到向左踢腿的閥值
            if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] <= ROBOT_POSE_WALK_LEFT_THRESHOLD)
            {
                // 判斷哪個方向的分量的權值更大
                if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
                {
                    if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=
                        abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                        return;
                    }
                    else
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
                        return;
                    }
                }
                else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
                {
                    if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
                        abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                        return;
                    }
                    else
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
                        return;
                    }
                }
                else
                {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
                    return;
                }
            }
            else
            {
                if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
                {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                        return;
                }
                else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
                {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                        return;
                }
            }
        }
        else    // 直接判斷是否是前進姿勢
        {
            if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
            {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                    return;
            }
            else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
            {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                    return;
            }
        }

        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
    }

////////////////////////////////////////////////////////////////////////////////

    if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN])
    {
        if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL])
        {
            if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] <= ROBOT_POSE_WALK_RIGHT_THRESHOLD)
            {
                if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
                {
                    if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >=
                        abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                        return;
                    }
                    else
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
                        return;
                    }
                }
                else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
                {
                    if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
                        abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                        return;
                    }
                    else
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
                        return;
                    }
                }
                else
                {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
                    return;
                }
            }
            else
            {
                if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
                {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                        return;
                }
                else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
                {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                        return;
                }
            }
        }
        else    // 直接判斷是否是前進姿勢
        {
            if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
            {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                    return;
            }
            else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
            {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                    return;
            }
        }

        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
    }

    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
}

/**
 * @brief   判別機器人行走及下蹲.
 * @ingroup SceneDrawer
 * 
 * @details 前後左右行走,下蹲.
 */
inline void PoseDetect()
{
    // 首先判斷左腿
    if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN])
    {
        // 判斷是否爲蹲
        if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD &&
            g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_SQUAT_THRESHOLD)
        {
            g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_SQUAT;
            return;
        }

        // 需要判斷向左踢腿的情況
        if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL])
        {
            // 判斷是否達到向左踢腿的閥值
            if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] <= ROBOT_POSE_WALK_LEFT_THRESHOLD)
            {
                // 判斷哪個方向的分量的權值更大
                if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
                {
                    if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >= 
                        abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                        return;
                    }
                    else
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
                        return;
                    }
                }
                else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
                {
                    if (abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
                        abs(g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_VERTICAL] - ROBOT_POSE_WALK_LEFT_THRESHOLD))
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                        return;
                    }
                    else
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
                        return;
                    }
                }
                else
                {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_LEFT;
                    return;
                }
            }
            else
            {
                if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)        
                {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                        return;
                }
                else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
                {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                        return;
                }
            }
        }
        else    // 直接判斷是否是前進姿勢
        {
            if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)        
            {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                    return;
            }
            else if (g_controlRobot.RobotJointAngle[ROBOT_LEFT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
            {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                    return;
            }
        }

        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
    } 

////////////////////////////////////////////////////////////////////////////////

    if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN])
    {
        if (INVALID_JOINT_VALUE != g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL])
        {
            if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] <= ROBOT_POSE_WALK_RIGHT_THRESHOLD)
            {
                if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)
                {
                    if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_FRONT_THRESHOLD) >= 
                        abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                        return;
                    }
                    else
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
                        return;
                    }
                }
                else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
                {
                    if (abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] - ROBOT_POSE_WALK_BACK_THRESHOLD) >=
                        abs(g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_VERTICAL] - ROBOT_POSE_WALK_RIGHT_THRESHOLD))
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                        return;
                    }
                    else
                    {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
                        return;
                    }
                }
                else
                {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_RIGHT;
                    return;
                }
            }
            else
            {
                if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)        
                {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                        return;
                }
                else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
                {
                        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                        return;
                }
            }
        }
        else    // 直接判斷是否是前進姿勢
        {
            if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] <= ROBOT_POSE_WALK_FRONT_THRESHOLD)        
            {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_FRONT;
                    return;
            }
            else if (g_controlRobot.RobotJointAngle[ROBOT_RIGHT_HIP_HORIZEN] >= ROBOT_POSE_WALK_BACK_THRESHOLD)
            {
                    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_WALK_BACK;
                    return;
            }
        }

        g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
    } 

    g_controlRobot.RobotJointAngle[ROBOT_HEAD] = ROBOT_ACTION_STOP;
}

繪製人體骨骼核心代碼

/**
 * @brief   繪製骨骼圖, 並返回相應的座標點.
 * @ingroup SceneDrawer
 *
 * @param   player 用戶ID
 * @param   eJoint1 第一個關節點ID
 * @param   eJoint2 第二個關節點ID
 * @param   joint1 [out] 關節點1
 * @param   joint2 [out] 關節點2
 */
inline void DrawLimbAndGetJoint(XnUserID player,
                                XnSkeletonJoint eJoint1,
                                XnSkeletonJoint eJoint2,
                                XnSkeletonJointPosition &joint1,
                                XnSkeletonJointPosition &joint2)
{
    if (!TrackerViewer::getInstance().UserGenerator
            .GetSkeletonCap().IsTracking(player))
    {
        printf(”not tracked!\n”);
        return;
    }

    TrackerViewer::getInstance().UserGenerator
        .GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
    TrackerViewer::getInstance().UserGenerator
        .GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);

    if (joint1.fConfidence < JOINT_CONFIDENCE || joint2.fConfidence < JOINT_CONFIDENCE)
    {
        return;
    }

    XnPoint3D pt[2];
    pt[0] = joint1.position;
    pt[1] = joint2.position;

    TrackerViewer::getInstance().DepthGenerator.
        ConvertRealWorldToProjective(2, pt, pt);

    glVertex3i(pt[0].X, pt[0].Y, 0);
    glVertex3i(pt[1].X, pt[1].Y, 0);
}

/**
 * @brief   繪製骨骼圖, 並返回相應的座標點.
 * @ingroup SceneDrawer
 * 
 * @param   player 用戶ID
 * @param   eJoint1 第一個關節點ID
 * @param   eJoint2 第二個關節點ID
 * @param   joint1 [out] 關節點1
 * @param   joint2 [out] 關節點2
 */
inline void DrawLimbAndGetJoint(XnUserID player, 
                                XnSkeletonJoint eJoint1,
                                XnSkeletonJoint eJoint2, 
                                XnSkeletonJointPosition &joint1,
                                XnSkeletonJointPosition &joint2)
{
    if (!TrackerViewer::getInstance().UserGenerator
            .GetSkeletonCap().IsTracking(player))
    {
        printf("not tracked!\n");
        return;
    }

    TrackerViewer::getInstance().UserGenerator
        .GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
    TrackerViewer::getInstance().UserGenerator
        .GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);

    if (joint1.fConfidence < JOINT_CONFIDENCE || joint2.fConfidence < JOINT_CONFIDENCE)
    {
        return;
    }

    XnPoint3D pt[2];
    pt[0] = joint1.position;
    pt[1] = joint2.position;

    TrackerViewer::getInstance().DepthGenerator.
        ConvertRealWorldToProjective(2, pt, pt);

    glVertex3i(pt[0].X, pt[0].Y, 0);
    glVertex3i(pt[1].X, pt[1].Y, 0);
}

DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND, joint2, joint3);

s_pointFilter[XN_SKEL_LEFT_SHOULDER] = joint1;
s_pointFilter[XN_SKEL_LEFT_ELBOW] = joint2;
s_pointFilter[XN_SKEL_LEFT_HAND] = joint3;

DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND, joint2, joint3);

s_pointFilter[XN_SKEL_RIGHT_SHOULDER] = joint1;
s_pointFilter[XN_SKEL_RIGHT_ELBOW] = joint2;
s_pointFilter[XN_SKEL_RIGHT_HAND] = joint3;

DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT, joint2, joint3);

s_pointFilter[XN_SKEL_LEFT_HIP] = joint1;
s_pointFilter[XN_SKEL_LEFT_KNEE] = joint2;
s_pointFilter[XN_SKEL_LEFT_FOOT] = joint3;

DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP, joint2, joint1);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE, joint1, joint2);
DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT, joint2, joint3);

s_pointFilter[XN_SKEL_RIGHT_HIP] = joint1;
s_pointFilter[XN_SKEL_RIGHT_KNEE] = joint2;
s_pointFilter[XN_SKEL_RIGHT_FOOT] = joint3;

                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER, joint2, joint1);
                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW, joint1, joint2);    
                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND, joint2, joint3);

                s_pointFilter[XN_SKEL_LEFT_SHOULDER] = joint1;
                s_pointFilter[XN_SKEL_LEFT_ELBOW] = joint2;
                s_pointFilter[XN_SKEL_LEFT_HAND] = joint3;

                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER, joint2, joint1);
                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW, joint1, joint2);
                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND, joint2, joint3);

                s_pointFilter[XN_SKEL_RIGHT_SHOULDER] = joint1;
                s_pointFilter[XN_SKEL_RIGHT_ELBOW] = joint2;
                s_pointFilter[XN_SKEL_RIGHT_HAND] = joint3;

                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP, joint2, joint1);
                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE, joint1, joint2);
                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT, joint2, joint3);

                s_pointFilter[XN_SKEL_LEFT_HIP] = joint1;
                s_pointFilter[XN_SKEL_LEFT_KNEE] = joint2;
                s_pointFilter[XN_SKEL_LEFT_FOOT] = joint3;

                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP, joint2, joint1);
                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE, joint1, joint2);
                DrawLimbAndGetJoint(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT, joint2, joint3);

                s_pointFilter[XN_SKEL_RIGHT_HIP] = joint1;
                s_pointFilter[XN_SKEL_RIGHT_KNEE] = joint2;
                s_pointFilter[XN_SKEL_RIGHT_FOOT] = joint3;

濾波算法核心代碼

static XnSkeletonJointPosition s_pointFilter[XN_SKEL_RIGHT_FOOT + 1];

/**
 * @brief   對空間座標點進行過濾.
 * @ingroup SceneDrawer
 *
 * @param   id 關節ID
 * @param   point [in out] 要過濾的點
 * @param   filter 過濾閥值
 */
inline void PointFilter(enum XnSkeletonJoint id,
                        XnSkeletonJointPosition &point,
                        const int filter)
{
    if (point.fConfidence < JOINT_CONFIDENCE)   // 過濾掉閥值以下的點
    {
        point = s_pointFilter[id];
    }
    else
    {
        if (s_pointFilter[id].fConfidence < JOINT_CONFIDENCE)
        {
            s_pointFilter[id] = point;
        }
        else
        {
            if (abs(s_pointFilter[id].position.X - point.position.X) <= filter &&
                abs(s_pointFilter[id].position.Y - point.position.Y) <= filter &&
                abs(s_pointFilter[id].position.Z - point.position.Z) <= filter)
            {
                point = s_pointFilter[id];
            }
            else
            {
                s_pointFilter[id] = point;
            }
        }
    }
}

static XnSkeletonJointPosition s_pointFilter[XN_SKEL_RIGHT_FOOT + 1];

/**
 * @brief   對空間座標點進行過濾.
 * @ingroup SceneDrawer
 * 
 * @param   id 關節ID
 * @param   point [in out] 要過濾的點
 * @param   filter 過濾閥值
 */
inline void PointFilter(enum XnSkeletonJoint id, 
                        XnSkeletonJointPosition &point,
                        const int filter)
{
    if (point.fConfidence < JOINT_CONFIDENCE)   // 過濾掉閥值以下的點
    {
        point = s_pointFilter[id];
    }
    else
    {
        if (s_pointFilter[id].fConfidence < JOINT_CONFIDENCE)
        {
            s_pointFilter[id] = point;
        }
        else
        {
            if (abs(s_pointFilter[id].position.X - point.position.X) <= filter &&
                abs(s_pointFilter[id].position.Y - point.position.Y) <= filter &&
                abs(s_pointFilter[id].position.Z - point.position.Z) <= filter)
            {
                point = s_pointFilter[id];
            }
            else
            {
                s_pointFilter[id] = point;
            }
        }
    }
}

完整源碼及論文

2015年9月6日更新下載鏈接：

論文

上位機源碼

2017年5月8日更新下載鏈接

論文

上位機源碼

下位機源碼