ARタグ

下の3つのマークは、それぞれID=7, 187, 548のARタグです。

    

ARタグは、QRコードなどの2次元バーコードを簡単にしたようなものに見えます。

QRコードは、カメラでマークを写して記録された情報を得ることが主な用途となっていますが、ARタグでは、カメラで写してタグの姿勢を得ることがその主目的といってよいでしょう。 ARタグでは黒枠内部のドットパターンは、タグのIDを表しており、これによりタグの識別も可能です。

あらかじめ使用するARタグのサイズが判っていれば、カメラ画像中のタグのサイズをもとに、カメラからARタグまでの距離を計算できます。 また、内部のドットパターンは回転非対称になっており、回転や傾きも含めた姿勢を得ることができます。

一般風景などからロボットの姿勢を推定したり、一般の対象物の姿勢を推定するには画像処理などに高度な技術が必要です。

ARタグを利用すれば周辺環境の影響を受けにくく、また比較的簡単に、カメラ画像から姿勢を得ることができます。

ar_track_alvarパッケージを使う

ROSには、カメラ画像からARタグの姿勢を取得することのできるar_track_alvarパッケージがあり、今回はこれを使います。

ARタグを使う前に、カメラのキャリブレーションをする必要があり、それはcamera_calibrationパッケージを利用して行うことができます。チェックマークを紙に印刷してボードに貼り付け、それをカメラに映しながら上下左右に移動したり回転させたり傾けたりさせます。それによりカメラのキャリブレーションデータを生成します。以前紹介しました参考文献に詳しく手順が書かれていたので、ここでは説明を省略します。ただ、マシンパワーのないPCでキャリブレーションを実行するとかなりの時間がかかります。

ar_track_alverパッケージのインストールは、sudo apt-get install ros-indigo-ar-track-alverで完了です。

ARタグを用意する

ar_track_alverパッケージにARタグを生成するツールが含まれています。

これを利用して、以下の手順でID:0~2の3つのタグを生成します。Enterを数回叩いてDefault値を選択、最後に-1で終了です。

$ rosrun ar_track_alvar createMarker -s 5.0 -p
・・・
Prompt marker placements interactively
  units: 1 cm 0.393701 inches
  marker side: 5 units
  marker id (use -1 to end) [0]: 
  x position (in current units) [0]: 
  y position (in current units) [0]: 
  marker id (use -1 to end) [1]: 
  x position (in current units) [10]: 
  y position (in current units) [0]: 
  marker id (use -1 to end) [2]: 
  x position (in current units) [10]: 
  y position (in current units) [10]: 
  marker id (use -1 to end) [3]: -1  <--- "-1"を入力して終了
Saving: MarkerData_0_1_2.png
Saving: MarkerData_0_1_2.xml
		

これで、実行したディレクトリに"MarkerData_0_1_2.png"ファイルが生成されます。(下図)

-s 5.0により、5cm角のタグを指定していますが、.pngファイルなので印刷したときにサイズがぴったり5cmにならないことがあります。

これを紙に印刷し、タグのサイズを測ります。私のタグは6.1cmでした。

launchファイルを作成する

ar_track_alvarパッケージには、個別のARタグを認識するための"Individual Marker"ノードと、複数のARタグを組み合わせて認識するための"Find Marker Bundles"ノードの2つのタイプがあります。さらにそれぞれKinectを使うタイプと通常の(Depth情報のない)カメラを使うタイプのノードが用意されています。

今回は、個別のARタグをWebカメラで認識するので、"individualMarkersNoKinect"のノードを使用します。ar_track_alvarパッケージに含まれるlaunchファイルを元に、以下のlaunchファイルを作成しました。


<launch>
	<arg name="marker_size" default="6.1" />
	<arg name="max_new_marker_error" default="0.08" />
	<arg name="max_track_error" default="0.2" />
	<arg name="cam_image_topic" default="/camera/rgb/image_raw" />
	<arg name="cam_info_topic" default="/camera/rgb/camera_info" />	
	<arg name="output_frame" default="/camera_frame" />

	<node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen"
	 args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" />
</launch>
		

"marker_size"パラメータでARタグのサイズ=6.1cmを指定しています。またカメラ画像とカメラ情報のトピック、カメラフレームの名称を"/camera_frame"に指定しています。

カメラから見たARタグの姿勢を取得

カメラノード、先ほどのlaunchファイルでar_track_alvarノード、それにRVizを起動します。

$ roslaunch ptcam_vision usb_cam.launch
$ roslaunch ptcam_vision ar_indiv.launch
$ rosrun rviz rviz
		

RVizで[Add]ボタン->Cameraを選択してCamera画面を追加します。Displaysにおいて、Camera--ImageTopicで/camera/rgb/image_rawを、FixedFrameでcamera_frameを選択、GlobalOptions--FixedFrameでcamera_frameを選択します。[Add]ボタン->TFを選択します。これで下図のような設定になります。

そしてカメラにARタグを映してみると

このようにカメラ画像にARタグの姿勢が座標軸として重ねて表示されます。 ARタグを回転させたり、傾けたりすると座標軸がそれに追従することが確認できます。

さらに、RVizの中央画面には、カメラから見たタグの姿勢が表示されます。

FixedFrameでカメラフレーム(camera_frame)を座標系に指定しているので、カメラフレームから見たARタグの姿勢を表しています。

もちろん、ARタグのIDも識別されているので、TF->ShowNamesのチェックをつけると各タグの名称も表示されます。(上の画像では非表示にしてあります。)

ar_track_alvarがpublishするトピック

ar_track_alvarノードは、カメラ画像の/camera/rgb/image_rawトピックをsubscribeしてARタグを認識、姿勢の計算をします。

各タグの姿勢(座標と向き))は、ar_pose_markerトピックにpublishされます。これは、カメラ(camera_frame)からみた各ARタグの姿勢です。

また、上でカメラ画像に座標軸を重ねて表示させましたが、これはar_track_alvarノードがpublishするvisualization_markerトピックを利用して画像に座標軸と中心のマークを表示しています。

カメラフレームをURDFに追加する

以前にパンチルトカメラのURDFを記述しました。 このURDFにカメラフレームを追加すれば、地面(base_link)からカメラフレームの位置をTFで計算できるようになります。


<?xml version="1.0"?>


<robot name="pan_tilt_camera">

  <link name="base_link">
    <visual>
      <geometry>
        <mesh filename="package://ptcam_description/meshes/BaseUnit.stl"/>
      </geometry>
      <material name="red">
        <color rgba="0.5 0 0 1"/>
      </material>
    </visual>
  </link>

  <link name="pantbl_link">
    <visual>
      <geometry>
        <mesh filename="package://ptcam_description/meshes/PanUnit3.stl"/>
      </geometry>
      <material name="green">
        <color rgba="0 0.7 0 1"/>
      </material>
    </visual>
  </link>

  <link name="cam_link">
    <visual>
      <geometry>
        <mesh filename="package://ptcam_description/meshes/CamUnit3.stl"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 0.7 1"/>
      </material>
    </visual>
  </link>

  <link name="camera_frame">
  </link>


  <joint name="pan" type="revolute">
    <parent link="base_link"/>
    <child link="pantbl_link"/>
    <origin xyz="0 0 0.0031" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="1000.0" lower="-1.5" upper="1.5" velocity="0.5"/>
  </joint>

  <joint name="tilt" type="revolute">
    <parent link="pantbl_link"/>
    <child link="cam_link"/>
    <origin xyz="0.0128 -0.0122 0.017" rpy="0 0 0"/>
    <axis xyz="0 -1 0"/>
    <limit effort="1000.0" lower="-1.5" upper="1.5" velocity="0.5"/>
  </joint>

  <joint name="arm_camera" type="fixed">
    <parent link="cam_link"/>
    <child link="camera_frame"/>
    <origin xyz="0.0126 0.0122 0.03825" rpy="-1.57 0 -1.57"/>
    <axis xyz="0 1 0"/>
    <limit effort="1000.0" lower="-1.5" upper="1.5" velocity="0.5"/>
  </joint>

</robot>
		

追加したのは、後半にある、"camera_frame"のlinkと"arm_camera"のjointだけです。

camera_frameは、linkですが座標系だけあればよいので、名称だけを指定しています。arm_cameraジョイントは、cam_linkとcamera_frameを接続する、fixed typeの関節です。 このジョイントでcamera_frameの位置と向きを指定しています。

カメラフレームの位置関係は、cam_linkの節点からカメラレンズあたりの位置を3D CADで測定して割り出しています。 厳密に言えば、カメラフレームの原点はカメラ内部のイメージセンサあたりにあるはずですが、今回そこまでの精度が要求されるはずもありません。

URDFにcamera_frameをきちんと設定できたか、RVizで確認します。

パンチルトカメラも含めたARタグの姿勢

パンチルトカメラも含め、地面から見たARタグの姿勢を求めてみます。

$ roslaunch ptcam_arduino ptcam_manctrl.launch
		

以前行ったように、実機を制御するノードを起動します。これで実機の関節情報が得られるので、それをもとにTFで各フレーム(ジョイント)の位置関係が計算されます。

RVizでRobotModelを表示し、FixedFrameをbase_linkに設定します。

このようにカメラが置かれた地面の座標系からみた各ARタグの姿勢が表示されます。

今回行ったARタグの姿勢の取得は、例えば次の応用が考えられます。

ARタグをある決まった場所に(既知の座標に)貼っておき、それを移動ロボットに搭載されたカメラで映します。これでロボットから見たARタグの姿勢がわかるので、もちろん逆向きにARタグから見たロボットの姿勢もわかるわけです。これにより空間におけるロボットの姿勢を知ることができます。