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

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

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

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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/ad19a6d1-2ce3-190e-11a6-88704ef705a4.png)

https://april.eecs.umich.edu/software/apriltag



# 手順
1. [AprilTagの生成と印刷](#apriltagの生成と印刷)
1. [*apriltag_ros*パッケージのインストール](#apriltag_rosパッケージのインストール)
1. [パラメータ設定](#パラメータ設定)
1. [パッケージ＆launchファイルの作成](#パッケージ＆launchファイルの作成)
1. [launchファイルの実行と確認](#launchファイルの実行と確認)

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

## AprilTagの生成と印刷
オリジナルで配布されているtag_imageは一辺当たり10ピクセルと非常に小さい．それを特定の大きさまで拡大＆印刷できるようpdfとして保存する．
1. [jpgファイルを組み合わせてpdfを生成できるように，権限を編集](#権限編集)
1. [Apriltagのtag_imageが記載されているGitHubページ](https://github.com/AprilRobotics/apriltag-imgs)をダウンロード
1. [`resize_tags.sh`ファイルの生成](#AprilTagを並べて一つのファイルにする)
1. [`resize_tags.sh`ファイルを実行して，pdfを作成](#pdf生成)

https://docs.cbteeple.com/robot/april-tags

### 権限編集
編集するファイルは`/etc/ImageMagick-6/policy.xml`にある．（[ ]内は記入しない）
```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ファイルとして出力する．

コードのベースには，[このサイト](https://docs.cbteeple.com/robot/april-tags)のものを利用したが，生成時間や生成枚数削減のため，一部コードを変更したのが以下．
```shell: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ファイルが保存される．
生成されるファイルのイメージ画像は以下．

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/689ca67e-e691-b464-9fc4-0df5eb39aac7.png)



## 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つを記載した．

:::note warn
＜追記＞
自分自身が作成したパッケージ内に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でタグの位置を可視化できるようになる．|

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/ad19a6d1-2ce3-190e-11a6-88704ef705a4.png)



例えば，以下のように設定した．
```yaml: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

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

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

  
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/07ec8f58-42d1-b523-aee0-e4a6f29fdda4.png)


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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/cf803920-2aed-b275-891a-c8b3822ffb3b.png)

```yaml: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
```
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3250323/f7075a54-43ee-f2f9-de61-571e130d13eb.png)

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`に再設定した．

```xml: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> 
```

:::note warn
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が映った時だけそれらを検知する．