KinectFusionの出力結果を読み込んで座標系を合わせて表示(オフライン)

KinectFusionColorBasisで得られるPointCloudは、キネクトのローカル座標系で表現されています。これをロボットのカメラの座標系に一致させて表示させていきます。

カメラの座標系の求め方は以前やっていますので、あとはキネクトデータの座標系をロボットの座標系にmKinectを掛けて一致させるだけです。

ofPushMatrix();

ofMatrix4x4 mKinect(0,-1,0,0, 0,0,-1,0, 1,0,0,0, 0,0,0,1);

ofMatrix4x4 mTilt = model.getMeshHelper(4).matrix; // tilt

ofMatrix4x4 mLoffset, mLeye;

mLoffset.setTranslation(0.085, 0.07, 0.08);

mLeye = mKinect * mLoffset * mTilt * model.getModelMatrix();

ofMultMatrix(mLeye);

point_cloud->draw();

ofPopMatrix();

データが一つなら簡単ですね。この回転行列の意味は、新しい座標系における元の座標系の各軸の単位方向ベクトルを表しています。

mKinect(

0,-1,0,0, // ロボット座標系におけるキネクト座標系のx軸方向ベクトル

0,0,-1,0, // ロボット座標系におけるキネクト座標系のy軸方向ベクトル

1,0,0,0, // ロボット座標系におけるキネクト座標系のz軸方向ベクトル

0,0,0,1

);

首を振ったり画面中の物体が移動したりしたとき、複数の画像を扱えるようにして世界を広く時系列に扱えるようにしたいですね。どのように管理するかが問題です。