Multi-tag bundles

以前、ar_track_alverパッケージを利用し、Webカメラ画像からARタグの姿勢を推定しました。

ar_track_alverパッケージには、タグの姿勢を個別に推定する他に、複数のタグが貼り付けられた物体の姿勢を推定することも可能です。

カメラに写っているタグのみから物体の姿勢を推定するので、最低1つのタグが写っている限り、その物体の姿勢を推定できることになります。

例えば、立方体の各面にARタグを貼り付ければ、カメラに写らないタグがあるものの、立方体の姿勢を推定することができます。

今回は、ar_track_alverパッケージのmulti-tag bundleを利用して立方体の姿勢を推定してみます。

まずは平面で試してみる

ネット上には、「ar_track_alverパッケージのmulti-tag bundleには、まだバグがある」という情報をよく見かけたので、先ずは平面上にタグがある場合を試してみることにしました。

前回使ったID=0~2のARタグを印刷した紙を使います。

タグの位置を定義する

ARタグの姿勢をもとに、物体の姿勢を推定するわけですから、物体上のタグの位置を定義しておく必要があります。

そのために、以下のXMLファイルを作成します。


<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<multimarker markers="3">
    <marker index="0" status="1">
        <corner x="-9.05" y="3.05" z="0" />
        <corner x="-3.05" y="3.05" z="0" />
        <corner x="-3.05" y="9.05" z="0" />
        <corner x="-9.05" y="9.05" z="0" />
    </marker>
    <marker index="1" status="1">
        <corner x="3.25" y="3.05" z="0" />
        <corner x="9.35" y="3.05" z="0" />
        <corner x="9.35" y="9.05" z="0" />
        <corner x="3.25" y="9.05" z="0" />
    </marker>
    <marker index="2" status="1">
        <corner x="3.25" y="-9.35" z="0" />
        <corner x="9.35" y="-9.35" z="0" />
        <corner x="9.35" y="-3.25" z="0" />
        <corner x="3.25" y="-3.25" z="0" />
    </marker>
</multimarker>
		

これは、物体上(今回は紙)における、ID=0~2のタグの姿勢を表しています。

marker index="0"の部分は、ID=0のタグを表し、cornerでARタグの左下から順に反時計回りでARタグの角の座標を指定しています。

座標系は、物体における座標なので、判りやすいように決めます。今回は、紙の中央付近(3つのARタグの真ん中)に原点を置いています。座標系は右手系、単位はcmです。

findMarkerBundlesNoKinectノードで姿勢を推定

multi-tag bundleの場合には、findMarkerBundlesNoKinectノードを使います。以下のlaunchファイルを作成してroslaunchで起動します。


<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" />
	<arg name="bundle_files" default="$(find ptcam_vision)/bundles/Marker012.xml" />

	<node name="ar_track_alvar" pkg="ar_track_alvar" type="findMarkerBundlesNoKinect" 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) $(arg bundle_files)" />
</launch>
		

"bundle_files"で先ほど作成したxmlファイルを指定しています。他は、以前行ったindividual tagの場合と同じです。

カメラにARタグが印刷された紙を写すと、紙の姿勢が表示されます。

座標軸のマークがタグではなく、XMLファイル作成時に決定した「紙の原点」に表示されていることがわかります。

また、複数のタグをまとめて紙の姿勢を推定しているので、最低1つタグがカメラに写っていれば姿勢が推定できることもわかります。

ARタグのある立方体を作る

今度は、立方体で試してみます。

下図のように立方体の展開図を作成し、それを厚めの紙に印刷して立方体を作りました。

ARタグのIDと向きがわかるよう、各タグの左下にIDを振っています。また、各ARタグは4cm角、立方体は各辺6cmとしました。

立方体のARタグ位置をXMLファイルで定義

立方体の場合も同じく、ARタグの位置を定義するXMLファイルを作成します。


<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<multimarker markers="6">
    <marker index="10" status="1">
        <corner x="-2" y="-2" z="-2" />
        <corner x="2"  y="-2" z="-2" />
        <corner x="2"  y="-2" z="2"  />
        <corner x="-2" y="-2" z="2"  />
    </marker>
    <marker index="11" status="1">
        <corner x="2" y="-2" z="-2" />
        <corner x="2" y="2"  z="-2" />
        <corner x="2" y="2"  z="2"  />
        <corner x="2" y="-2" z="2"  />
    </marker>
    <marker index="12" status="1">
        <corner x="2"  y="2" z="-2" />
        <corner x="-2" y="2" z="-2" />
        <corner x="-2" y="2" z="2"  />
        <corner x="2"  y="2" z="2"  />
    </marker>
    <marker index="15" status="1">
        <corner x="-2" y="-2" z="2" />
        <corner x="-2" y="2"  z="2" />
        <corner x="-2" y="2"  z="-2"  />
        <corner x="-2" y="-2" z="-2"  />
    </marker>
    <marker index="13" status="1">
        <corner x="2"  y="-2" z="2" />
        <corner x="2"  y="2"  z="2" />
        <corner x="-2" y="2"  z="2"  />
        <corner x="-2" y="-2" z="2"  />
    </marker>
    <marker index="14" status="1">
        <corner x="-2" y="-2" z="-2" />
        <corner x="-2" y="2"  z="-2" />
        <corner x="2"  y="2"  z="-2"  />
        <corner x="2"  y="-2" z="-2"  />
    </marker>
</multimarker>
		

座標は、原点を立方体の中心、X軸:ID11、Y軸:ID12、Z軸:ID13の各面の法線方向としました。

立方体なのでARタグの頂点座標の指定は難しくありません。ARタグの左下から反時計回りにcornerを指定します。

試してみます

紙の場合と同じくlaunchファイルで起動します。


<launch>
	<arg name="marker_size" default="4.0" />
	<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" />
	<arg name="bundle_files" default="$(find ptcam_vision)/bundles/MarkerBox10.xml" />

	<node name="ar_track_alvar" pkg="ar_track_alvar" type="findMarkerBundlesNoKinect" 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) $(arg bundle_files)" />
</launch>
		

ARタグのサイズ指定とxmlファイルの指定のみを変更しました。

このように、立方体の姿勢をうまく推定できました。