視覚フィードバック制御のためのカメラ導入その１

初出：2013/10/25, 最終更新：2013/10/28

２１．KinectFusionColorBasicsのOpenCV対応

次の中期開発目標を、ロボットの視覚を用いたフィードバック制御にします。というのは、HIROのアームは位置決め精度が悪くmoveRelativeなどのコマンドを繰り返し使用すると残差が溜まって元の位置に戻らない、というような問題があることが判ったためです。アームのエンコーダだけでなく、ビジョンを使ってアームの手先位置を決める事で精度向上を目指します。

２１．１　Kinectをロボット頭部に搭載

カメラデバイスは世の中に沢山の種類がありますが、ここでは

Kinect for Windows
2台のCMOSカメラによるステレオビジョン

をロボット頭部に載せて扱っていく予定です。Kinectだけで済ませないのは、赤外光を投射して使っているKinectなどのセンサーでは、太陽光の下では投射パターンがかき消されてしまうために屋外で使えないという制限があるためです。*1

２１．２　Kinectのライセンス

kinect関係のライセンスを確認しておきます。まとめてくださっている方がいましたので、こちらを参考にします。デバイスはXBOX用ではなくWindows用のKinectを用いていて、大学の研究ですので非商用、アカデミックということで、大丈夫そうですね。

商売用に使いたいという方は、ライセンス条項を精査してください。

２１．３　Kinect for Windows SDKとKinectFusionColorBasics-D2Dのインストール

Kinect for Windowsの最新版をダウンロードしてインストールします。こちらを参考にしてください。余裕があれば、OpenNI2.2も入れておいてください。

http://kgxpx834.blog58.fc2.com/blog-entry-35.html

マイクロソフト公式のサンプルプログラムをDirectXではなくOpenCVで扱えるように改造して使っていきますので、まずKinectFusionColorBasics-D2Dのソースコードをインストールします。[スタートメニュー]->[Kinect for Windows SDK v1.8]->[Kinect for Windows Developer Toolkit v1.8.0]を起動して、Samples: c++からKinectFusionColorBasics-D2Dを適当な場所にInstallしてください。

また、OpenCV最新版の開発環境をCMAKEから普通に入れておいてください。

２１．４　KinectFusionのOpenCV対応

撮影した画像に自分で画像処理を加えたい場合、OpenCVの利用が便利です。またWindowsシステムでイベントループの制御をされていると、タイミングの制御が手間ですので自分で管理できるとやり易いです。旧バージョンv17のKinectFusionのOpenCV対応サンプルを公開されている方が居ますのでこれ参考にさせていただいて、v18をOpenCV対応にしていきます。

まずmain関数を取り戻します。プロジェクトのプロマティ、リンカ→システム→コンソールに設定、で、int main(int argc, char* argv)からプログラムが起動するようになります。

openFrameworksの立ち上げと同じように、設定、更新、描画の3段階程度のわかり易い構成になるように整理します。まず初期化です。もともと綺麗に整理された関数がありますので、キネクトの接続の面倒を見てくれるCreateFirstConnected()と、KinectFusionの初期化を行うInitalizeKinectFusion()を順に呼びます。

void CKinectFusionColorBasics_OCV::Initialize()

{

if(FAILED( CreateFirstConnected() )) m_bInitializeError = true;

else if(FAILED( InitializeKinectFusion() )) m_bInitializeError = true;

}

メインループは、OpenCVの標準的な使い方であるwaitKey関数でタイミングを制御します。更新処理は、元々サンプルにあるUpdate関数の引数にOpenCVの画像ポインタを渡せるように改造して使用します。描画はOpenCVのimshow関数です。またGUIが無くなりますので、代わりにキー操作でパラメータを変更できるようにします。メインループは次のような関数になります。

int CKinectFusionColorBasics_OCV::Run()

{

cv::Mat image;

while( 1 ){

Update( image );

cv::imshow( "KinectSample", image );

int key = cv::waitKey( 1 );

if ( key == 'c' ) m_bCaptureColor = !m_bCaptureColor;

else if ( key == 'n' ) {

m_bNearMode = !m_bNearMode;

if (m_pNuiSensor)

m_pNuiSensor->NuiImageStreamSetImageFrameFlags(m_pDepthStreamHandle, m_bNearMode ? NUI_IMAGE_STREAM_FLAG_ENABLE_NEAR_MODE : 0);

}

else if ( key == 'r' ) ResetReconstruction();

else if ( key == 'q' ) break;

}

return 0;

}

Update関数の中で、WaitForSingleObject関数が呼ばれており画像の取得が終了するまでこの中でも少し待ちが入りますから、イベント制御のタイミングはだいぶいい加減になります。ここでは気にしませんが、厳密にフレームレートを管理したいときなどには、少し工夫を入れたほうが良さそうです。

void CKinectFusionColorBasics_OCV::Update( cv::Mat& mat )

{

if (!m_pNuiSensor) return;

if ( WAIT_OBJECT_0 == WaitForSingleObject(m_hNextDepthFrameEvent, 0) )

{

ProcessDepth(mat);

}

}

ProcessDepthの中で何を位置合わせ処理や色合わせ処理が行われています。ただし、残念ながらKinectFusionは全ソースコードが公開されているわけではなくライブラリでの提供なので、ソースコードを最後まで追っていっても ProcessFrame()やIntegrateFrame()やCalculatePointCloud()などの関数名が見えるだけで、中で何が行われているかは分かりません。*2

肝心のOpenCVへの対応はProcessDepthの//Renderセクションの中のこの一行です。

// Make sure we've received valid data

if (ShadedLockedRect.Pitch != 0) {

unsigned char * pBuffer = (unsigned char *)ShadedLockedRect.pBits;

mat = cv::Mat( height, width, CV_8UC4, ShadedLockedRect.pBits );

}

処理の大枠ができてしまえば、あとはGUIに関する処理やWindows方言っぽい宣言を地道に消していくだけです。

f:id:cvl-robot:20131025012934j:plain fig.KinectFusionのOpenCV対応