AprilTagを使った位置＆姿勢推定

Last updated at 2024-01-16Posted at 2023-11-30

AprilTagとは

ミシガン大学が開発したシステム（visual fiducial system）．

AprilTagと呼ばれるQRコードみたいな "tag"（下図）をカメラで読み取ることで，その正確な三次元情報（位置，姿勢，tagの種類）を計算できる．画像処理を応用した手法．

家にあるプリンタでAprilTagを印刷して，カメラで読み取れば実行可能な手軽さにもかかわらず，高い正確さを誇るため，様々な分野で利用されている．

手順

AprilTagの生成と印刷
apriltag_rosパッケージのインストール
パラメータ設定
パッケージ＆launchファイルの作成
launchファイルの実行と確認

用意するもの

カメラ（今回はrealsenseを使った）
印刷したAprilTag
ros（複数タグを同時に探知するには，rosパッケージが不可欠）

AprilTagの生成と印刷

オリジナルで配布されているtag_imageは一辺当たり10ピクセルと非常に小さい．それを特定の大きさまで拡大＆印刷できるようpdfとして保存する．

jpgファイルを組み合わせてpdfを生成できるように，権限を編集
Apriltagのtag_imageが記載されているGitHubページをダウンロード
resize_tags.shファイルの生成
resize_tags.shファイルを実行して，pdfを作成

権限編集

編集するファイルは/etc/ImageMagick-6/policy.xmlにある．（[ ]内は記入しない）

/etc/ImageMagick-6/policy.xml

  [change memory size to 'value="32GB"']
  <policy domain="resource" name="memory" value="256MiB"/> 
  
  [... about 70 lines deleted ...]
  <!-- disable ghostscript format types -->
  [comment out former lines]
  <!--
  <policy domain="coder" rights="none" pattern="PS" />
  <policy domain="coder" rights="none" pattern="EPI" />
  <policy domain="coder" rights="none" pattern="PDF" />
  <policy domain="coder" rights="none" pattern="XPS" />
  -->
  [add following lines]
  <policy domain="coder" rights="read|write" pattern="PDF,PS" />
</policymap>

AprilTagを並べて一つのファイルにする

githubに上げられているApriltagのimageファイルを利用して，単体/複数のtagを好きなサイズ＆並べ方で配列し，一つのpdfファイルとして出力する．

コードのベースには，このサイトのものを利用したが，生成時間や生成枚数削減のため，一部コードを変更したのが以下．

resize_tags.sh

#!/bin/bash

# ==============================================
# User inputs (change these settings!)
# ==============================================

# Set the folder to use
# !!! Path should be absolute path (not relative) !!!
FILES=~/hauser_ws/src/apriltag-imgs/tag36h11/*.png  # Set the folder to convert from
OUT_FILES=~/hauser_ws/src/hauser/hauser_localization/tags/tag36h11/  # Set the folder to output

# Set the desired width of your tags
width=80.0         # [mm]  Set the desired width in real units
dpi=300            # [ppi] Set the dpi (to control the print quality)
extra_margin=0.1     # [px]  Add an extra margin around your tags
default_width=10.0 # [px]  The pixel width of the original images

# Set the grid size you want to make
grid_h=2           # [num] Number of rows
grid_w=3           # [num] Number of columns
grid_filetype=pdf  # [png, jpg, pdf] Set the filetype for the grids
page_num=1             # [num] Number of pages to generate 

# Decide whether to keep intermediate files
keep_single_tag_images=false



# ==============================================
# Do some magick! (ImageMagick)
# ==============================================

# Get the total number of files
total_files=0

for f in $FILES
do
	total_files=$((total_files+1))
done

total_files_check=$((total_files-1))


# Calculate the scale factors
scale1=$(awk "BEGIN {printf \"%.2f\",${width}*(1+(2/${default_width})) * ${dpi} / 24.5 * 100 / ${default_width}"})
scale=$(awk "BEGIN {printf \"%d\", ${scale1}"})

pixels1=$(awk "BEGIN {printf \"%.2f\",${width}*(1+(2/${default_width})) * ${dpi} / 24.5"})
pixels=$(awk "BEGIN {printf \"%d\", ${pixels1}"})

margin=$(awk "BEGIN {printf \"%d\", ${pixels1}* ${extra_margin}/${default_width}"})

grid_check=$((grid_h * grid_w))

echo "$scale1"
echo "$scale"
echo "$margin"


# Make a fresh output directory
base="$(dirname -- $OUT_FILES[0])"
OUTFOLDER="${base}/${width}mm"
mkdir -p $OUTFOLDER
rm -r $OUTFOLDER/*


# Set up some variables
allnames=""
i=0
last_i=0
montage_num=0
work_size=$((grid_h * grid_w * page_num + 1))
work_cnt=0

# Loop over all pictures
for f in $FILES
do
    # Done convertion only to the neccesary pictures
    if [ ${work_cnt} -le ${work_size} ] 
    then
        # Skip the mosaic picture
        if [[ "$f" == *"mosaic"* ]]; then
            total_files_check=$((total_files_check-1))
            continue
        fi

        # Generate the output filename for the current image
        ext="${f#*.}"
        filename="$(basename -- $f)"
        name=$(echo "$filename" | cut -f 1 -d '.')
        outname="${OUTFOLDER}/${name}.${ext}"
        #echo "$outname"

        # Rescale the current image
        convert $f -scale $scale% $outname

        # Add the image to the list of images to add to the current grid
        allnames="${allnames} ${outname}"

        # Every time we need to generate a grid, do it 
        v=$(((i+1)%grid_check))
        if [[ ( "$v" -eq 0  &&  "$i" != 0 ) || "$i" == "$total_files_check" ]]; then

            # Generate the output filename for the grid
            last_i_str=`printf "%05d\n" $last_i`
            i_str=`printf "%05d\n" $i`
            outmontage="${OUTFOLDER}/grid_${last_i_str}_${i_str}.${grid_filetype}"

            # Generate the grid and save it as a pdf
            montage $allnames -geometry ${pixels}x${pixels}+${margin}+${margin} -tile ${grid_w}x -density ${dpi} $outmontage
            echo "$outmontage"

            # Update grid counters
            last_i=$((i+1))
            montage_num=$((montage_num+1))
            allnames=""
        fi
        # Update single image counter
        i=$((i+1))
    fi
    work_cnt=$((work_cnt+1))
done

# Combine all single-page PDFs into one
if [ "$grid_filetype" == "pdf" ]; then
	echo "Combining PDFs"
	sleep 1
	convert -density ${dpi} ${OUTFOLDER}/*.pdf ${OUTFOLDER}/all_tags.pdf
	rm -r $OUTFOLDER/grid*
fi

if [ "$keep_single_tag_images" != true ]; then
	rm -r $OUTFOLDER/*.png
fi

使う際は，コード上部のパラメータを各自設定することで，自分の用途に合った出力を得られる．

Setting parameters	Meaning
FILES	githubからダウンロードしたtag_imageのあるフォルダへの絶対パス ※ このパスによって，使用するタグの種類も決まる
OUT_FILES	生成したpdfを保存するフォルダへの絶対パス
width	タグの印刷したときのサイズ（width x width）[mm]
dpi	印刷したときのタグの解像度 [ppi]
extra_margin	タグとタグの間隔 [px]
default_width	参考にするtag_imageの大きさ．githubからダウンロードしたimageを使う場合は，変更不要
grid_h, grid_w	タグを表示する配列の大きさ．2x3なら，縦2行・横3行の配列に計6個のタグを並べて出力する．
grid_filetype	出力するファイル形式．png/jpg/pdfの3種類から選ぶ
page_num	出力するページ数．複数ページの場合は，同じ配列で，タグの組み合わせを変えたものを複数出力する．

pdf生成

先ほど作成したshellファイルを実行する．

bash ./path_to_shell_file/resize_tags.sh

すると，OUT_FILESで指定したファイルに出力されたpdfファイルが保存される．
生成されるファイルのイメージ画像は以下．

apriltag_rosパッケージのインストール

$ sudo apt update & sudo apt upgrade
$ sudo apt install ros-melodic-apriltag-ros

パラメータ設定

パラメータ設定について記述したyamlファイルは，apriltag_rosパッケージの中にあるconfigフォルダに記載する．
私の場合，/opt/ros/melodic/share/apriltag_ros内部に以下のyamlファイル2つを記載した．

＜追記＞
自分自身が作成したパッケージ内にconfigファイルを作成することも可能．その場合は，launchファイル内に読み込むべきパスを記述する必要がある．

config/settings.yaml：tagの種類と大きさ，識別方法などの設定を事前に読み込むためのファイル
config/tags.yaml：tagの配置や数を事前に読み込むためのファイル

setting.yaml

設定するパラメータの種類と各パラメータの意味は以下の通り．

Parameter	意味
tag_family	Tagの種類（名前は下図参照）
tag_border	黒い四角形1個1個の大きさ（ビット）．デフォルトでは 1bit で生成される．
tag_threads	Tagの追跡アルゴリズムの一部を並列に処理する場合の並列処理の数．３つ同時に処理するなら，３を入力．自分のパソコンのCPU性能に合わせて設定
tag_decimate	Tagを認識するときの解像度をどれだけ下げるか設定．１より大きい値に設定すると，値が大きいほど解像度は下がり，処理速度も向上するが，小さなtagの識別性能などは下がる．
tag_blur	画像の鋭さ？の設定．0より大きい（>0）とボケた画像になり，0より小さい（<0）と鋭い画像として識別する．
tag_refine_edges	0/1で入力．1(True)に設定すると，tagの認識の正確性が高まる．
tag_refine_decode	0/1で入力．1(True)に設定すると，間違った検出を削減することが出来る．
tag_refine_pose	0/1で入力．1(True)に設定すると，位置推定の正確性が高まるが，計算コストも上昇する．
tag_debug	simgle image detector専用．1(True)に設定すると，デバック用の画像が ~/.rosに保存される．video streamの際た0(False)に設定する．
publish_tf	true/falseで入力．trueに設定すると，検出したtagの位置などを/tfトピックに送信する．これによって，rvizでタグの位置を可視化できるようになる．

例えば，以下のように設定した．

config/setting.yaml

tag_family:        'tag36h11' # Name of tag family
tag_border:        1          # Size (in bits) of the black border (black square). Always 1 if made by optitag
tag_threads:       2          # Number of detection thread. Tune per your CPU
tag_decimate:      1.0        # Reduce the resolution of the image by this number. Increases speed at the sacrifice of detecting smaller tags
tag_blur:          0.0        # tag_blur>0 blurs the image and tag_blur<0 sharpens the image
tag_refine_edges:  1          # improves edge detection and therefore improves pose estimation. Light computation
tag_refine_decode: 0          # reduces false negative detection rate. Medium computation
tag_refine_pose:   0          # improves pose estimation accuracy. Heavy computation
tag_debug:         0          # save debug images to ~/.ros Careful if running with video
publish_tf:        true       # publish tag/bundle poses to /tf topic

tags.yaml

Parameter		意味
standalone_tags		一つ一つのtagを区別して検出するものについての設定
	id	id番号．見つけたいtagのid番号を入力する．id番号は，tag_imageで配布されているpngファイルの名前についている番号と対応（ex. tag36_11_00039.png なら，id=39）
	size	tagの一片の長さ（写真参照）．単位はメートル．
tag_bundles		複数のtagを組み合わせて，一つのtagとして検出するものについての設定
	name	複数tagまとめての名称．rvizに表示させるときに使われる．
	layout	standaleone_tagsと同じように各tagのidとsizeを記載する．それに加えて，すべてのtagの配置情報をxyz座標とquaternionで記載．単位はメートル．

tag_bundlesは複数タグを一つのタグとして検出するので，姿勢などの算出精度はタグ一つの場合と同程度になる．（ただし，タグを組み合わせることで，判別できる総数が増える）
つまり，姿勢や位置の計算精度を高めるなら，standalone_tag，様々なタグを様々な場所に配置するためにバリエーションが欲しいなら，tag_bundlesを用いる！

下図のように0.05m（50mm）間隔で普通にまっすぐな向きのtagを5つ配置した場合のyamlファイルの例を記載する．（単体のtagはないので，standalone_tagsの中身はコメントアウトしてある．standalone_tags自体をコメントアウトすると，警告が出るので注意）

config/tags.yaml

standalone_tags:
  [
#     {id: 10, size: 0.15},
#     {id: 20, size: 0.1},
#     {id: 30, size: 0.07}
  ]
tag_bundles:
  [
    {
      name: 'my_bundle',
      layout:
        [
          {id: 0, size: 0.05, x: 0.0000, y: 0.0000, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0},
          {id: 4, size: 0.05, x: 0.05, y: -0.05, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0},
          {id: 3, size: 0.05, x: -0.05, y: -0.05, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0},
          {id: 2, size: 0.05, x: 0.05, y: 0.05, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0},
          {id: 1, size: 0.05, x: -0.05, y: 0.05, z: 0.0, qw: 1.0, qx: 0.0, qy: 0.0, qz: 0.0}
        ]
    }
  ]

パッケージ＆launchファイルの作成

カメラ⇔apriltag_ros間のデータをやり取りするトピック名がカメラによって異なるので，そこを修正するために新規のlaunchファイルを作成する必要がある．

$ cd ~/catkin_ws/src/
$ catkin_create_pkg april_localization apriltag_ros
$ catkin build
$ source ~/.bashrc
$ cd april_localication
$ mkdir launch
$ vim april_detection.launch

apriltag_rosパッケージは/camera/image_rectでカメラ映像，/camera/camera_infoでカメラ情報をsubscribeするが，このトピック名はカメラの種類&バージョンによって異なる．
ちなみにros-melodicのrealsense_rosパッケージでは

/camera/image_rect==>/camera/color/image_raw
/camera/camera_info==>/camera/color/camera_info
だった．

そこで以下のようにlaunchファイルをかくことで，

/camera/color/~~~のトピックを受け取れるようにし，
image_rectをimage_rawに再設定した．

april_detection.launch

<?xml version="1.0" encoding="UTF-8"?>              
<launch>
  <include file="$(find apriltag_ros)/launch/continuous_detection.launch">
    <arg name="camera_name" value="/camera/color"/>
    <arg name="image_topic" value="image_raw" />
  </include>
</launch>

configファイルをパッケージの中に書く場合は，includeの中に以下の記述を追加

<!-- load parameters (incl. tag family, tags, etc.) -->
    <rosparam command="load" file="$(find your_package_name)/config/settings.yaml"/>
    <rosparam command="load" file="$(find your_package_name)/config/tags.yaml"/>

launchファイルの実行と確認

$ roslaunch april_localization april_detection.launch

これを実行した状態で/tag_detections_imageトピックをrqtなどで表示する（普通のimageトピック）と，apriltagが映った時だけそれらを検知する．

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up