かっしぃのメモ帳: Kinect OpenNIによる3次元テクスチャマップ

前回「Kinect OpenNIによる3次元ポイントクラウド - 3次元描画」でポイントクラウド(Point Cloud)にしてみました

しかし、ポイントクラウドでは近づいたときに点と点の隙間から奥の何もない空間が見えてしまい荒く見えてしまいます。

ということで、次はテクスチャマップ（Texture Maps）してみましょう。

テクスチャを貼るサンプル

//ポイントクラウド描画

void drawTextureMaps(Mat &rgbImage,Mat &pointCloud_XYZ){
 static int x,y;
 static uchar *p[2];
 static Point3f *point[2];
 int channel = rgbImage.channels();
 p[0] = rgbImage.data;          //上の色
 p[1] = rgbImage.data + rgbImage.step;      //下の色
 point[0] = (Point3f*)pointCloud_XYZ.data;     //上の座標
 point[1] = &((Point3f*)pointCloud_XYZ.data)[KINECT_DEPTH_WIDTH]; //下の座標
 for(y = 0;y < KINECT_DEPTH_HEGIHT - 1;y++){
  for(x = 0;x < KINECT_DEPTH_WIDTH - 1;x++,p[1] += channel,point[1]++,p[0] += channel,point[0]++){ 
   //奥行きが取得できてなかったら何もしない
   if(point[0]->z == 0) 
    continue;
   //対角の奥行きが遠ければテクスチャを貼らない
   if(abs(point[0]->z - (point[1] + 1)->z) > THRESHOLD || abs((point[0] + 1)->z - point[1]->z) > THRESHOLD) 
    continue;  

   //テクスチャを貼る
   glBegin(GL_TRIANGLE_STRIP);
   //左上
   glTexCoord2f(0, 0);
   glColor3ubv(p[0]);
   glVertex3f(point[0]->x,point[0]->y,point[0]->z);
   //左下
   glTexCoord2f(1, 0);
   glColor3ubv(p[1]);
   glVertex3f(point[1]->x,point[1]->y,point[1]->z);
   //右上
   glTexCoord2f(0, 1);
   glColor3ubv(p[0]+channel);
   glVertex3f((point[0] + 1)->x,(point[0] + 1)->y,(point[0] + 1)->z);
   //右下
   glTexCoord2f(1, 1);
   glColor3ubv(p[1]+channel);
   glVertex3f((point[1] + 1)->x,(point[1] + 1)->y,(point[1] + 1)->z);   

   glEnd();
  }
  p[0] += channel,point[0]++;
  p[1] += channel,point[1]++;
 }
}

//対角の奥行きが閾値より遠ければテクスチャを貼らない

if(abs(point[0]->z - (point[1] + 1)->z) > THRESHOLD ||
 abs((point[0] + 1)->z - point[1]->z) > THRESHOLD)       
         continue;

このif文は隣の画素が閾値より奥行き距離が離れていたらテクスチャを貼らないという処理です

ここで対角の距離を調べれば４点の奥行きがそれぞれ離れていないことがわかるんじゃないでしょうか。

ちなみに

glBegin(GL_TRIANGLE_STRIP);
と
glEnd();
の間に書かれた点で面を貼ることができます。

OpenGLの詳しい説明は調べてください。

ポインタを使ってOpenCVのMatクラスにアクセスしてるので高速化できてるかと思いますが・・・

もし、もっと速度が欲しい時は

GPUを使った描画などで高速化してはどうでしょうか

glewを使うんですかね？

今度調べるかもしれません。

全体サンプル

#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 drawTextureMaps(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();        //視点変更


//テクスチャを貼る閾値
#define THRESHOLD 0.1

//描画
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();  
    //テクスチャマップ
    drawTextureMaps(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:
        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 drawTextureMaps(Mat &rgbImage,Mat &pointCloud_XYZ){
 static int x,y;
 static uchar *p[2];
 static Point3f *point[2];
 int channel = rgbImage.channels();
 p[0] = rgbImage.data;          //上の色
 p[1] = rgbImage.data + rgbImage.step;      //下の色
 point[0] = (Point3f*)pointCloud_XYZ.data;     //上の座標
 point[1] = &((Point3f*)pointCloud_XYZ.data)[KINECT_DEPTH_WIDTH]; //下の座標
 for(y = 0;y < KINECT_DEPTH_HEGIHT - 1;y++){
  for(x = 0;x < KINECT_DEPTH_WIDTH - 1;x++,p[1] += channel,point[1]++,p[0] += channel,point[0]++){ 
   //奥行きが取得できてなかったら何もしない
   if(point[0]->z == 0) 
    continue;
   //対角の奥行きが遠ければテクスチャを貼らない
   if(abs(point[0]->z - (point[1] + 1)->z) > THRESHOLD || abs((point[0] + 1)->z - point[1]->z) > THRESHOLD) 
    continue;  

   //テクスチャを貼る
   glBegin(GL_TRIANGLE_STRIP);
   //左上
   glTexCoord2f(0, 0);
   glColor3ubv(p[0]);
   glVertex3f(point[0]->x,point[0]->y,point[0]->z);
   //左下
   glTexCoord2f(1, 0);
   glColor3ubv(p[1]);
   glVertex3f(point[1]->x,point[1]->y,point[1]->z);
   //右上
   glTexCoord2f(0, 1);
   glColor3ubv(p[0]+channel);
   glVertex3f((point[0] + 1)->x,(point[0] + 1)->y,(point[0] + 1)->z);
   //右下
   glTexCoord2f(1, 1);
   glColor3ubv(p[1]+channel);
   glVertex3f((point[1] + 1)->x,(point[1] + 1)->y,(point[1] + 1)->z);   

   glEnd();
  }
  p[0] += channel,point[0]++;
  p[1] += channel,point[1]++;
 }
}
//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);
}

サンプルダウンロード

github

4 件のコメント:

禊2012年10月5日 12:55
このサンプルコードはとても素晴らしいと思います！シンプルな構成でわかりやすくテクスチャマップが実装されて驚きました。

すみませんが、質問があります。
このサンプルコードにほんのちょちょっと手を加えて3D形状データをobjファイルで出力できるようにしてみたのですが、どうにも実測の形状と一致しません。
プログラムとしては単純で、こちらのサンプルのテクスチャマップ描画部分で取得した頂点座標と、その頂点の組み合わせによるポリゴンのデータを変数に格納してobjファイルにして出力するというものです。
ですが、これで机を出力すると、机の長さと高さの比率が実物と変化してしまいます。大まかな形状などは一致しているのですが、なぜか比率が若干異なります。おそらくdepthの値が多少現実と異なるのでは、と思うのですが…。ものすごく厚かましいのですが、何かいい考えは無いでしょうか？
返信削除
返信
禊2012年10月10日 11:03
なるほど、そういうものだったのですね。
私もその関数を頑張って求めて見ようと思います。
丁寧に教えてくださってありがとうございました！
返信削除
返信
windspeed80kmh2013年4月17日 5:42
Hello!
If I use OpenCV to get a binary image of an object with the background removed, how to render only the object with OpenGL and with the help of this example? Thank you.
返信削除
返信

コメントを追加

かっしぃのメモ帳

2011年10月5日水曜日

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

4 件のコメント:

人気の投稿