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に画像カメラ座標が入っている
  //この後に表示処理等を追加する。