cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

ロボットの視界シミュレーション

初稿: 2013/10/13, 最終更新: 

15.ロボットの視線で画面を表示する

しばらくプログラミング優先でロボットっぽい話題から離れていましたので、ロボットに戻りましょう。次は、ロボットのカメラとGUIコントローラをつないでいく方法を検討していきます。

15.1 カメラの座標を調べる

ロボットの視線で画面を表示できると、ロボットに搭載したカメラの映像と重ね合わせて表示できます。何かを操作したりつかんだりしたいとき、そのような視線が得られると便利に使えそうです。実際に行う処理は、現在は適当に置いているカメラの位置と姿勢をロボットモデルのカメラの位置と姿勢に合わせる座標変換です。

カメラはロボットの首のチルトノードから先に配置されています。ロボットの設計情報があればその数値を使えばよいのですが、残念ながら資料を調べてもカメラの座標が分かりませんでしたので、カメラの位置を手探りで調べていきましょう。

HIRONXGUIControler.cppのdraw関数内に以下のテストコードを追加します。

ofMatrix4x4 mRoffset, mReye;

mRoffset.setTranslation(0.085, -0.07, 0.08);

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

mReye = mRoffset * mTilt * model.getModelMatrix();

ofPushMatrix();

ofMultMatrix(mReye);

ofDrawAxis(0.12);

ofPopMatrix();

(0.085, -0.07, 0.08)がチルト関節からの位置オフセットです。単位はメートルです。首のチルト座標とロボットの座標を掛けて、世界座標における座標を計算します。その位置に、少し大きい軸を書いています。*1

f:id:cvl-robot:20131013024020j:plainfig.右目の位置に座標系表示

適当に数字を合わせて調整していきます。まあ、大体こんなものでしょうか。ロボット実機で正確に求めるためには、カメラキャリブレーションが必要になります。同様に、左目の位置は(0.085, 0.07, 0.08)と求めました。

ハンドカメラの位置も同様に調べることができます。ここでは使いませんので、省略します。

15.2 ofEasyCamの位置姿勢を変更する

ロボット側にカメラの位置姿勢を設定する関数を加えます。ロボット毎にカメラの数や位置や種類が違いますので、ハードコーディングで対応してしまいます。HIRONXGUIControler.hに関数の宣言を加えます。

int getCameraPosition(int id, ofEasyCam& cam);

HIRONXGUIControler.cppにgetCameraPositionの実装を記述します。idはカメラ番号です。使用しているカメラによってFovなどが変わってきますので調整します。

int HIRONXGUIControler::getCameraPosition(int id, ofEasyCam& cam)

{

  switch(id){

    case 0: // Normal

      cam.setTarget(model.getPosition());

      cam.lookAt(model.getPosition(),ofVec3f(0,-1,0));

      break;

    case 1: { // Left Eye

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

      ofMatrix4x4 mLoffset, mLeye;

      mLoffset.setTranslation(0.085, 0.07, 0.08);

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

      cam.reset();

      cam.setFov(82.4);/ / 実際のカメラのレンズ仕様

      cam.setPosition(mLeye.getTranslation());

      cam.lookAt(ofVec3f(1,0,0)*mLeye, ofVec3f(0,-1,0));

    }

    break;

    case 2: { // Right Eye

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

      ofMatrix4x4 mRoffset, mReye;

      mRoffset.setTranslation(0.085, -0.07, 0.08);

      mReye = mRoffset * mTilt * model.getModelMatrix();

      cam.reset();

      cam.setFov(82.4); // 実際のカメラのレンズ仕様

      cam.setPosition(mReye.getTranslation());

      cam.lookAt(ofVec3f(1,0,0)*mReye, ofVec3f(0,-1,0));

    }

    break;

    default:

      return -1;

  }

  return 0;

}

 15.3 複数カメラに対応

testApp.h

ofEasyCam cam[3];

int viewpoint;

 testApp.cpp

setup

cam[0].setFov(80);

for(int i=0; i<3; i++) cam[i].setScale(0.001);

cam[0].setPosition( (float)ofGetWidth() * -0.5, (float)ofGetHeight() * -0.5 , 0);

cam[0].setTarget(hiro.model.getPosition() );

cam[0].lookAt(hiro.model.getPosition(),ofVec3f(0,-1,0) );

cam[0].setDistance(1.2);

 

viewpoint = 0;

update

 hiro.getCameraPosition(viewpoint,cam[viewpoint]);

draw

cam[viewpoint].begin();

for(int i=0; i<model.size(); i++){

  model[i]->draw();

}

cam[viewpoint].end();

keyPressed

 void testApp::keyPressed(int key){

  switch(key){

  case '0': viewpoint = 0; break; // Normal View

  case '1': viewpoint = 1; break; // Left Camera View

  case '2': viewpoint = 2; break; // Right Camera View

  default: break;

  }

}

 mouseReleased

void testApp::mouseReleased(int x, int y, int button){

  cam[viewpoint].enableMouseInput();

}

 なんだか大工事になってしまいましたが、とりあえず3視点をキーボードで変更できるようになりました。

f:id:cvl-robot:20131013033522j:plainfig.ロボットの左目の視界

アームを少し持ち上げてから、カメラの視界でパン、チルト、チェストを変更してみます。パン、チルトを変更すると首を振っているように視界が変わります。チェストを変更しても、今見えているアームも同じように動いていますので相対座標は変わっていませんから、変わりません。良さそうですね。

*1:getMeshHelperは名前での呼び出しにまだ対応させていませんので、直接チルト関節を表す4をハードコーディングして与えています。いずれ対応させたいと思います。