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

【ミッション： OpenCVでNiSimpleViewerもどきを作ろう】

前回、Kinect の深度データと RGB 画像を OpenCV の IplImage で取得する実験をしてみた。

この調子で、OpenNI Tips に載っている『デプスと画像を重ね合わせするデモのサンプルコード』を試してみよう。

…って、あれ？ ずれていてうまく重ならない。なにこれダメじゃん。

どうやら、深度センサとカメラの位置が微妙にずれているため、単なるオーバレイ表示にしただけではダメらしい。

---

ところが、OpenNI に付属の NiSimpleViewer というサンプルでは、見事に深度データと画像がぴたっと重なった結果が得られている。

幸い、NiSimpleViewer のソースコードも同梱されていたので、深追いする事ができそうだ。

このサンプルは OpenGL 向けだが、解読してみる価値はある。

複雑な射影幾何の知識が必要かと思いきや、調べてみたところ、次の関数を実行するだけで深度データの座標をカメラ画像座標に一致させる事ができた。

// 深度データの座標ををカメラに合わせる
depth.GetAlternativeViewPointCap().SetViewPoint(image);

（depth は xn::DepthGenerator のインスタンス、image は xn::ImageGenerator のインスタンス）。

---

せっかくなので、お勉強も兼ねて、OpenCV でカメラ画像と深度を得るプログラムをぼくなりに作ってみたよ。

ただし、『OpenNI Tips』のように、深度データの抜けを補うような小細工はしていない（ちなみにぼくの環境は VC++ 2008 と OpenCV 2.0 だよ。特に OpenCV は、バージョンによってインクルードするヘッダファイルやリンクするライブラリのファイル名が微妙に違う場合があるので注意してね）。

#include<cv.h>
#include<highgui.h>
#include<XnCppWrapper.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")

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

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

int main() {
    xn::Context context;
    xn::EnumerationErrors errors;

    context.InitFromXmlFile(SAMPLE_XML_PATH);
  
    xn::DepthGenerator depth;  
    context.FindExistingNode(XN_NODE_TYPE_DEPTH, depth); 

    xn::ImageGenerator image;
    context.FindExistingNode(XN_NODE_TYPE_IMAGE, image);

    xn::DepthMetaData depthMD;
    xn::ImageMetaData imageMD;

    int key = 0;

    // 大人の事情で、IplImageで管理することに。
    cv::Ptr<IplImage> iplimage, ipldepth, overlay;

    iplimage = cvCreateImage(cvSize(IMAGE_WIDTH,IMAGE_HEIGHT),
        IPL_DEPTH_8U, 3);

    ipldepth = cvCreateImage(cvSize(IMAGE_WIDTH,IMAGE_HEIGHT),
        IPL_DEPTH_8U, 1);
   
    overlay = cvCreateImage(cvSize(IMAGE_WIDTH,IMAGE_HEIGHT),
        IPL_DEPTH_8U, 3);

    const int BIT = 12; // 深度センサの有効ビット範囲

    while (key!='q') {
        context.WaitAnyUpdateAll();

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

        // カメラデータはそのまま取得
        iplimage->imageData = (char *)imageMD.WritableData();
        cvCvtColor(iplimage, iplimage, CV_BGR2RGB);

        // オーバレイ用の画像にコピー
        cvCopy(iplimage, overlay);

        // Kinectの深度データは符号つき16ビット整数
        // ⇒これをモノクロ256階調に変換して格納する
        // 　※ RGBカメラの階調に合わせている
        for(int y = 0; y < IMAGE_HEIGHT; ++y) {

            // ipldepthのピクセルデータへのポインタ
            unsigned char* pDepthImgData = (unsigned char*)(
                ipldepth->imageData + y * ipldepth->widthStep
            );

            // Kinectの深度データのポインタ
            short* pKinectDepth = (short*)(
                depthMD.WritableData() + y * ipldepth->widthStep
            );

            // iplimageのピクセルデータへのポインタ
            // ※ オーバレイ表示が不要の場合は要らない
            unsigned char* pCameraImgData = (unsigned char*)(
                overlay->imageData + y * iplimage->widthStep
            );
            
            for(int x = 0; x < IMAGE_WIDTH; ++x) {
                // Kinectの深度データを取得・スケーリング
                int depthValue = (int)(
                    255.0 * pKinectDepth[x] / ((0x1 << BIT) - 1)
                );

                // 深度データの設定（指定した範囲を超えた場合は無視）
                pDepthImgData[ipldepth->nChannels * x] = 
                    depthValue > 255 ? 
                        (unsigned char)0 : (unsigned char)-depthValue;

                // オーバレイ表示
                // もし深度が無効値（0）じゃなかったら…
                if(pDepthImgData[ipldepth->nChannels * x] != 0) {
                    pCameraImgData[iplimage->nChannels * x + 0] = 0;  // B

                    pCameraImgData[iplimage->nChannels * x + 1] =     // G
                        pCameraImgData[iplimage->nChannels * x + 2] = // R
                            pDepthImgData[ipldepth->nChannels * x];   // D
                }
            }
        }

        // iplImageはcvShowImageで表示
        cvShowImage("depth", ipldepth);
        cvShowImage("image", iplimage);
        cvShowImage("overlay", overlay);

        key = cv::waitKey(33);
    }

    context.Shutdown();
    return 0;
}

上記ソースコード中で IplImage の各画素値へのアクセスにポインタを用いているのは、それが最も効率のよい方法だからである。

Gary Bradski, adrian Kaehler 著、松田晃一訳『詳解 OpenCV』の第3章には、画像データにアクセスするための以下のようなコード例が載っており、上記のプログラムはそれに準じた形となっている。

// HSV画像のSとVの部分だけを最大化（飽和）させる
void saturate_sv(IplImage* img) {
    for(int y=0; y<img->height; y++) {
        uchar* ptr = (uchar*) (
            img->imageData + y * img->widthStep
        );
        for(int x=0; x<img->width; x++) {
            ptr[3*x+1] = 255;
            ptr[3*x+2] = 255;
        }
    }
}

---

実行結果は以下の通り。

見た目は NiSimpleViewer に近いものになっているが、決定的な違いはレンダリングに OpenGL ではなく OpenCV を使っているという事（ときにはこっちの方が便利な事もあるのだ）。

これでよし！ ぴったり重なった！