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

【ミッション： Kinectから奥行き情報を推定しよう】

前回、カメラ画像に位置を合わせた深度データを取ってくる事ができたので、今回はそれを 3 次元的にマッピングしてみようと思った。

これは、椅子と机の一部の奥行きデータを3次元的にプロットした結果である。

【射影変換の原理】

以下のような、カメラの光学中心を原点とする 3 次元 X-Y-Z 座標空間を、カメラ座標空間という。ここで便宜上、Z 軸はカメラ光軸に一致し、X 軸および Y 軸は、画像（投影面）の水平・垂直方向に対して平行な軸であるとする（※ 定義としてはかなり不十分なんだけど、厳密な説明はめんどいのでご容赦）。

カメラ座標空間における被写体の 3 次元点 X = (X, Y, Z)^t を、投影面上の 2 次元座標 x = (x, y)^t に射影する変換を射影変換と呼び、カメラの内部パラメータ行列

を用いて以下のように行う事ができる（ただし f はカメラ焦点距離、(c_x, c_y) は画像中心座標である）。

すなわち、

である。

仮に Z = f とおいて、上式を X, Y, Z について解くと、3次元点を貫くベクトルの各要素は X = x - c_x, Y = y - c_y, Z = f と求められる。

さらに、画素 (x, y) に対応する奥行き d が判れば、2 次元画像上の任意の点 (x, y) の3次元位置は以下のように表す事ができる。

この計算を、画像中の全画素に対して施せば、理論上、被写体の3次元化が可能である（※ 実は、ここで示した内部パラメータ行列はかなり簡略的な形である。精度はご容赦）。

だが、そのためには、Kinect のカメラの焦点距離 f 、および画像中心座標 (c_x, c_y) が既知でなくてはならない。

自力でそれらを推定するのはそれなりに骨が折れるので、Web 上を適当に探しまわったらこの辺のページになんか思わせぶりな数字が出てきたので、今回はそれをそのまま用いる事にした（本当は製品ごとに個体差がある）。

• f = 526.37013657
• c_x = 313.68782938
• c_y = 259.01834898

で、ひとまずこれを適当に実装したものが以下のソースコード。今回は 3 次元レンダリングという事で、OpenGL （と GLUT）を使用している。適当すぎて視点移動とかができない。

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. // まぁ適当です  
33. const double f  = 526.37013657;  
34. const double cx = 313.68782938;  
35. const double cy = 259.01834898;  
36.   
37. // ==================  
38. // 暇になったら再描画  
39. // ==================  
40. void idle() {  
41.     glutPostRedisplay();  
42. }  
43.   
44. // =======================================  
45. // 深度データを3次元的にマッピングして表示  
46. // =======================================  
47. void display() {  
48.   
49.     context.WaitAnyUpdateAll();  
50.   
51.     // 深度・画像メタデータ取得  
52.     depth.GetMetaData(depthMD);  
53.     image.GetMetaData(imageMD);  
54.           
55.     // 深度データの座標ををカメラに合わせる  
56.     depth.GetAlternativeViewPointCap().SetViewPoint(image);  
57.   
58.     // ============================  
59.     // IplImage構造体にデータを格納  
60.     // ============================  
61.     iplimage->imageData = (char *)imageMD.WritableData();  
62.     ipldepth->imageData = (char *)depthMD.WritableData();  
63.   
64.     glClear(GL_COLOR_BUFFER_BIT);  
65.     glPointSize(1);  
66.     glBegin(GL_POINTS);  
67.   
68.     // 点を打っていく  
69.     for(int y = 0; y < IMAGE_HEIGHT; y++) {  
70.         // ipldepthのピクセルデータへのポインタ  
71.         short* pDepthImgData = (short*)(  
72.             ipldepth->imageData + y * ipldepth->widthStep  
73.         );  
74.   
75.         unsigned char* pCameraImgData = (unsigned char*)(  
76.             iplimage->imageData + y * iplimage->widthStep  
77.         );  
78.               
79.         for(int x = 0; x < IMAGE_WIDTH; x++) {  
80.             // 画素(x, y)に対応する深度データ取得  
81.             int d =   
82.                 (int)pDepthImgData[ipldepth->nChannels * x];  
83.             if (d == 0) continue;  
84.             d += (-SHRT_MAX / 2);  
85.               
86.             // 3次元位置の推定  
87.             double X = x - cx;  
88.             double Y = y - cy;  
89.             double Z = f;  
90.   
91.             double R =   
92.                 pCameraImgData  
93.                     [iplimage->nChannels*x+0]/255.0;  
94.             double G =  
95.                 pCameraImgData  
96.                     [iplimage->nChannels*x+1]/255.0;  
97.             double B =   
98.                 pCameraImgData  
99.                     [iplimage->nChannels*x+2]/255.0;  
100.   
101.             // 3次元点を求める  
102.             X *= d;  Y *= d;  Z *= d;  
103.               
104.             // 焦点距離で割ってスケーリング  
105.             X /= f;  Y /= f;  Z /= f;  
106.               
107.   
108.             // ↓こう書くと、点に画像データの色がつく  
109.             //glColor3d(R, G, B);  
110.               
111.             // ↓こう書くと、白い点がプロットされる  
112.             glColor3d(1.0, 1.0, 1.0);  
113.   
114.             // 座標を指定して点をプロット  
115.             glVertex3d(X, Y, Z);  
116.         }  
117.     }  
118.     glEnd();  
119.   
120.     glFlush();  
121. }  
122.   
123. void resize(int w, int h) {  
124.     glViewport(0, 0, w, h);  
125.     glMatrixMode(GL_PROJECTION);  
126.     glLoadIdentity();  
127.     gluPerspective(45.0, (double)w / h,   
128.                     1.0, INT_MAX);  
129.       
130.     // 適当に視点を設定  
131.     gluLookAt(  
132.          SHRT_MAX/4,  SHRT_MAX/4,  SHRT_MAX/8,   
133.         -SHRT_MAX/4, -SHRT_MAX/4,   -SHRT_MAX,   
134.                   0,           1,           0  
135.     );  
136.   
137. }  
138.   
139. void init() {  
140.     glClearColor(0.0, 0.0, 0.0, 1.0);  
141. }  
142.   
143. int main(int argc, char** argv) {  
144.   
145.     // OpenNI関連のいろいろ  
146.     context.InitFromXmlFile(SAMPLE_XML_PATH);  
147.     context.FindExistingNode(XN_NODE_TYPE_DEPTH,depth);   
148.     context.FindExistingNode(XN_NODE_TYPE_IMAGE,image);  
149.       
150.     // OpenCV関連のいろいろ  
151.     iplimage = cvCreateImage(  
152.         cvSize(IMAGE_WIDTH,IMAGE_HEIGHT),IPL_DEPTH_8U,3  
153.     );  
154.     ipldepth = cvCreateImage(  
155.         cvSize(IMAGE_WIDTH,IMAGE_HEIGHT),IPL_DEPTH_16S,1  
156.     );  
157.   
158.     // OpenGL関連のいろいろ  
159.     glutInitWindowSize(640, 480);  
160.     glutInit(&argc, argv);  
161.     glutInitDisplayMode(GLUT_RGBA);  
162.     glutCreateWindow("T");  
163.     glutIdleFunc(idle);  
164.     glutDisplayFunc(display);  
165.     glutReshapeFunc(resize);  
166.     init();  
167.     glutMainLoop();  
168.     return 0;  
169. }  

#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();
}

// =======================================
// 深度データを3次元的にマッピングして表示
// =======================================
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();

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

    // 点を打っていく
    for(int y = 0; y < IMAGE_HEIGHT; y++) {
        // ipldepthのピクセルデータへのポインタ
        short* pDepthImgData = (short*)(
            ipldepth->imageData + y * ipldepth->widthStep
        );

        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;
            d += (-SHRT_MAX / 2);
            
            // 3次元位置の推定
            double X = x - cx;
            double Y = y - cy;
            double Z = f;

            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;

            // 3次元点を求める
            X *= d;  Y *= d;  Z *= d;
            
            // 焦点距離で割ってスケーリング
            X /= f;  Y /= f;  Z /= f;
            

            // ↓こう書くと、点に画像データの色がつく
            //glColor3d(R, G, B);
            
            // ↓こう書くと、白い点がプロットされる
            glColor3d(1.0, 1.0, 1.0);

            // 座標を指定して点をプロット
            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(
         SHRT_MAX/4,  SHRT_MAX/4,  SHRT_MAX/8, 
        -SHRT_MAX/4, -SHRT_MAX/4,   -SHRT_MAX, 
                  0,           1,           0
    );

}

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;
}