robo8080のブログ

ロボット製作や電子工作など。

11月29日(月)のつぶやき

2010年11月30日 | 日記
10:10 from ついっぷる/twipple
さて次は、Kinectの距離情報、カメラ画像、ARToolKitのマーカで検出したオブジェクト、この３つを重ね合わせてOpenGLでグリグリ動かす。カメラ画像が少しずれるのは当面目をつぶろう。
22:16 from web
OpenGLを使うのは久しぶりなので、もうほとんど使い方を忘れている。今日はOpenGLの再勉強で終わってしまった。
by robo8080 on Twitter

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

2010年11月28日 | OpenKinect

【座標補正処理実験】
　いろいろな方が報告しているが、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に画像カメラ座標が入っている
  //この後に表示処理等を追加する。

 


11月27日(土)のつぶやき

2010年11月28日 | 日記
09:50 from Twitpic
距離情報に補正かけた時の画像がこれ。これはKinectの距離情報に補正をかけ、その上に対応するカメラ画像ピクセルをマッピングし、さらにARToolKitのマーカで検出したオブジェクトを重ねている。


11:46 from web
Kinectの距離情報とカメラ画像補正、一見うまくいっているように見えたがよく見てみるとまだ微妙にずれている。もう少し調べてみよう。
17:20 from Twitpic
Kinectの距離情報とカメラ画像の補正処理を見直してみたが、特に問題はなさそうだ。まだ微妙にずれる。やはりキャリブレーションパラメータは個別に求める必要があるのだろうか。（個体差が大きい？）だとするとちょっと面倒だな。


by robo8080 on Twitter

11月26日(金)のつぶやき

2010年11月27日 | 日記
09:31 from web
Kinectもう一台ほしくなってきた。でも、もっとパワーのあるPCを手に入れるほうが先かな。
09:48 from web
ARToolKitでKinectとUSBカメラを同時に使う手もあるな。Kinectを固定カメラ、USBカメラを移動カメラにする。同じマーカーを見せてやれば、移動カメラ側もKinectの距離情報を使えるように出来るはずだ。
10:49 from ついっぷる/twipple
Kinectって屋外でも使えるのかな。
12:52 from web
ARToolKitでKinectが使えるように出来たので、今日はいろいろ実験。ARToolKitにいろんな機能が用意されているので、自分で一から作らずに済んで便利。
14:34 from Twitpic
Kinectの距離情報の上に、ARToolKitのマーカで検出したオブジェクトを描画してみた。
いろいろな方が報告しているが、補正しないとそのままでは微妙にずれる。


20:07 from ついっぷる/twipple
ここを参考に補正を入れてみた。ARToolKitのマーカで検出したオブジェクトの位置がずれなくなった。まだ少し挙動がおかしいところがあるが、概ねうまくいったようだ。http://bit.ly/eXANTG
21:42 from web
さぁて、次はOpenGLを使って３Dでグリグリっと動かしてみたいが、今使っている貧弱なノートPCで出来るかな。
by robo8080 on Twitter