K4W v2 C++ Part 7a：繪製人體骨架

雖然最近一直在寫 Kinect 相關的程式了，不過已經很久沒寫這系列的文章了…而這一篇呢，基本上算是延續上一篇、《Part 7：偵測、追蹤人體骨架》，來簡單用範例來說明，怎麼把整個人的骨架畫出來。

這邊提供兩個版本，一個是用 OpenCV、以 2D 的形式來把骨架畫出來，程式在這裡；另一個則是使用 OpenGL，把骨架用 3D 的形式來繪製，程式在這邊。

在開始之前，先回過頭看一下之前在《Part 7：偵測、追蹤人體骨架》裡提到的內容。

K4W SDK v2 的人體架分析的介面，主要是 IBodyFrameSource、IBodyFrameReader、IBodyFrame、IBody 這系列以「Body」為名的類別，在使用上和 K4W SDK v2 其他的介面類似，要先透過 IKinectSensor 的物件來取得 IBodyFrameSource、然後開啟 IBodyFrameReader，之後在主迴圈裡面去取得封包成 IBodyFrame 這個介面的資料。

在 IBodyFrame 的物件裡面，則是包含了當下所有有在追蹤的使用者資訊，如果要取得的話，則是要透過 GetAndRefreshBodyData() 這個韓式，把資料寫入 IBody 的物件陣列中，之後則就可以個別讀取 IBody 內的資料了。這邊的程式可以寫成下面的形式：

IBodyFrame* pFrame = nullptr;
if (pBodyFrameReader->AcquireLatestFrame(&pFrame) == S_OK)
{
    // update body data
    pFrame->GetAndRefreshBodyData(iBodyCount, aBody);

    pFrame->Release();
    pFrame = nullptr;

    for (int i = 0; i < iBodyCount; ++i)
    {
        IBody* pBody = aBody[i];

        // Do things
    }
}

其中，iBodyCount 是可以追蹤的最大人數，在目前來說應該會是 6 人，可以透過 IBodyFrameSource 的 get_BodyCount() 這個函式來取得；aBody 的型別則是 IBody**，代表一個 IBody 指標的陣列，在使用前必須先初始化好。這部分的程式可以寫成：

INT32 iBodyCount = 0;
pFrameSource->get_BodyCount(&iBodyCount);

IBody** aBody = new IBody*[iBodyCount];
for (int i = 0; i < iBodyCount; ++i)
    aBody[i] = nullptr;

在把新的資料寫入 aBody 後，接下來就可以依序去讀取每個人的骨架資料了～不過這邊也要記得要先用 IBody 的get_IsTracked() 這個函式，來確認這個 IBody 是否正被追蹤。

之後呢，則就可以透過 GetJoints() 這個函式，來把所有的關節點的位置資訊都讀取出來了。而現行版本的 K4W SDK v2 提供的關節數量共有 25 個，算是相當多的了～

這些關節點被定義在 JointType 這個列舉型別，完整的列表可以參考前一篇文章、或是 MSDN。

個別關節點的型別是「Joint」，裡面記錄了關節點的類型（JointType）、位置（Position）以及狀態（TrackingState）。

其中，位置的型別是 CameraSpacePoint、是以「攝影機空間座標系統」裡的座標系統、來記錄關節點在三度空間中的位置；所以如果是要把相關的資料用在 2D 影像上的話，是需要做座標系統的轉換的。（請參考《使用 OpenGL 繪製場景》）

---

如果要直接透過 OpenGL、來把關節點連線畫出來的話，程式基本上可以寫成下面的形式：

Joint aJoints[JointType::JointType_Count];
if (pBody->GetJoints(JointType::JointType_Count, aJoints) == S_OK)
{
    glLineWidth(1.0f);
    glBegin(GL_LINES);
        glColor3f(1.0f, 1.0f, 1.0f);

        DrawLine(aJoints[JointType_SpineBase],        aJoints[JointType_SpineMid]);
        DrawLine(aJoints[JointType_SpineMid],        aJoints[JointType_SpineShoulder]);
        DrawLine(aJoints[JointType_SpineShoulder],    aJoints[JointType_Neck]);
        DrawLine(aJoints[JointType_Neck],            aJoints[JointType_Head]);

        DrawLine(aJoints[JointType_SpineShoulder],    aJoints[JointType_ShoulderLeft]);
        DrawLine(aJoints[JointType_ShoulderLeft],    aJoints[JointType_ElbowLeft]);
        DrawLine(aJoints[JointType_ElbowLeft],        aJoints[JointType_WristLeft]);
        DrawLine(aJoints[JointType_WristLeft],        aJoints[JointType_HandLeft]);
        DrawLine(aJoints[JointType_HandLeft],        aJoints[JointType_HandTipLeft]);
        DrawLine(aJoints[JointType_HandLeft],        aJoints[JointType_ThumbLeft]);

        DrawLine(aJoints[JointType_SpineShoulder],    aJoints[JointType_ShoulderRight]);
        DrawLine(aJoints[JointType_ShoulderRight],    aJoints[JointType_ElbowRight]);
        DrawLine(aJoints[JointType_ElbowRight],        aJoints[JointType_WristRight]);
        DrawLine(aJoints[JointType_WristRight],        aJoints[JointType_HandRight]);
        DrawLine(aJoints[JointType_HandRight],        aJoints[JointType_HandTipRight]);
        DrawLine(aJoints[JointType_HandRight],        aJoints[JointType_ThumbRight]);

        DrawLine(aJoints[JointType_SpineBase],        aJoints[JointType_HipLeft]);
        DrawLine(aJoints[JointType_HipLeft],        aJoints[JointType_KneeLeft]);
        DrawLine(aJoints[JointType_KneeLeft],        aJoints[JointType_AnkleLeft]);
        DrawLine(aJoints[JointType_AnkleLeft],        aJoints[JointType_FootLeft]);

        DrawLine(aJoints[JointType_SpineBase],        aJoints[JointType_HipRight]);
        DrawLine(aJoints[JointType_HipRight],        aJoints[JointType_KneeRight]);
        DrawLine(aJoints[JointType_KneeRight],        aJoints[JointType_AnkleRight]);
        DrawLine(aJoints[JointType_AnkleRight],        aJoints[JointType_FootRight]);

    glEnd();
}

這邊的作法，基本上就是把 25 個關節點、根據對應的關係來連線、並把線畫出來。可以看到在 glBegin() 和 glEnd() 之間的 24 個 DrawLine() 被分成五個區段，他們分別代表軀幹、左手、右手、左腳、右腳。

而 DrawLine() 這個函式的內容則如下：

void DrawLine(const Joint& rJ1, const Joint& rJ2)
{
    if (rJ1.TrackingState == TrackingState_NotTracked || rJ2.TrackingState == TrackingState_NotTracked)
        return;

    glVertex3f(rJ1.Position.X, rJ1.Position.Y, rJ1.Position.Z);
    glVertex3f(rJ2.Position.X, rJ2.Position.Y, rJ2.Position.Z);
}

在這裡面，會試著去檢察關節點的狀態，如果其中一個關節典謨有被追蹤到，就不會把線畫出來。

如果想要強調關節點的話，也可以以點的形式，把 25 個關節點都畫出來，程式可以寫成：

glPointSize(5.0f);
glBegin(GL_POINTS);
for (int i = 0; i < JointType_Count; ++ i )
{
    const Joint& rJoint = aJoints[i];
    if (rJoint.TrackingState == TrackingState_NotTracked)
        continue;
    else if(rJoint.TrackingState == TrackingState_Tracked)
        glColor3f(1.0f, 0.0f, 0.0f);
    else
        glColor3f(0.0f, 1.0f, 0.0f);
    glVertex3f(rJoint.Position.X, rJoint.Position.Y, rJoint.Position.Z);
}
glEnd();

這邊也會去檢查各關節點的狀態，並以不同的顏色來做繪製。

這樣繪製出來的話，就會像下圖的樣子（左邊大大的三原色線條是另外畫的座標軸）：

完整的範例程式，請參考這邊。

---

而如果想要用 OpenCV 來畫的話，則需要先把每個關節點的位置、由「攝影機空間座標系統」轉換到「深度空間座標系統」或「彩色空間座標系統」才行；而至於要轉換到哪一個，就是看要和哪種影像作套疊了。

在這邊的例子，Heresy 是把本來的彩色影像先畫出來（請參考之前的《Part 3：讀取彩色影像與紅外線影像》），然後在把骨架畫上去。

這邊的程式 Heresy 是寫成：

Joint aJoints[JointType::JointType_Count];
if (pBody->GetJoints(JointType::JointType_Count, aJoints) == S_OK)
{
    DrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_SpineMid], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_SpineMid], aJoints[JointType_SpineShoulder], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_Neck], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_Neck], aJoints[JointType_Head], pCoordinateMapper);

    DrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderLeft], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_ShoulderLeft], aJoints[JointType_ElbowLeft], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_ElbowLeft], aJoints[JointType_WristLeft], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_WristLeft], aJoints[JointType_HandLeft], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_HandLeft], aJoints[JointType_HandTipLeft], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_HandLeft], aJoints[JointType_ThumbLeft], pCoordinateMapper);

    DrawLine(mImg, aJoints[JointType_SpineShoulder], aJoints[JointType_ShoulderRight], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_ShoulderRight], aJoints[JointType_ElbowRight], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_ElbowRight], aJoints[JointType_WristRight], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_WristRight], aJoints[JointType_HandRight], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_HandRight], aJoints[JointType_HandTipRight], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_HandRight], aJoints[JointType_ThumbRight], pCoordinateMapper);

    DrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_HipLeft], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_HipLeft], aJoints[JointType_KneeLeft], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_KneeLeft], aJoints[JointType_AnkleLeft], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_AnkleLeft], aJoints[JointType_FootLeft], pCoordinateMapper);

    DrawLine(mImg, aJoints[JointType_SpineBase], aJoints[JointType_HipRight], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_HipRight], aJoints[JointType_KneeRight], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_KneeRight], aJoints[JointType_AnkleRight], pCoordinateMapper);
    DrawLine(mImg, aJoints[JointType_AnkleRight], aJoints[JointType_FootRight], pCoordinateMapper);
}

可以看到，程式的部分大致上就是把 OpenGL 範例裡的 OpenGL 程式拿掉、然後一樣去呼叫 DrawLine() 這個函式。不過由於額外的需求，所以 DrawLine() 的參數有些變化，他的程式是：

void DrawLine(cv::Mat& rImg, const Joint& rJ1, const Joint& rJ2, ICoordinateMapper* pCMapper )
{
    if (rJ1.TrackingState == TrackingState_NotTracked || rJ2.TrackingState == TrackingState_NotTracked)
        return;

    ColorSpacePoint ptJ1, ptJ2;
    pCMapper->MapCameraPointToColorSpace(rJ1.Position, &ptJ1);
    pCMapper->MapCameraPointToColorSpace(rJ2.Position, &ptJ2);

    cv::line(rImg, cv::Point(ptJ1.X, ptJ1.Y), cv::Point(ptJ2.X, ptJ2.Y), cv::Vec3b(0, 0, 255), 5);
}

這邊一樣會先針對關節的狀態做檢查，如果沒有被追蹤到的話，就可以不用畫了。

而之後呢，則是需要先透過 pCMapper 這個 ICoordinateMapper 的物件，來將關節點的位置由「攝影機空間座標系統」轉換到「彩色空間座標系統」；這邊使用的函式是 MapCameraPointToColorSpace()、一次轉換一個點。

轉換好了之後，接下來就是使用 OpenCV 的 cv::line() 這個函式，在指定的影像上畫線了～這邊的 rImg 就是代表之前的彩色影像。

在所有的線條都畫好之後，接下來再用把整張畫好的彩色影像顯示出來就可以了～完整的範例程式可以參考這裡。下面就是程式執行的截圖：

這邊比較有趣的，應該是 K4W SDK 把椅子也當成一個人了！ XD
所以可以看到，除了左邊的人有追蹤到骨架之外，椅子上也有一個骨架。 ^^"

---

其實這邊的東西在前一篇都已經寫了，這邊主要是簡單解釋一下範例啦～

而實際上，Heresy 在使用 Kinect for Windows SDK v2 的人體骨架追蹤的時候，是覺得在這部分微軟真的進步很多！他的人體骨架的追蹤速度變得很快，常常只要進到攝影機視角內、稍微動一下就馬上能被追蹤到了～甚至可能只要坐在椅子上舉個手，都可以被追蹤到！而且，當有被追蹤到、且關節點都在視野內的時候，追蹤的品質大多不錯～

不過相對的，他還是有一定程度的誤判的可能性。像上面的圖片就是一個很好的例子。這部分，目前還沒研究是否有辦法透過參數的調整來避免，不過個人覺得可以的機會應該不大就是了…

---

Kinect for Windows v2 C++ 程式開發目錄