2012年2月5日日曜日

OpenNIによるポイントクラウドと骨格追跡 - 3次元描画

OpenNIは骨格追跡を行うことができ、更に各関節の3次元位置を取得することができます。

今回は、OpenGLを使ったポイントクラウドに各関節を描画してみます。


まず、ユーザ検出プログラムの説明を行います。

その前に、OpenNIで骨格追跡を有効にするため「SamplesConfig.xml」に
<Node type="User"/>
を追加します。


では、プログラムの説明を行います。


まず、初期化のところに以下のプログラムを加えます。

   
context.FindExistingNode(XN_NODE_TYPE_USER, userGenerator);
xn::SkeletonCapability skeleton = userGenerator.GetSkeletonCap();

// ユーザー認識のコールバックを登録
// キャリブレーションのコールバックを登録
XnCallbackHandle userCallbacks, calibrationCallbacks;
userGenerator.RegisterUserCallbacks(&::UserDetected, 0, 0, userCallbacks);
skeleton.RegisterCalibrationCallbacks( 0, &::CalibrationEnd, 0, calibrationCallbacks );
skeleton.SetSkeletonProfile(XN_SKEL_PROFILE_ALL);

ここで、UserGeneratorの準備とキャリブレーションのコールバックを登録しました。

UserDetectedはユーザの検出時のコールバック
CalibrationEndはユーザロスト時のコールバックです

// ユーザー検出
void XN_CALLBACK_TYPE UserDetected( xn::UserGenerator& generator, XnUserID nId, void* pCookie )
{
    printf("ユーザID%d 検出  %d人目\n",nId,generator.GetNumberOfUsers());
    generator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
// キャリブレーションの終了
void XN_CALLBACK_TYPE CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie)
{
    if ( bSuccess ) {
        printf("ユーザID%d キャリブレーション成功\n",nId);
        capability.StartTracking(nId);
    }
    else {
        printf("ユーザID%d キャリブレーション失敗\n",nId);
    }
}


では、実際にユーザの骨格位置の取得を行います。


XnUserID aUsers[15];
XnUInt16 nUsers = 15;
userGenerator.GetUsers(aUsers, nUsers);
for (int i = 0; i < nUsers; ++i){
    if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i])){
        XnSkeletonJointPosition joint;
        //aUsers[i]の頭部位置をjointに格納
        userGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_HEAD, joint);
    }
}

上記のプログラムは全ユーザの頭部位置をjoingに格納して取得するプログラム例です。

・各関数の説明
userGenerator.GetUsers(aUsers, nUsers);
    aUsersの配列に検出されたユーザのIDを格納
    nUsersに検出したユーザの数を格納

userGenerator.GetSkeletonCap().IsTracking(aUsers[i]);
    aUsers[i]がトラッキングされているかを返す
       
userGenerator.GetSkeletonCap().GetSkeletonJointPosition(aUsers[i], XN_SKEL_HEAD, joint);
    aUsers[i]の頭部位置をjointに格納

    XN_SKEL_HEADは「enum XnSkeletonJoint」で宣言されており、他にも
        XN_SKEL_HEAD    
        XN_SKEL_NECK    
        XN_SKEL_TORSO    
        XN_SKEL_WAIST    
        XN_SKEL_LEFT_COLLAR    
        XN_SKEL_LEFT_SHOULDER    
        XN_SKEL_LEFT_ELBOW    
        XN_SKEL_LEFT_WRIST    
        XN_SKEL_LEFT_HAND    
        XN_SKEL_LEFT_FINGERTIP    
        XN_SKEL_RIGHT_COLLAR    
        XN_SKEL_RIGHT_SHOULDER    
        XN_SKEL_RIGHT_ELBOW    
        XN_SKEL_RIGHT_WRIST    
        XN_SKEL_RIGHT_HAND    
        XN_SKEL_RIGHT_FINGERTIP    
        XN_SKEL_LEFT_HIP    
        XN_SKEL_LEFT_KNEE    
        XN_SKEL_LEFT_ANKLE    
        XN_SKEL_LEFT_FOOT    
        XN_SKEL_RIGHT_HIP    
        XN_SKEL_RIGHT_KNEE    
        XN_SKEL_RIGHT_ANKLE    
        XN_SKEL_RIGHT_FOOT
    と24箇所の関節が取得できる。



では、以上のことを踏まえてOpenGLで骨格を描画してみます。

プログラムは「Kinect OpenNIによる3次元ポイントクラウド - 3次元描画 高速化」のプログラムに
追加していく感じでやってきます。

#include <GL/glut.h>
#include <opencv2/opencv.hpp>
#ifdef _DEBUG
    //Debugモードの場合
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_core220d.lib")           // opencv_core
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_imgproc220d.lib")        // opencv_imgproc
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_highgui220d.lib")        // opencv_highgui
#else
    //Releaseモードの場合
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_core220.lib")            // opencv_core
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_imgproc220.lib")        // opencv_imgproc
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_highgui220.lib")        // opencv_highgui
#endif

#include <XnCppWrapper.h>
#pragma comment(lib,"C:/Program files/OpenNI/Lib/openNI.lib")

#define SAMPLE_XML_PATH "C:/Program Files/OpenNI/Data/SamplesConfig.xml"
using namespace cv;
using namespace xn;

//openNIのための宣言・定義
//マクロ定義
#define KINECT_IMAGE_WIDTH 640
#define KINECT_IMAGE_HEGIHT 480
#define KINECT_DEPTH_WIDTH 640
#define KINECT_DEPTH_HEGIHT 480

DepthGenerator depthGenerator;// depth context
ImageGenerator imageGenerator;//image context
UserGenerator userGenerator;
DepthMetaData depthMD;
ImageMetaData imageMD;
Context context;
//トラッキングの際のコールバック
void XN_CALLBACK_TYPE UserDetected( xn::UserGenerator& generator, XnUserID nId, void* pCookie );
void XN_CALLBACK_TYPE CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie);


Mat image(480,640,CV_8UC3);
Mat depth(480,640,CV_16UC1);  
//ポイントクラウドの座標
Mat pointCloud_XYZ(480,640,CV_32FC3,cv::Scalar::all(0));

void retrievePointCloudMap(Mat &depth,Mat &pointCloud_XYZ); //3次元ポイントクラウドのための座標変換
void drawPointCloud(Mat &rgbImage,Mat &pointCloud_XYZ);  //ポイントクラウド描画
void drawUserJoint();          //ユーザの関節を描画
void DrawLimb(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2);
void DrawJoint(XnUserID player, XnSkeletonJoint eJoint1);

//openGLのための宣言・定義
//---変数宣言---
int FormWidth = 640;
int FormHeight = 480;
int mButton;
float twist, elevation, azimuth;
float cameraDistance = 0,cameraX = 0,cameraY = 0;
int xBegin, yBegin;
//---マクロ定義---
#define glFovy 45        //視角度
#define glZNear 1.0        //near面の距離
#define glZFar 150.0    //far面の距離
void polarview();        //視点変更

//描画
void display(){  
    // clear screen and depth buffer
    glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); 
    // Reset the coordinate system before modifying
    glLoadIdentity();   
    glEnable(GL_DEPTH_TEST); //「Zバッファ」を有効
    gluLookAt(0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0);   //視点の向き設定
    //wait and error processing
    context.WaitAnyUpdateAll();

    imageGenerator.GetMetaData(imageMD);
    depthGenerator.GetMetaData(depthMD);
    depthGenerator.GetAlternativeViewPointCap().SetViewPoint(imageGenerator);//ズレを補正
         
    memcpy(image.data,imageMD.Data(),image.step * image.rows);    //イメージデータを格納
    memcpy(depth.data,depthMD.Data(),depth.step * depth.rows);    //深度データを格納
      
    //3次元ポイントクラウドのための座標変換
    retrievePointCloudMap(depth,pointCloud_XYZ);

    //視点の変更
    polarview();  
    //ポイントクラウド
    drawPointCloud(image,pointCloud_XYZ);

 //関節の描画
 drawUserJoint();
             
    //convert color space RGB2BGR
    cvtColor(image,image,CV_RGB2BGR);     
     
    imshow("image",image);
    imshow("depth",depth);
  
    glFlush();
    glutSwapBuffers();
}
//初期化
void init(){
    context.InitFromXmlFile(SAMPLE_XML_PATH); 
    context.FindExistingNode(XN_NODE_TYPE_DEPTH, depthGenerator); 
    context.FindExistingNode(XN_NODE_TYPE_IMAGE, imageGenerator);
 context.FindExistingNode(XN_NODE_TYPE_USER, userGenerator);   
    if (!userGenerator.IsCapabilitySupported(XN_CAPABILITY_SKELETON)) {   // スケルトン・トラッキングをサポートしているか確認
  printf("ユーザー検出をサポートしてません\n");
    }

    // キャリブレーションにポーズが必要
    xn::SkeletonCapability skeleton = userGenerator.GetSkeletonCap();
    if ( skeleton.NeedPoseForCalibration() ) {
  printf("このOpenNIのバージョンはポーズ無しには対応していません\n");
    }

    // ユーザー認識のコールバックを登録
    // キャリブレーションのコールバックを登録
    XnCallbackHandle userCallbacks, calibrationCallbacks;
    userGenerator.RegisterUserCallbacks(&::UserDetected, 0, 0, userCallbacks);
    skeleton.RegisterCalibrationCallbacks( 0, &::CalibrationEnd, 0, calibrationCallbacks );

    skeleton.SetSkeletonProfile(XN_SKEL_PROFILE_ALL);
}
// アイドル時のコールバック
void idle(){
    //再描画要求
    glutPostRedisplay();
}
//ウィンドウのサイズ変更
void reshape (int width, int height){
    FormWidth = width;
    FormHeight = height;
    glViewport (0, 0, (GLsizei)width, (GLsizei)height);
    glMatrixMode (GL_PROJECTION);
    glLoadIdentity ();
    //射影変換行列の指定
    gluPerspective (glFovy, (GLfloat)width / (GLfloat)height,glZNear,glZFar); 
    glMatrixMode (GL_MODELVIEW);
}
//マウスの動き
void motion(int x, int y){
    int xDisp, yDisp;  
    xDisp = x - xBegin;
    yDisp = y - yBegin;
    switch (mButton) {
    case GLUT_LEFT_BUTTON:
        azimuth += (float) xDisp/2.0;
        elevation -= (float) yDisp/2.0;
        break;
    case GLUT_MIDDLE_BUTTON:
        cameraX -= (float) xDisp/40.0;
        cameraY += (float) yDisp/40.0;
        break;
    case GLUT_RIGHT_BUTTON:
  cameraDistance += (float) xDisp/40.0;
        break;
    }
    xBegin = x;
    yBegin = y;
}
//マウスの操作
void mouse(int button, int state, int x, int y){
    if (state == GLUT_DOWN) {
        switch(button) {
        case GLUT_RIGHT_BUTTON:
        case GLUT_MIDDLE_BUTTON:
        case GLUT_LEFT_BUTTON:
            mButton = button;
            break;
        }
        xBegin = x;
        yBegin = y;
    }
}
//視点変更
void polarview(){
    glTranslatef( cameraX, cameraY, cameraDistance);
    glRotatef( -twist, 0.0, 0.0, 1.0);
    glRotatef( -elevation, 1.0, 0.0, 0.0);
    glRotatef( -azimuth, 0.0, 1.0, 0.0);
}
//メイン
int main(int argc, char *argv[]){
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DEPTH | GLUT_DOUBLE | GLUT_RGB);
    glutInitWindowSize(FormWidth, FormHeight);
    glutCreateWindow(argv[0]);
    //コールバック
    glutReshapeFunc (reshape);
    glutDisplayFunc(display);
    glutIdleFunc(idle);
    glutMouseFunc(mouse);
    glutMotionFunc(motion);
    init();
    glutMainLoop();
    context.Shutdown();
    return 0;
}

//ポイントクラウド描画
void drawPointCloud(Mat &rgbImage,Mat &pointCloud_XYZ){
 static int x,y;
 glPointSize(2);
 glBegin(GL_POINTS);
 uchar *p = rgbImage.data;
 Point3f *point = (Point3f*)pointCloud_XYZ.data;
 for(y = 0;y < KINECT_DEPTH_HEGIHT;y++){
  for(x = 0;x < KINECT_DEPTH_WIDTH;x++,p += 3,point++){ 
   if(point->z == 0)
    continue;
   glColor3ubv(p);
   glVertex3f(point->x,point->y,point->z);
  }
 }
 glEnd();
}
//3次元ポイントクラウドのための座標変換
void retrievePointCloudMap(Mat &depth,Mat &pointCloud_XYZ){
 static const int size = KINECT_DEPTH_HEGIHT * KINECT_DEPTH_WIDTH;
 static XnPoint3D proj[size] = {0};
 static int x,y;
 XnPoint3D *p = proj;
 unsigned short* dp = (unsigned short*)depth.data;
 for(y = 0; y < KINECT_DEPTH_HEGIHT; y++ ){
  for(x = 0; x < KINECT_DEPTH_WIDTH; x++, p++, dp++){ 
   p->X = x;
   p->Y = y;
   p->Z = *dp * 0.001f; // from mm to meters
  }
 }
 //現実座標に変換
 depthGenerator.ConvertProjectiveToRealWorld(size, proj, (XnPoint3D*)pointCloud_XYZ.data);
}   

//人物の骨格を描画
void drawUserJoint(){
 XnUserID aUsers[15];
 XnUInt16 nUsers = 15;
 userGenerator.GetUsers(aUsers, nUsers);
 for (int i = 0; i < nUsers; ++i){
  if (userGenerator.GetSkeletonCap().IsTracking(aUsers[i])){
   //各関節に点を打つ
   glPointSize(5);
   glBegin(GL_POINTS);  
    glColor4f(0,255,0, 1); 
    for(int Joint = XN_SKEL_HEAD;Joint < XN_SKEL_RIGHT_FOOT + 1;Joint++)
     DrawJoint(aUsers[i],(XnSkeletonJoint)Joint);
   glEnd();

   //各肢に線を書く
   glBegin(GL_LINES);
    glColor4f(255,0,0, 1);
    DrawLimb(aUsers[i], XN_SKEL_HEAD, XN_SKEL_NECK);

    DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_LEFT_SHOULDER);
    DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_LEFT_ELBOW);
    DrawLimb(aUsers[i], XN_SKEL_LEFT_ELBOW, XN_SKEL_LEFT_HAND);

    DrawLimb(aUsers[i], XN_SKEL_NECK, XN_SKEL_RIGHT_SHOULDER);
    DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_RIGHT_ELBOW);
    DrawLimb(aUsers[i], XN_SKEL_RIGHT_ELBOW, XN_SKEL_RIGHT_HAND);

    DrawLimb(aUsers[i], XN_SKEL_LEFT_SHOULDER, XN_SKEL_TORSO);
    DrawLimb(aUsers[i], XN_SKEL_RIGHT_SHOULDER, XN_SKEL_TORSO);

    DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_LEFT_HIP);
    DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_LEFT_KNEE);
    DrawLimb(aUsers[i], XN_SKEL_LEFT_KNEE, XN_SKEL_LEFT_FOOT);

    DrawLimb(aUsers[i], XN_SKEL_TORSO, XN_SKEL_RIGHT_HIP);
    DrawLimb(aUsers[i], XN_SKEL_RIGHT_HIP, XN_SKEL_RIGHT_KNEE);
    DrawLimb(aUsers[i], XN_SKEL_RIGHT_KNEE, XN_SKEL_RIGHT_FOOT);

    DrawLimb(aUsers[i], XN_SKEL_LEFT_HIP, XN_SKEL_RIGHT_HIP);

   glEnd();
  }
 }
}
void DrawJoint(XnUserID player, XnSkeletonJoint eJoint){
 XnSkeletonJointPosition joint;
 userGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);

 //信頼性の低い場合描画しない
 if (joint.fConfidence < 0.5)
  return;

 glVertex3f(joint.position.X* 0.001f, joint.position.Y* 0.001f, joint.position.Z* 0.001f);
}
void DrawLimb(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2){
 XnSkeletonJointPosition joint1, joint2;
 userGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
 userGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);

 //信頼性の低い場合描画しない
 if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
  return;

 glVertex3f(joint1.position.X* 0.001f, joint1.position.Y* 0.001f, joint1.position.Z* 0.001f);
 glVertex3f(joint2.position.X* 0.001f, joint2.position.Y* 0.001f, joint2.position.Z* 0.001f);
} 
// ユーザー検出
void XN_CALLBACK_TYPE UserDetected( xn::UserGenerator& generator, XnUserID nId, void* pCookie )
{
 printf("ユーザID%d 検出  %d人目\n",nId,generator.GetNumberOfUsers());
    generator.GetSkeletonCap().RequestCalibration(nId, TRUE);
}
// キャリブレーションの終了
void XN_CALLBACK_TYPE CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie)
{
    if ( bSuccess ) {
  printf("ユーザID%d キャリブレーション成功\n",nId);
        capability.StartTracking(nId);
    }
    else {
  printf("ユーザID%d キャリブレーション失敗\n",nId);
    }
}



プログラムダウンロード

github

0 件のコメント:

コメントを投稿