Kinectプチハック（エピソード4）

【ミッション： Kinectで立体復元に挑戦しよう】

前回奥行き情報を（一応）推定する事ができたので、そろそろ 3D にしてみようと思う。

本日のターゲットは、このどうしようもない写真。

これを、Kinect で適当に 3 次元復元して俯瞰したら、こうなった。

机上の本や PC はもちろん、部屋の隅っこの柱やパイプなどの凹凸がくっきり浮き出ている事が確認できる。

※ ちなみにぼくの隣は博士様の席なのだ。いないけど。

---

理論自体はほとんどそのままなのだが、前のソースは数値計算の精度的にアレだったり座標系の置き方的にアレだったりしたので、ほんのわずかながら改良を加えた。

ちなみに、依然として視点操作ができない。この辺はダメなままである。あと、毎度の事ながら、OpenCV 2.0 以外のバージョンではうまくビルドできない可能性があるので注意。もしダメそうだったらリンクするライブラリとかインクルードするヘッダファイルとかをいじってみてね。

view plainprint?

1. #include<cv.h>  
2. #include<highgui.h>  
3. #include<XnCppWrapper.h>  
4.   
5. #include<GL/glut.h>  
6. #include<limits.h>  
7.   
8. #pragma comment(lib,"cv200.lib")  
9. #pragma comment(lib,"cxcore200.lib")  
10. #pragma comment(lib,"highgui200.lib")  
11. #pragma comment(lib,"C:/Program files/OpenNI/Lib/openNI.lib")  
12.   
13. #pragma comment(lib, "glut32.lib")  
14.   
15. const int IMAGE_WIDTH  = 640;  
16. const int IMAGE_HEIGHT = 480;  
17.   
18. const char* SAMPLE_XML_PATH =   
19.     "C:/Program Files/OpenNI/Data/SamplesConfig.xml";  
20.   
21. // グローバル変数。使い過ぎよくない。  
22. xn::Context context;  
23. xn::EnumerationErrors errors;  
24. xn::DepthGenerator depth;    
25. xn::ImageGenerator image;  
26. xn::DepthMetaData depthMD;  
27. xn::ImageMetaData imageMD;  
28.   
29. cv::Ptr<IplImage> iplimage, ipldepth;  
30.   
31. // Kinectカメラの内部パラメータ  
32. const double f  = 526.37013657;  
33. const double cx = 313.68782938;  
34. const double cy = 259.01834898;  
35.   
36.   
37. // ==================  
38. // 暇になったら再描画  
39. // ==================  
40. void idle() {  
41.     glutPostRedisplay();  
42. }  
43.   
44. void display() {  
45.     context.WaitAnyUpdateAll();  
46.   
47.     // 深度・画像メタデータ取得  
48.     depth.GetMetaData(depthMD);  
49.     image.GetMetaData(imageMD);  
50.           
51.     // 深度データの座標ををカメラに合わせる  
52.     depth.GetAlternativeViewPointCap().SetViewPoint(image);  
53.   
54.     // ============================  
55.     // IplImage構造体にデータを格納  
56.     // ============================  
57.     iplimage->imageData = (char *)imageMD.WritableData();  
58.     ipldepth->imageData = (char *)depthMD.WritableData();  
59.   
60.     // 画像を反転  
61.     cvFlip(iplimage, iplimage, 1);  
62.     cvFlip(ipldepth, ipldepth, 1);  
63.   
64.     glClear(GL_COLOR_BUFFER_BIT);  
65.     glPointSize(1);  
66.     glBegin(GL_POINTS);  
67.   
68.     // 点の色は白  
69.     glColor3d(1.0, 1.0, 1.0);  
70.   
71.     // 点を打っていく  
72.     for(int y = 0; y < IMAGE_HEIGHT; y++) {  
73.         // ピクセルデータへのポインタ  
74.         short* pDepthImgData = (short*)(  
75.             ipldepth->imageData + y * ipldepth->widthStep  
76.         );  
77.           
78.         /* 点群にRGB画像のテクスチャを貼りたいとき用 
79.           unsigned char* pCameraImgData = (unsigned char*)( 
80.               iplimage->imageData + y * iplimage->widthStep 
81.           ); 
82.         */  
83.   
84.         for(int x = 0; x < IMAGE_WIDTH; x++) {  
85.             // 画素(x, y)に対応する深度データ取得  
86.             int d =   
87.                 (int)pDepthImgData[ipldepth->nChannels * x];  
88.             if (d == 0) continue;  
89.   
90.             // 3次元位置の推定  
91.             double X = (x - cx)/f;  
92.             double Y = (y - cy)/f;  
93.             double Z =          1;  
94.   
95.             // 3次元点を求める  
96.             X *= (double)d;  
97.             Y *= (double)d;  
98.             Z *= (double)d;  
99.   
100.             /* ↓これを書くと、点に画像データの色がつく 
101.               double R =  
102.                   pCameraImgData 
103.                       [iplimage->nChannels*x+0]/255.0; 
104.               double G = 
105.                   pCameraImgData 
106.                       [iplimage->nChannels*x+1]/255.0; 
107.               double B =  
108.                   pCameraImgData 
109.                       [iplimage->nChannels*x+2]/255.0; 
110.               glColor3d(R, G, B); 
111.             */  
112.   
113.             // 座標を指定して点をプロット  
114.             glVertex3d(X, Y, Z);  
115.         }  
116.     }  
117.     glEnd();  
118.     glFlush();  
119. }  
120.   
121. void resize(int w, int h) {  
122.     glViewport(0, 0, w, h);  
123.     glMatrixMode(GL_PROJECTION);  
124.     glLoadIdentity();  
125.     gluPerspective(45.0, (double)w / h,   
126.                     1.0, INT_MAX);  
127.   
128.     // 俯瞰視点（てきとうです）  
129.     gluLookAt(  
130.         0, -5000, 1950,   
131.         0,     0, 2000,   
132.         0,     0,    1  
133.     );  
134. }  
135.   
136. void init() {  
137.     glClearColor(0.0, 0.0, 0.0, 1.0);  
138. }  
139.   
140. int main(int argc, char** argv) {  
141.   
142.     // OpenNI関連のいろいろ  
143.     context.InitFromXmlFile(SAMPLE_XML_PATH);  
144.     context.FindExistingNode(XN_NODE_TYPE_DEPTH,depth);   
145.     context.FindExistingNode(XN_NODE_TYPE_IMAGE,image);  
146.       
147.     // OpenCV関連のいろいろ  
148.     iplimage = cvCreateImage(  
149.         cvSize(IMAGE_WIDTH,IMAGE_HEIGHT),IPL_DEPTH_8U, 3  
150.     );  
151.     ipldepth = cvCreateImage(  
152.         cvSize(IMAGE_WIDTH,IMAGE_HEIGHT),IPL_DEPTH_16S,1  
153.     );  
154.   
155.     // OpenGL関連のいろいろ  
156.     glutInitWindowSize(640, 480);  
157.     glutInit(&argc, argv);  
158.     glutInitDisplayMode(GLUT_RGBA);  
159.     glutCreateWindow("T");  
160.     glutIdleFunc(idle);  
161.     glutDisplayFunc(display);  
162.     glutReshapeFunc(resize);  
163.     init();  
164.     glutMainLoop();  
165.     return 0;  
166. }  

#include<cv.h>
#include<highgui.h>
#include<XnCppWrapper.h>

#include<GL/glut.h>
#include<limits.h>

#pragma comment(lib,"cv200.lib")
#pragma comment(lib,"cxcore200.lib")
#pragma comment(lib,"highgui200.lib")
#pragma comment(lib,"C:/Program files/OpenNI/Lib/openNI.lib")

#pragma comment(lib, "glut32.lib")

const int IMAGE_WIDTH  = 640;
const int IMAGE_HEIGHT = 480;

const char* SAMPLE_XML_PATH = 
    "C:/Program Files/OpenNI/Data/SamplesConfig.xml";

// グローバル変数。使い過ぎよくない。
xn::Context context;
xn::EnumerationErrors errors;
xn::DepthGenerator depth;  
xn::ImageGenerator image;
xn::DepthMetaData depthMD;
xn::ImageMetaData imageMD;

cv::Ptr<IplImage> iplimage, ipldepth;

// Kinectカメラの内部パラメータ
const double f  = 526.37013657;
const double cx = 313.68782938;
const double cy = 259.01834898;


// ==================
// 暇になったら再描画
// ==================
void idle() {
    glutPostRedisplay();
}

void display() {
    context.WaitAnyUpdateAll();

    // 深度・画像メタデータ取得
    depth.GetMetaData(depthMD);
    image.GetMetaData(imageMD);
        
    // 深度データの座標ををカメラに合わせる
    depth.GetAlternativeViewPointCap().SetViewPoint(image);

    // ============================
    // IplImage構造体にデータを格納
    // ============================
    iplimage->imageData = (char *)imageMD.WritableData();
    ipldepth->imageData = (char *)depthMD.WritableData();

    // 画像を反転
    cvFlip(iplimage, iplimage, 1);
    cvFlip(ipldepth, ipldepth, 1);

    glClear(GL_COLOR_BUFFER_BIT);
    glPointSize(1);
    glBegin(GL_POINTS);

    // 点の色は白
    glColor3d(1.0, 1.0, 1.0);

    // 点を打っていく
    for(int y = 0; y < IMAGE_HEIGHT; y++) {
        // ピクセルデータへのポインタ
        short* pDepthImgData = (short*)(
            ipldepth->imageData + y * ipldepth->widthStep
        );
        
        /* 点群にRGB画像のテクスチャを貼りたいとき用
          unsigned char* pCameraImgData = (unsigned char*)(
              iplimage->imageData + y * iplimage->widthStep
          );
        */

        for(int x = 0; x < IMAGE_WIDTH; x++) {
            // 画素(x, y)に対応する深度データ取得
            int d = 
                (int)pDepthImgData[ipldepth->nChannels * x];
            if (d == 0) continue;

            // 3次元位置の推定
            double X = (x - cx)/f;
            double Y = (y - cy)/f;
            double Z =          1;

            // 3次元点を求める
            X *= (double)d;
            Y *= (double)d;
            Z *= (double)d;

            /* ↓これを書くと、点に画像データの色がつく
              double R = 
                  pCameraImgData
                      [iplimage->nChannels*x+0]/255.0;
              double G =
                  pCameraImgData
                      [iplimage->nChannels*x+1]/255.0;
              double B = 
                  pCameraImgData
                      [iplimage->nChannels*x+2]/255.0;
              glColor3d(R, G, B);
            */

            // 座標を指定して点をプロット
            glVertex3d(X, Y, Z);
        }
    }
    glEnd();
    glFlush();
}

void resize(int w, int h) {
    glViewport(0, 0, w, h);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(45.0, (double)w / h, 
                    1.0, INT_MAX);

    // 俯瞰視点（てきとうです）
    gluLookAt(
        0, -5000, 1950, 
        0,     0, 2000, 
        0,     0,    1
    );
}

void init() {
    glClearColor(0.0, 0.0, 0.0, 1.0);
}

int main(int argc, char** argv) {

    // OpenNI関連のいろいろ
    context.InitFromXmlFile(SAMPLE_XML_PATH);
    context.FindExistingNode(XN_NODE_TYPE_DEPTH,depth); 
    context.FindExistingNode(XN_NODE_TYPE_IMAGE,image);
    
    // OpenCV関連のいろいろ
    iplimage = cvCreateImage(
        cvSize(IMAGE_WIDTH,IMAGE_HEIGHT),IPL_DEPTH_8U, 3
    );
    ipldepth = cvCreateImage(
        cvSize(IMAGE_WIDTH,IMAGE_HEIGHT),IPL_DEPTH_16S,1
    );

    // OpenGL関連のいろいろ
    glutInitWindowSize(640, 480);
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_RGBA);
    glutCreateWindow("T");
    glutIdleFunc(idle);
    glutDisplayFunc(display);
    glutReshapeFunc(resize);
    init();
    glutMainLoop();
    return 0;
}

ふぅ…。

もはやこのシリーズは自分の中で備忘録的な位置づけになってしまったので、ソースコードが解りにくいのは仕様。あしからず。