KinectのDepthカメラ情報とカメラ画像の座標補正実験

【座標補正処理実験】
　いろいろな方が報告しているが、KinectのDepthカメラ情報とカメラ画像は
そのまま重ね合わせても微妙にずれる。
Depthカメラ情報とカメラ画像間の座標補正処理実験をしてみた。

（１）実験環境
　OS:Windows XP
 使用言語：C++（Visual C++ 2010 Express）
 ベースソフト：OpenKinect　"Kinect-v15-withsource.zip"

（２）補正処理作成
　ここを参考に補正処理を作ってみた。（詳細は下記ソースを参照してください。）

（３）実験結果
　実験結果はこんな感じ。

　補正前：かなりずれている。

　　

　補正後：だいぶ良くはなっているが、まだ微妙にずれる。

　　

　やはりキャリブレーションパラメータは、個別に求める必要があるのだろうか。
（個体差が大きい？）だとするとちょっと面倒だな。

【実験に使用した座標補正処理のソース】
　（１）まず、OpenKenectのソースをここからダウンロードする。
　実験に使用したのは、"Kinect-v15-withsource.zip"。

　（２）"Kinect-Utility.h"に以下の行を追加する。
 void KinectDepthToWorld2(float &x, float &y, float &z);
 void KinectWorldToRGBSpace2(float &x, float &y, float z);
 float Kinect_DepthValueToZ2(unsigned short Depth);
//追記　2010.12.05
 void KinectDepthToRGBWorld(float &x, float &y, float &z);
 void KinectRGBWorldToRGBSpace(float &x, float &y, float z);

　（３）"Kinect-Utility.cpp"に以下を追加する。

//---ここから-----------------------------------------------------------------------------
 //
 //ここを参考に作成した。
 //http://nicolas.burrus.name/index.php/Research/KinectCalibration
 //
 //Color
 double fx_rgb = 5.2921508098293293e+02;
 double fy_rgb = 5.2556393630057437e+02;
 double cx_rgb = 3.2894272028759258e+02;
 double cy_rgb = 2.6748068171871557e+02;
 double k1_rgb = 2.6451622333009589e-01;
 double k2_rgb = -8.3990749424620825e-01;
 double p1_rgb = -1.9922302173693159e-03;
 double p2_rgb = 1.4371995932897616e-03;
 double k3_rgb = 9.1192465078713847e-01;

 //Depth
 double fx_d = 5.9421434211923247e+02;
 double fy_d = 5.9104053696870778e+02;
 double cx_d = 3.3930780975300314e+02;
 double cy_d = 2.4273913761751615e+02;
 double k1_d = -2.6386489753128833e-01;
 double k2_d = 9.9966832163729757e-01;
 double p1_d = -7.6275862143610667e-04;
 double p2_d = 5.0350940090814270e-03;
 double k3_d = -1.3053628089976321e+00;

 //Relative transform between the sensors (in meters)
 double R[3][3] =
 { 9.9984628826577793e-01, 1.2635359098409581e-03, -1.7487233004436643e-02,
  -1.4779096108364480e-03, 9.9992385683542895e-01, -1.2251380107679535e-02,
   1.7470421412464927e-02, 1.2275341476520762e-02,  9.9977202419716948e-01 };
 double T[3] =
 { 1.9985242312092553e-02, -7.4423738761617583e-04,-1.0916736334336222e-02 };

 //////////////////////////////////////////////////////
 //Depthカメラ深度(raw)->Depthカメラ深度（単位ｍ）
 //入力：
 //　Depth:unsigned short :Depthカメラ深度 0～2047(raw)
 //戻り値：　深度(単位ｍ）
 //////////////////////////////////////////////////////
 float Kinect_DepthValueToZ2(unsigned short Depth)
 {
  return 1.0 / (Depth * -0.0030711016 + 3.3309495161);
 };

 //////////////////////////////////////////////////////
 //Depthカメラ(raw)->Depthカメラワールド座標（単位ｍ）
 //入力：
 //　x:参照型 float :DepthカメラX座標 0～639
 //　y:参照型 float :DepthカメラX座標 0～479
 //　z:参照型 float :DepthカメラDepth 0～2047
 //出力：
 //　x:参照型 float :Depthカメラワールド座標X（単位ｍ）
 //　y:参照型 float :Depthカメラワールド座標Y（単位ｍ）
 //　z:参照型 float :Depthカメラワールド座標Z（単位ｍ）
 //////////////////////////////////////////////////////
 void KinectDepthToWorld2(float &x, float &y, float &z)
 {
  //P3D.x = (x_d - cx_d) * depth(x_d,y_d) / fx_d;
  //P3D.y = (y_d - cy_d) * depth(x_d,y_d) / fy_d;
  //P3D.z = depth(x,y);
  float zz = (float)(1.0 / (z * -0.0030711016 + 3.3309495161));
  float xx = (float)((x - cx_d) * zz / fx_d);
  float yy = (float)((y - cy_d) * zz / fy_d);

  x = (float)xx; //xは参照型
  y = (float)yy; //yは参照型
  z = (float)zz; //zは参照型
 };

 //////////////////////////////////////////////////////
 //Depthカメラワールド座標（単位ｍ）->画像カメラ座標(画素）
 //入力：
 //　x:参照型 float :Depthカメラワールド座標X（単位ｍ）
 //　y:参照型 float :Depthカメラワールド座標Y（単位ｍ）
 //　z:       float :Depthカメラワールド座標Z（単位ｍ）
 //出力：
 //　x:参照型 float :画像カメラX座標(画素）
 //　y:参照型 float :画像カメラY座標(画素）
 //////////////////////////////////////////////////////
 void KinectWorldToRGBSpace2(float &x, float &y, float z)
 {
  //P3D' = R.P3D + T
  double xx = R[0][0]*x + R[0][1]*y + R[0][2]*z + T[0];
  double yy = R[1][0]*x + R[1][1]*y + R[1][2]*z + T[1];
  double zz = R[2][0]*x + R[2][1]*y + R[2][2]*z + T[2];

  //P2D_rgb.x = (P3D'.x * fx_rgb / P3D'.z) + cx_rgb;
  //P2D_rgb.y = (P3D'.y * fy_rgb / P3D'.z) + cy_rgb;
  float ox = (float)((xx * fx_rgb / zz) + cx_rgb);
  float oy = (float)((yy * fy_rgb / zz) + cy_rgb);

  x = __min(640,__max(0,ox)); //xは参照型
  y = __min(480,__max(0,oy)); //yは参照型
 };

//追記　2010.12.05

 //////////////////////////////////////////////////////
 //Depthカメラ(raw)->画像カメラワールド座標（単位ｍ）
 //（ARToolKit内は単位がｍｍなので注意）
 //入力：
 //　x:参照型 float :DepthカメラX座標 0～639
 //　y:参照型 float :DepthカメラX座標 0～479
 //　z:参照型 float :DepthカメラDepth 0～2047
 //出力：
 //　x:参照型 float :画像カメラワールド座標X（単位ｍ）
 //　y:参照型 float :画像カメラワールド座標Y（単位ｍ）
 //　z:参照型 float :画像カメラワールド座標Z（単位ｍ）
 //////////////////////////////////////////////////////
  void KinectDepthToRGBWorld(float &x, float &y, float &z)
  {
    //P3D.x = (x_d - cx_d) * depth(x_d,y_d) / fx_d;
    //P3D.y = (y_d - cy_d) * depth(x_d,y_d) / fy_d;
    //P3D.z = depth(x,y);
    double zz = 1.0 / (z * -0.0030711016 + 3.3309495161);
    double xx = (x - cx_d) * zz / fx_d;
    double yy = (y - cy_d) * zz / fy_d;

    //P3D' = R.P3D + T
    x = (float)(R[0][0]*xx + R[0][1]*yy + R[0][2]*zz + T[0]);
    y = (float)(R[1][0]*xx + R[1][1]*yy + R[1][2]*zz + T[1]);
    z = (float)(R[2][0]*xx + R[2][1]*yy + R[2][2]*zz + T[2]);
  };

 //////////////////////////////////////////////////////
 //画像カメラワールド座標（単位ｍ）->カメラ座標(画素）
 //（ARToolKit内は単位がｍｍなので注意）
 //入力：
 //　x:参照型 float :画像カメラワールド座標X（単位ｍ）
 //　y:参照型 float :画像カメラワールド座標Y（単位ｍ）
 //　z:       float :画像カメラワールド座標Z（単位ｍ）
 //出力：
 //　x:参照型 float :画像カメラX座標(画素）
 //　y:参照型 float :画像カメラY座標(画素）
 //////////////////////////////////////////////////////
  void KinectRGBWorldToRGBSpace(float &x, float &y, float z)
  {
    //P2D_rgb.x = (P3D'.x * fx_rgb / P3D'.z) + cx_rgb;
    //P2D_rgb.y = (P3D'.y * fy_rgb / P3D'.z) + cy_rgb;
    //float ox,oy;
    float ox = (float)((x * fx_rgb / z) + cx_rgb);
    float oy = (float)((y * fy_rgb / z) + cy_rgb);

    x = __min(640,__max(0,ox));
    y = __min(480,__max(0,oy));
  };

//---ここまで-----------------------------------------------------------------------------

【使用例】

 Depthカメラ情報から、対応するカメラ画像の座標を求める。

  //Depthカメラ(raw)->ワールド座標（単位ｍ）
  float depth_x = DepthカメラX座標
  float depth_y = DepthカメラY座標
  float depth_z = DepthカメラDepth(raw)　//注意：Kinect_IsDepthValid関数でチェック済みのもの
  Kinect::KinectDepthToWorld2(depth_x,depth_y,depth_z);

  //ここで、depth_x,depth_y,depth_zにワールド座標（単位ｍ）が入っている

  //ワールド座標（単位ｍ）->カメラ座標(画素）
  float camera_x = depth_x;     
  float camera_y = depth_y;     
  Kinect::KinectWorldToRGBSpace2(camera_x,camera_y,depth_z);

  //ここで、camera_x,camera_yに画像カメラ座標が入っている
  //この後に表示処理等を追加する。

 

OpenKinect、v14が動かない。

　OpenKinect、Kinect-v10-withsourceは動いていたが、v14とv15は動かなくなってしまった。コンソール出力を見ると、データの取りこぼしが大量に発生している。ソースを調べてみたらいろいろ機能が追加されているようなので、今使っているPCでは力不足なのかなという気がする。まあ、v10が動くから当面困らないが．．．

　やはり、Kinect-v14-withsourceが動かないのは今使っているノートPCのパワー不足が原因のようだ。OpenGLの表示を無効にしてコンパイル後動かしたら、データの取りこぼしもなく正常に動いた。今使っているPCは、OSがXP、CPUはPentium Mの２GHzだ。

OpenKinectを動かしてみた。

XBOX本体も持っていないのに、Kinectだけゲット。さっそくOpenKinectで動かしてみよう。

　

ここからOpenKinectのソースをダウンロードして動かしてみた。

何のトラブルもなくサクッと動いてしまった。完成度高いな。

ただし、そこそこのCPUパワーは必要なみたいだ。今使っているノートPCでは時々データを取りこぼす。

OpenKinectのサンプルプログラムを動かした結果はこんな感じ。

距離情報を色に変換して表示している。

　

Kinect & OpenKinect 素晴らしい。

従来だと高価なレーザーレンジファインダでも使わないと出来なかったことが、解像度は低いのかもしれないがこんな安価な機器で出来るとは。

いついか移動ロボットに載せてみたい。

OpenKinectをダウンロードしてみた。

（１）　OpenKinect / libfreenectのソースをダウンロードしてみた。　解凍してみるといくつか種類があったが、この中にあるC#のプロジェクトはKinectのモータとLEDを制御するサンプルのようだ。

Cのソースもあったが、コンパイルする環境を作るのが面倒なので、とりあえずC#のソースをVC＃2010Expressでビルドしてみた。一応正常にビルド出来た。

（２）　penkinect.orgでもソースを公開していたのでこちらもダウンロードしてみた。こちらはWindows用にVC++のプロジェクトがあった。

VC++2010Expressでプロジェクトを開くと、プロジェクトを変換するかとメッセージが出たので、そのまま変換してビルドすると正常にビルド出来た。

これはKinectから取り込んだデータをOpenGLで表示するサンプルのようだ。Kinectの実物を持ってないのでどういう動きをするのかはよくわからないが．．．。

ソースをざっと見たところ、距離情報を色分けして表示するプログラムなのかな。

KinectとARToolKitを組み合わせたら面白いことができそうだ。