kinectキャリブレーション

ロボットアームに基準を設置し、アームを動かしポイントクラウドを撮像することにより、kinectをキャリブレーションする方法を説明します。

準備

あらかじめ、基準モデルとそのVRMLファイルを作成し、基準モデルをロボットに設置します。

MotionFile、PCL、RobotInterfaceプラグインを使用できる状態にします。

■基準情報の設定
extplugin/graspPlugin/PCL/config.iniファイルを開きます。
[Calibration]セクションを編集します（無ければ追加します）。

 [Calibration]
 target_link_name=LARM_JOINT4 ;基準を設置したリンク名を指定します。
 calib_model_file=./calibleftHrp.wrl ;基準のVRMLファイルパスを記述します。

■モーションの作成
基準を含んだポイントクラウドを複数個撮像するためのモーションを作成します。
extplugin/graspPlugin/MotionFile/data/motion.datを開き編集します。
モーション例としてextplugin/graspPlugin/MotionFile/data/motion.dat.HIROcalibがあるので参考にしてください。
※kinectと基準が近い（50cm以内）ポーズでは基準が撮像できないので注意してください。

■キャリブレーション行列の設定
あらかじめ、おおまかなキャリブレーション行列を設定しておきます。
編集ファイルは固定カメラ使用時は、extplugin/graspPlugin/PCL/calibtools/calibmat.txt
ハンドに取り付けたカメラ使用時は、ロボットのYAMLファイルに記述したパス（ViewPlannerプラグイン参照）のファイルを編集します

■キャリブレーション用データディレクトリのクリア
extplugin/graspPlugin/PCL/calibrationdataディレクトリがある場合は削除します。

使用方法

・キャリブレーション用ポイントクラウドのキャプチャ
1.Choreonoidを起動します。
2.SetRobot,SetCameraで対象ロボットと使用カメラを選択します。
3.MotionFileツールバーのLoadボタンを押し,ロボットのモーションを生成します。
4.InterfaceツールバーのMoveボタンを押し、ロボットを動作させます。
5．PCLツールバーのCalibCapボタン押すと、ポイントクラウドがキャプチャされポイントクラウドとロボットの関節各情報が保存されます。
※CalibCapボタンを押すとき、Choreonoid上のロボットの姿勢が実際のロボットの姿勢と同じであることを確認してください。
6.3-5を繰り返します。

・キャリブレーション
1.PCLツールバーのGenCalibMatを押します。
2.キャリブレーション行列の保存先を指定します。
3.画面上にポイントクラウドが表示、基準モデル（アイテム名：Calib)が追加、ダイアログが表示されます。
　基準モデルをポイントクラウド上の基準位置の近くに移動させ、ダイアログのOKボタンを押します。（ポイントクラウドに基準がなければSkipを押します）
4.基準モデルとポイントクラウドの位置合わせが実行され基準モデルが移動しますので、ポイントクラウドと一致していたらAcceptを押します。
　一致していなければRejectボタンを押すか、基準モデルを移動させ、Retryボタンを押し再度位置合わせを実行します。
5.キャプチャデータの個数分3-4を繰り返します。指定した保存先にキャリブレーション行列が保存されます。

ポイントクラウドの補正

kinectで得られたデータはレンズ歪み等で歪んでいるため補正を行う。
kinectで得た点の座標を(x,y,z)を下記の式を用い座標(x,y,z')へ補正を行う。
　r^2=(x^2+y^2) / z^2
　z_tmp = z * (1+ k1*r^2 + k2*r^4)
　z' = d1 * z_tmp + d2 * z_tmp^2
係数k1,k2,d1,d2はextplugin/graspPlugin/PCL/config.iniファイルに記述する。以下に例を示します。

[Capture]
do_correcton=true
k1=-0.09
k2=0.193
d1=0.95
d2=0.07