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）を使用している。適当すぎて視点移動とかができない。

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