Kinect OpenNIによる3次元ポイントクラウド - 3次元座標取得

Kinect Hackといえば点で部屋を3次元復元する動画が有名ですよね。


誰もが最初にしてみたいと思うはず！たぶん！


ということでOpenNIを使ってKinectセンサで取得した奥行き情報から3次元位置を求める方法を紹介します。

これも実はOpenNIを使えば簡単にできます。



この関数でできます。
DepthGenerator.ConvertProjectiveToRealWorld(座標数,奥行き情報の配列,現実座標の配列);


ということでサンプルプログラム

#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
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_objdetect220d.lib")    // opencv_objdetect
#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
    #pragma comment(lib,"C:\\OpenCV2.2\\lib\\opencv_objdetect220.lib")    // opencv_objdetect
#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;

//マクロ定義
#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;

void retrievePointCloudMap(Mat &depth,Mat &pointCloud_XYZ);

int main()
{
    Context context;
    EnumerationErrors errors;

    context.InitFromXmlFile(SAMPLE_XML_PATH); 
    context.FindExistingNode(XN_NODE_TYPE_DEPTH, depthGenerator); 
    context.FindExistingNode(XN_NODE_TYPE_IMAGE, imageGenerator);

    Mat image(480,640,CV_8UC3);
    Mat depth(480,640,CV_16UC1);  
    //ポイントクラウドの座標
    Mat pointCloud_XYZ(480,640,CV_32FC3,cv::Scalar::all(0));

    int key = 0;
    bool isWarp=false;
    while (key!='q')
    {
        //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);
             
        //convert color space RGB2BGR
        cvtColor(image,image,CV_RGB2BGR);     
     
        imshow("image",image);
        imshow("depth",depth);
        key = waitKey(33);
    }
    context.Shutdown();
    return 0;
}
//3次元ポイントクラウドのための座標変換
void retrievePointCloudMap(Mat &depth,Mat &pointCloud_XYZ){
    static XnPoint3D proj[KINECT_DEPTH_HEGIHT * KINECT_DEPTH_WIDTH] = {0};
    int SEQ = 0;    //配列番号
    for(int y = 0; y < KINECT_DEPTH_HEGIHT; y++ ){
        for(int x = 0; x < KINECT_DEPTH_WIDTH; x++ ){  
            proj[SEQ].X = (XnFloat)x;
            proj[SEQ].Y = (XnFloat)y;
            proj[SEQ].Z = ((unsigned short*)(depth.data + depth.step.p[0]*y))[x] * 0.001f; // from mm to meters
            SEQ++;
        }
    }
    //現実座標に変換    depthGenerator.ConvertProjectiveToRealWorld(KINECT_DEPTH_HEGIHT*KINECT_DEPTH_WIDTH, proj, (XnPoint3D*)pointCloud_XYZ.data);
}   

今回
void retrievePointCloudMap(Mat &depth,Mat &pointCloud_XYZ);

という関数を作りました。

これは引数に、深度画像と変換した現実座標を格納するMatクラスを渡しています。

現実座標を格納するMatクラスは
Mat pointCloud_XYZ(480,640,CV_32FC3,cv::Scalar::all(0));

と宣言しています。

CV_32FC3で32 ビット 3チャンネル（x,y,z float型）を意味します。


なので、このプログラムでは深度データ取得後

retrievePointCloudMap(depth,pointCloud_XYZ);
を呼ぶことにより。

depthからの情報でpointCloud_XYZに変換された3次元座標の情報を格納することができます。



3次元座標にアクセスするにはOpenCV2のcv::Matから画素を取り出すで紹介した方法で取り出します。

for(int y = 0 ; y < pointCloud_XYZ.rows; y++){
    for(int x = 0 ; x < pointCloud_XYZ.cols; x++){
        Point3f &point = pointCloud_XYZ.at<Point3f>(y,x);
        printf("%f %f %f\n",point.x,point.y,point.z);  //座標を表示
    }
}

または

for(int y = 0 ; y < pointCloud_XYZ.rows; y++){
    for(int x = 0 ; x < pointCloud_XYZ.cols; x++){
        Point3f &point = ((Point3f*)(data + step.p[0]*ｙ))[ｘ];
        printf("%f %f %f\n",point.x,point.y,point.z);  //座標を表示
    }
}

次回は取り出した3次元位置座標をOpenGLで3D描画する方法を紹介しようかと思います。

ポイントクラウドというやつですね。
次回：Kinect OpenNIによる3次元ポイントクラウド - 3次元描画