ROSによるカメラ外部パラメータ(ハンドアイ)キャリブレーション

カメラの内部パラメータキャリブレーション(歪補正)の日本語記事がいっぱいあるのに、カメラの外部パラメータのキャリブレーションの日本語の記事があまりない。自分で調べたものをここに記録する。
カメラの外部パラメータは二つの意味合いを持つ：

1. カメラがロボットに取り付けられている。カメラの取り付けたリンク座標系(エンドエフェクタ座標系の場合が多い)とカメラ座標系との相対位置姿勢。この場合のカメラをハンドアイとも言う。
2. カメラが環境に固定されている。ロボット座標系とカメラの座標系との相対位置姿勢。

この記事は1.のハンドアイキャリブレーションをnextage openシミュレーションで体験し、最後に実機への適用仕方を説明する(同じパッケージで2.のキャリブレーションも可能)

本記事の依存パッケージ

私の環境はubuntu16.04で、

• ros-kinetic-vision-visp
• https://github.com/IFL-CAMP/easy_handeye
• https://github.com/marcoesposito1988/easy_handeye_demo
• https://github.com/hsnuhayato/rtmros_nextage/tree/handeye_calib_demo
• moveitとrqtとか標準のrosパッケージ

ここのrtmros_nextageパッケージはオリジナルパッケージにハンドアイキャリブレーションのノードを起動するスクリプトを追加したものである。
githubのパッケージをローカルのcatkinワークスペースにcloneし、catkin_makeを行う。

ハンドアイキャリブレーションのシミュレーションを行う

以下にnextageの右エンドエフェクタRARM_JOINT5_Link座標系からみた仮想ハンドカメラVIRTUAL_CAMERA_HAND_R座標系の位置姿勢をキャリブレーションする。

1. ２つのターミナルから、それぞれ以下のroslaunchを起動：
   • $ rtmlaunch nextagea_ros_bridge nextagea_ros_bridge_simulation.launch
   • $ roslaunch nextage_moveit_config moveit_handeye_calibration.launch handeye_calibration:=true right_handeye:=true(左ハンドアイの場合はfalse)
2. (オプション)rvizのaddボタン押して、tfを表示させる。座標系の相対位置は以下の図のように表示する。
   
3. moveitでキャリブレーション対象のハンドを移動させ(plan and execute)、 rqt_easy_handeye.perspective 画面の"Take Sample"を押す。この手順を4回以上実行する(10回くらいは良いでしょう)。
4. rqt_easy_handeye.perspective 画面のComputeを押す。するとキャリブレーション結果はResult entryで表示する。
   

シミュレーションの説明

moveit_handeye_calibration.launchがmoveit以外に以下のものを起動する：

• easy_handeye (キャリブレーション計算)のノード
• ハンドアイからみた仮想のマーカーの位置姿勢を計算するスクリプト

なお、このlaunchファイルに以下の引数を定義している:

• robot_base_frame ロボット座標系名
• tracking_marker_frame マーカー座標系名
• calibration_transformation エンドエフェクタからみたハンドアイ座標系の位置姿勢。前節のキャリブレーション結果はこれに近い値として計算される。
• arbitrary_transformation 絶対座標系からみた仮想マーカーの位置姿勢
• robot_effector_frame エンドエフェクタ座標系名
• tracking_base_frame ハンドアイ座標系名

launchファイルで起動したtracking_simulator.pyが以上の値を用いて、ハンドアイからみた仮想のマーカーの位置姿勢を繰り返し計算し、ノイズをつけてからtfにbroadcastしている。
"Take Sample"ボタン押すとそのときのエンドエフェクタとカメラからみたマーカー位置姿勢がペアで保存され、後にキャリブレーション計算のときに参照される。

実機への適用

ここで実機への適用方法を記録する。
以下に easy_handeye_demo/scripts/simulator.py のwhileの部分を抜粋する：

while not rospy.is_shutdown():

    if not tfBuffer.can_transform(tracking_marker_frame, tracking_base_frame+'_dummy', rospy.Time(0)):
        rospy.loginfo('Waiting for tf tree to be connected...')
        rospy.sleep(1)
        continue

    # measure tracking transform
    measured_tracking_transformation_msg_stmpd = tfBuffer.lookup_transform(tracking_marker_frame, tracking_base_frame+'_dummy', rospy.Time(0))

    # add noise
    t = measured_tracking_transformation_msg_stmpd.transform.translation
    r = measured_tracking_transformation_msg_stmpd.transform.rotation
    fuzzy_translation, fuzzy_quaternion = fuzzy_transformation(
        np.array((t.x, t.y, t.z)),
        np.array((r.x, r.y, r.z, r.w)))

    # publish tracking transform
    tracking_transform_msg_stmpd.header.stamp = rospy.Time.now() - rospy.Duration(0.1)
    tracking_transform_msg_stmpd.transform.translation = Vector3(*fuzzy_translation)
    tracking_transform_msg_stmpd.transform.rotation = Quaternion(*fuzzy_quaternion)
    tfBroadcaster.sendTransform(tracking_transform_msg_stmpd)

    rate.sleep()

ここの tracking_transform_msg_stmpd.transform.translation と tracking_transform_msg_stmpd.transform.rotation を実ハンドアイからみた位置と姿勢に置き換えれば同じ手順で実機のハンドアイのキャリブレーションすることができるはず。事前にカメラの歪補正と何かしらのマーカー(ARとか)が必要でしょう。キャリブレーション向いているおすすめのマーカーとライブラリーあればぜひコメントで教えてください。

まとめ

• ハンドアイキャリブレーションをシミュレーションで体験した。
• このパッケージの名前通り本当に超easy。