Kinect OpenNIによる3次元ポイントクラウド - 3次元描画

前回の続き
前回：Kinect OpenNIによる3次元ポイントクラウド - 3次元座標取得

前回の話でKinectからの奥行き情報から3次元座標を取得しました。

次はその3次元座標を目に見える形に描画しましょう。

ここでは3D描画にOpenGLというグラフィックスライブラリを用います。

OpenGLの導入方法を説明しません。

ということでサンプルどーん

#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
DepthMetaData depthMD;
ImageMetaData imageMD;
Context context;

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);        //ポイントクラウド描画

//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);
             
    //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);
}
// アイドル時のコールバック
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:
    case GLUT_RIGHT_BUTTON:
        cameraX -= (float) xDisp/40.0;
        cameraY += (float) yDisp/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){
    int SEQ = 0;//配列番号
    int channel = rgbImage.channels();
    int step = rgbImage.step;
    glPointSize(2);   //点の大きさ
    glBegin(GL_POINTS);  //今から点を描画しますよっと
    for(int y = 0;y < KINECT_DEPTH_HEGIHT;y++){
        for(int x = 0;x < KINECT_DEPTH_WIDTH;x++){
            //Point3f &point = &pointCloud_XYZ.at<Point3f>(y,x);
            Point3f &point = ((Point3f*)(pointCloud_XYZ.data + pointCloud_XYZ.step.p[0]*y))[x];
            if(point.z == 0)  //奥行きがとれてなければ描画しない
                continue;
            SEQ = y * step + x * channel;
            glColor3f(rgbImage.data[SEQ + 0] / 255.0f,rgbImage.data[SEQ + 1] / 255.0f,rgbImage.data[SEQ + 2] / 255.0f);
            glVertex3f(point.x,point.y,point.z);
        }
    }
    glEnd();  //描画終了
}
//3次元ポイントクラウドのための座標変換
void retrievePointCloudMap(Mat &depth,Mat &pointCloud_XYZ){
    static XnPoint3D proj[KINECT_DEPTH_HEGIHT * KINECT_DEPTH_WIDTH] = {0};
    int SEQ = 0;    //配列番号
    for(int y = 0; y < KINECT_DEPTH_HEGIHT; y++ ){
        for(int x = 0; x < KINECT_DEPTH_WIDTH; x++ ){  
            proj[SEQ].X = (XnFloat)x;
            proj[SEQ].Y = (XnFloat)y;
            proj[SEQ].Z = ((unsigned short*)(depth.data + depth.step.p[0]*y))[x] * 0.001f; // from mm to meters
            SEQ++;
        }
    }
    //現実座標に変換
    depthGenerator.ConvertProjectiveToRealWorld(KINECT_DEPTH_HEGIHT*KINECT_DEPTH_WIDTH, proj, (XnPoint3D*)pointCloud_XYZ.data);
}   

ほとんどOpenGLの設定です。

ここでポイントクラウド（Point Cloud）の処理を行っているのは
retrievePointCloudMap()と
drawPointCloud()だけですね。


OpenGLの説明をするとOpenGLの話だけになってしまいそうなので、基本的なところだけ説明します。


とりあえずdisplay()の中だけ説明。

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);
             
    //convert color space RGB2BGR
    cvtColor(image,image,CV_RGB2BGR);     
     
    imshow("image",image);
    imshow("depth",depth);
  
    glFlush();
    glutSwapBuffers();
}

glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); 
画面を初期化して消してます

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);
視点の向きをZ方向に向けます。

glFlush();
今までの処理をすべて実行する。
OpenGLでは関数を呼んでも、すぐには行われずいくつか溜まってから処理を行います。
なので、ここで溜まった処理を実行するわけです。

glutSwapBuffers();
描画を行います。
この描画はダブルバッファモードのときだけです。
詳しくは調べてください。


といった感じで描画してきます。

では点を描画している drawPointCloud()の説明を行います。

void drawPointCloud(Mat &rgbImage,Mat &pointCloud_XYZ){
    int SEQ = 0;//配列番号
    int channel = rgbImage.channels();
    int step = rgbImage.step;
    glPointSize(2);   //点の大きさ

    glBegin(GL_POINTS);  //今から点を描画しますよっと
    for(int y = 0;y < KINECT_DEPTH_HEGIHT;y++){
        for(int x = 0;x < KINECT_DEPTH_WIDTH;x++){
            //Point3f &point = &pointCloud_XYZ.at<Point3f>(y,x);
            Point3f &point = ((Point3f*)(pointCloud_XYZ.data + pointCloud_XYZ.step.p[0]*y))[x];
            if(point.z == 0)    //奥行きがとれてなければ描画しない
                continue;
            SEQ = y * step + x * channel;
            glColor3f(rgbImage.data[SEQ + 0] / 255.0f,rgbImage.data[SEQ + 1] / 255.0f,rgbImage.data[SEQ + 2] / 255.0f);
            glVertex3f(point.x,point.y,point.z);
        }
    }
    glEnd();  //描画終了
}

drawPointCloud(Mat &rgbImage,Mat &pointCloud_XYZ)
引数にRGBイメージと3次元座標を格納したMatクラスを入れます


glPointSize();
描画する点の大きさを設定します。

glBegin(GL_POINTS);
いまからglEnd()まで点を描画する処理を行いますよっということを意味する

で、for文の中

glColor3f();
点の色をr,g,bで設定

 glVertex3f();
引数のx,y,zに点を描画


とまあ、こんな感じで3次元ポイントクラウドを表現します。

サンプルプログラムはマウスで視点を変更することができます。

もっといろいろしてみたい！っという人はOpenGLを勉強してください。


んー、次はテクスチャマップでもしようかな？



追記：高速化目指しました。
Kinect OpenNIによる3次元ポイントクラウド - 3次元描画 高速化


追記：テクスチャマップ
Kinect OpenNIによる3次元テクスチャマップ - 3次元描画