使用 OpenCV 畫出 NiTE2 的人體骨架

這篇基本上是《NiTE2 的人體骨架追蹤》的延伸，算是提供一個以 OpenCV 來做顯示的完整地 NiTE 2 OpenNI 2 的人體骨架追蹤範例；另外，他也算是從《用 OpenCV 畫出 OpenNI 2 的深度、彩色影像》延伸出來的範例，如果還沒看過這兩篇文章的話，建議先看一下。

而這個範例程式所做的事，主要就是透過 OpenNI 2 的 VideoStream 來讀取彩色影像當作背景，並透過 NiTE 2 的 UserTracker 來讀取人體骨架關節點的資訊，並以圓和線、畫出來。最後的結果，應該會像右圖這樣子。

下面就是這個程式的主要架構：

// STL Header
#include <iostream>
  
// OpenCV Header
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
  
// o1. OpenNI Header
#include <OpenNI.h>
  
// n1. NiTE Header
#include <NiTE.h>
  
// namespace
using namespace std;
using namespace openni;
using namespace nite;
  
int main( int argc, char **argv )
{
  // o2. Initial OpenNI
  OpenNI::initialize();
  
  // o3. Open Device
  Device  mDevice;
  mDevice.open( ANY_DEVICE );
  
  // o4. create depth stream
  VideoStream mDepthStream;
  mDepthStream.create( mDevice, SENSOR_DEPTH );
  // o4a. set video mode
  VideoMode mDMode;
  mDMode.setResolution( 640, 480 );
  mDMode.setFps( 30 );
  mDMode.setPixelFormat( PIXEL_FORMAT_DEPTH_1_MM );
  mDepthStream.setVideoMode( mDMode);
  
  // o5. Create color stream
  VideoStream mColorStream;
  mColorStream.create( mDevice, SENSOR_COLOR );
  // o5a. set video mode
  VideoMode mCMode;
  mCMode.setResolution( 640, 480 );
  mCMode.setFps( 30 );
  mCMode.setPixelFormat( PIXEL_FORMAT_RGB888 );
  mColorStream.setVideoMode( mCMode);
   
  // o6. image registration
  mDevice.setImageRegistrationMode( IMAGE_REGISTRATION_DEPTH_TO_COLOR );
  
  // n2. Initial NiTE
  NiTE::initialize();
  
  // n3. create user tracker
  UserTracker mUserTracker;
  mUserTracker.create( &mDevice );
  mUserTracker.setSkeletonSmoothingFactor( 0.1f );
  
  // create OpenCV Window
  cv::namedWindow( "User Image",  CV_WINDOW_AUTOSIZE );
  
  // p1. start
  mColorStream.start();
  mDepthStream.start();
  
  while( true )
  {
    // main loop
    // p2 - p5 ...
  
    // p6. check keyboard
    if( cv::waitKey( 1 ) == 'q' )
      break;
  }
  
  // p7. stop
  mUserTracker.destroy();
  mColorStream.destroy();
  mDepthStream.destroy();
  mDevice.close();
  NiTE::shutdown();
  OpenNI::shutdown();
  
  return 0;
}

基本上，在前面「o2」到「o6」的部分，都是 OpenNI 的初始化與設定。在這段程式碼裡面，除了進行 OpenNI 的初始化外，還建立出深度、以及彩色影像用的 VideoStream；這邊之後雖然都不會直接讀取到深度影像，不過因為 NiTE 2 的 UserTracker 會使用深度影像的資料，所以為了讓彩色影像和深度影相的大小是一致的（這邊是 640×480），所以要建立出深度影像的 VideoStream（mDepthStream），並針對他進行相關的設定。

而之後，「n2」、「n3」的部分，則是 NiTE 2 的初始化，以及 UserTracker 的建立、設定了。

接下來，就是透過 OpenCV，建立一個名為「User Image」的視窗，準備將來用來做顯示之用；都好了之後，就是開始 OpenNI VideoStream 的資料讀取（p1），並進入主迴圈了～在上面的程式碼裡面，這部分是先省略掉的，會在接下來的部分做說明。

而裡面的「p6」的部分，則是去檢查是否有按下鍵盤的「q」，如果有的話，就離開主迴圈，並將 NiTE 和 OpenNI 的所有物件都關閉、並停止程式（p7）。

至於迴圈裡主要處理的部分，程式碼基本上就是下面的樣子：

// p2. prepare background
cv::Mat cImageBGR;
// p2a. get color frame
VideoFrameRef mColorFrame;
mColorStream.readFrame( &mColorFrame );
// p2b. convert data to OpenCV format
const cv::Mat mImageRGB( mColorFrame.getHeight(), mColorFrame.getWidth(),
                         CV_8UC3, (void*)mColorFrame.getData() );
// p2c. convert form RGB to BGR
cv::cvtColor( mImageRGB, cImageBGR, CV_RGB2BGR );
 
// p3. get user frame
UserTrackerFrameRef  mUserFrame;
mUserTracker.readFrame( &mUserFrame );
 
// p4. get users data
const nite::Array<UserData>& aUsers = mUserFrame.getUsers();
for( int i = 0; i < aUsers.getSize();    i )
{
  const UserData& rUser = aUsers[i];
 
  // p4a. check user status
  if( rUser.isNew() )
  {
    // start tracking for new user
    mUserTracker.startSkeletonTracking( rUser.getId() );
  }
 
  if( rUser.isVisible() )
  {
    // p4b. get user skeleton
    const Skeleton& rSkeleton = rUser.getSkeleton();
    if( rSkeleton.getState() == SKELETON_TRACKED )
    {
      // p4c. build joints array
      SkeletonJoint aJoints[15];
      aJoints[ 0] = rSkeleton.getJoint( JOINT_HEAD );
      aJoints[ 1] = rSkeleton.getJoint( JOINT_NECK );
      aJoints[ 2] = rSkeleton.getJoint( JOINT_LEFT_SHOULDER );
      aJoints[ 3] = rSkeleton.getJoint( JOINT_RIGHT_SHOULDER );
      aJoints[ 4] = rSkeleton.getJoint( JOINT_LEFT_ELBOW );
      aJoints[ 5] = rSkeleton.getJoint( JOINT_RIGHT_ELBOW );
      aJoints[ 6] = rSkeleton.getJoint( JOINT_LEFT_HAND );
      aJoints[ 7] = rSkeleton.getJoint( JOINT_RIGHT_HAND );
      aJoints[ 8] = rSkeleton.getJoint( JOINT_TORSO );
      aJoints[ 9] = rSkeleton.getJoint( JOINT_LEFT_HIP );
      aJoints[10] = rSkeleton.getJoint( JOINT_RIGHT_HIP );
      aJoints[11] = rSkeleton.getJoint( JOINT_LEFT_KNEE );
      aJoints[12] = rSkeleton.getJoint( JOINT_RIGHT_KNEE );
      aJoints[13] = rSkeleton.getJoint( JOINT_LEFT_FOOT );
      aJoints[14] = rSkeleton.getJoint( JOINT_RIGHT_FOOT );
 
      // p4d. convert joint position to image
      cv::Point2f aPoint[15];
      for( int  s = 0; s < 15;    s )
      {
        const Point3f& rPos = aJoints[s].getPosition();
        mUserTracker.convertJointCoordinatesToDepth( 
                                 rPos.x, rPos.y, rPos.z,
                                 &(aPoint[s].x), &(aPoint[s].y) );
      }
 
      // p4e. draw line
      cv::line( cImageBGR, aPoint[ 0], aPoint[ 1], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[ 1], aPoint[ 2], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[ 1], aPoint[ 3], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[ 2], aPoint[ 4], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[ 3], aPoint[ 5], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[ 4], aPoint[ 6], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[ 5], aPoint[ 7], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[ 1], aPoint[ 8], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[ 8], aPoint[ 9], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[ 8], aPoint[10], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[ 9], aPoint[11], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[10], aPoint[12], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[11], aPoint[13], cv::Scalar( 255, 0, 0 ), 3 );
      cv::line( cImageBGR, aPoint[12], aPoint[14], cv::Scalar( 255, 0, 0 ), 3 );
 
      // p4f. draw joint
      for( int  s = 0; s < 15;    s )
      {
        if( aJoints[s].getPositionConfidence() > 0.5 )
          cv::circle( cImageBGR, aPoint[s], 3, cv::Scalar( 0, 0, 255 ), 2 );
        else
          cv::circle( cImageBGR, aPoint[s], 3, cv::Scalar( 0, 255, 0 ), 2 );
      }
    }
  }
}

// p5. show image
cv::imshow( "User Image", cImageBGR );

在「p2」的部分，就是從 mColorStream 裡，讀取出彩色感應器的影像，並請轉換成 OpenCV 的格式，也就是 cImageBGR 這個物件，並在之後當作背景來使用。

接下來的「p3」，則是從 mUserTracker 裡，讀取出當下的 UserTracker 分析結果。在「p4」，則是取出分析結果中，使用者的陣列 aUser，並針對裡面每一個 user、依序做處理。而處理的第一部，就是「p4a」，也就是去檢查這個 user 是否是新發現的使用者，如果是的話，就呼叫 UserTracker 的 startSkeletonTracking() 這個函式，開始對這個 user 進行骨架的追蹤。

而如果在使用者是可以看的到的狀況下，就是要開始處理骨架的資料了～在「p4b」就是先取出使用者的骨架資料，並確定目前正在追蹤他的骨架。接下來，則是建立一個大小為 15 的 SkeletonJoint 的陣列，並依序把 15 個關節點的資料都讀取出來（p4c）。

由於關節點的位置是在世界座標系統上，所以如果要用 OpenCV 把他們畫到彩色影像上的話，需要先做座標系統的轉換，把關節點的位置轉換到深度座標系統上；這邊，就是上方「p4d」的部分，轉換過後的點位資料，會儲存在 aPoint 這個 cv::Point2f 型別的陣列裡面。

到上面為止，基本上都算是關節資料的前置處理。處理之後，接下來就是要把關節的相關資訊畫出來了～在「p4e」的部分，是透過 cv::line() 把對應的關節和關節之間，連線連起來畫在 cImageBGR 上。而在「p4f」的部分，則是在把每一個關節點，依序用 cv::circle()，畫出一個一個圓；Heresy 這邊稍微特別一點的，是有去檢查各個關節點位置的可靠度，如果可靠度大於 0.5 的話，就用紅色畫，不然就用綠色。

最後，就是「p5」的部分，用 cv::imshow() 把最後的結果、也就是 cImageBGR 畫出來了～