イントロ

はっきりいってはっちゃけ
ガッツリやってガッテン...

皆様こんばんは。Mas@Sensynです。今日はあの不朽の名作のイントロからスタートしようとしたのですが、著作権上の問題から断念し、自作の歌詞、「俺もマジで恋する2,3秒前」のイントロからしっとりとスタートさせていただきました。

目的

画角に全く重なりの無い複数のステレオカメラセットの外部パラメータ（回転と並進）を導出する。

TagSLAMとは

有名なfiducial markerの一つであるApril tagだけを認識し、SLAM(simultaneous localization and mapping)を行えるソフト。今回の目的の、画角に重なりのないカメラの外部パラメータキャリブレーションにも使える。
詳細は、本家のTagSLAMのページ。

前提環境

OS: Linux
distribution: Ubuntu 18.04
キャリブレーションターゲットボード: Kalibrのtarget作成ページから作成したaprilgridをA0サイズに印刷したボード(今回は新規作成した6x8tagのボードを使用)
内部パラメータのキャリブレーションは実施済み

インストール

ROS本家ページを参考にROSをインストール

aptで必要ライブラリをインストール

sudo apt install python-catkin-tools 
sudo apt -y remove gtsam
sudo add-apt-repository -y --remove ppa:bernd-pfrommer/gtsam
sudo apt-add-repository -y ppa:borglab/gtsam-release-4.0
sudo apt update
sudo apt install libgtsam-dev libgtsam-unstable-dev

本家のTagSLAM github pageから以下のようにclone

git clone https://github.com/berndpfrommer/tagslam_root.git

tagslamサブモジュールの取得とビルド

cd tagslam_root
git submodule update --init --recursive
catkin config -DCMAKE_BUILD_TYPE=Release
catkin build

sample dataの展開
```
bunzip2 src/tagslam_test/tests/*/*.bz2
```

インストールできたことの確認

source tagslam_root/devel/setup.bash
roslaunch tagslam tagslam.launch bag:=`rospack find tagslam`/example/example.bag

キャリブレーション手順

カメラ内部パラメータキャリブレーション

TagSLAMはKalibrを用いたカメラ内部パラメータキャリブレーションと非常に連携を良くしているので、Kalibrによって行うことをおすすめします。手順は上記リンクを参考にして下さい。今回はこちらは割愛。

カメラ外部パラメータキャリブレーション

データの用意

データを撮る
いずれかのカメラには必ずTagが写っているようにする。
そうなるように床や壁にTagを配置する(今回は一つのボードだけでどこかのカメラにタグが映るようにできたので、１つのボードだけを使用した)。
そうしないとTagSLAMはtagのみを認識するので、カメラが自己位置を見失ってしまう。
今回はpng画像として保存。
rosの.bag fileを作る
kalibrのkalibr_bagcreatorプログラムを使用する。詳細はKalibrのページを参考にしてください。
カメラの内部パラメータ等のcamera_infoは、後にファイルとしてTagSLAMに渡すため、bag fileに埋め込まなくて良く、png画像だけからbag fileを作れば良いです。(私はここでハマった)
今回は、"images.bag"というファイル名を仮定します。
内部パラメータファイルを保存
tagslam_root/data　を作成し、そこに、以下のファイルを保存します。

images.bag：　上記で作成したキャリブレーション画像のbagfile
camera_poses.yaml: カメラ0の位置。src/tagslam/example/camera_poses.yamlからコピー
cameras.yaml: カメラ内部パラメータキャリブレーションファイルに以下のような記述を追記
```
```
cam0:
...  
  rostopic: /cam0/image_raw
  tagtopic: /cam0/tags
  rig_body: rig
...
```
```
tagslam.yaml: src/tagslam/example/tagslam.yamlからコピーし、使用するボードに合わせて以下のように編集
```
```
max_num_incremental_opt: 5000
default_tag_size: 0.07
...
「
tags:
- id: 0
  size: 0.12
」
```
の部分を以下に
「
- board_1:
is_static: true
type: board
board:
tag_start_id: 0
tag_size: 0.07
tag_bits: 6
tag_spacing: 0.25
tag_rows: 6
tag_columns: 8
tag_rotation_noise: 0.001
tag_position_noise: 0.001
」
```

外部パラメータキャリブレーション実行

編集が必要なファイルをコピー

mkdir script
cp src/tagslam/launch/sync_and_detect.launch script/
cp src/tagslam/launch/tagslam.launch script/
cp example/tagslam_example.rviz script/tagslam.rviz

script内にコピーした sync_and_detect.launch　を以下のように編集。下記例はステレオカメラが３セット(6カメラ)の場合。

<arg name="images_are_compressed" default="false"/>
...
<rosparam param="image_topics">
  ["/cam0/image_raw",
  "/cam1/image_raw",
  "/cam2/image_raw",
  "/cam3/image_raw",
  "/cam4/image_raw",
  "/cam5/image_raw"     
  ]
</rosparam>
<rosparam param="image_output_topics">
  ["/cam0/annotated/compressed",
  "/cam1/annotated/compressed",
  "/cam2/annotated/compressed",
  "/cam3/annotated/compressed",
  "/cam4/annotated/compressed",
  "/cam5/annotated/compressed"]
</rosparam>
<rosparam param="tag_topics"> ["/cam0/tags",
"/cam1/tags",
"/cam2/tags",
"/cam3/tags",
"/cam4/tags",
"/cam5/tags"]</rosparam>
...
<param name="black_border_width" value="2"/>

Kalibrのapril boardを用いている場合、そのボードを見ればわかるように、最後のblack_border_widthを2にすることもポイント。私はここでもハマりました。

script内にコピーした tagslam.launch　を以下のように編集

<arg name="data_dir" default="$(find tagslam)/../../data"/>
...
<arg name="bag" default="$(arg data_dir)/images.bag"/>
<arg name="max_number_of_frames" default="100000"/>
...
<arg name="has_compressed_images" default="false"/>
...
  <param name="has_compressed_images" value="$(arg has_compressed_images)"/>

script内にコピーした tagslam.rviz　はお好きなように編集。
自分で色々windowを付け足してそれを保存すればOK。
以下をtagslam_root/scriptに、run_tagslam.shという名前で置き、実行。(あ、説明がめんどくさくなってサボりました)

#/bin/sh
# ref: https://berndpfrommer.github.io/tagslam_web/getting_started/
#######

# prepare envs
ROOTDIR=$(dirname $(readlink -f "$0"))/.. # we need absolute path for sync_and_detect.launch
source ${ROOTDIR}/devel/setup.bash

###############
## parameters

# # # example params(if you want to use your own, comment out this block, and uncomment next one)
# LAUNCHDIR="`rospack find tagslam`/launch"
# RVIZFILE=`rospack find tagslam`/example/tagslam_example.rviz
# BAGFILE=`rospack find tagslam`/example/example.bag
# TOPICS=/pg_17274483/image_raw/compressed
# # SEPARATESTEP=true

# # multicam calib example. (but this example send already extracted odometry)
# RVIZFILE=`rospack find tagslam`/example/tagslam_example.rviz
# TAGSLAMLAUNCH="tagslam tagslam.launch"
# APRILTAGLAUNCH="tagslam apriltag_detector_node.launch"
# BAGFILE=${ROOTDIR}/src/tagslam_test/tests/test_2/reference.bag
# TOPICS=""

# your params(you can edit)
LAUNCHDIR=${ROOTDIR}/script
RVIZFILE=${LAUNCHDIR}/tagslam.rviz
BAGFILE=${ROOTDIR}/data/images.bag
TOPICS="" # empty to play all topics
SEPARATESTEP=true #currently, only true is suppported.
RESULTDIR=${ROOTDIR}/result
################

TAGSLAMLAUNCH=${LAUNCHDIR}/tagslam.launch
APRILTAGLAUNCH=${LAUNCHDIR}/apriltag_detector_node.launch
SYNCDETLAUNDH=${LAUNCHDIR}/sync_and_detect.launch

# main process
(
    roscore & 
    sleep 3s
    rosparam set use_sim_time true
    rviz -d $RVIZFILE &

    if [ -z $SEPARATESTEP ] ; then # originally called as online 
    	roslaunch ${TAGSLAMLAUNCH} run_online:=true &
    	roslaunch ${APRILTAGLAUNCH} &
    	rosbag play --clock $BAGFILE --topics $TOPICS
    else
	roslaunch ${SYNCDETLAUNDH} bag:=$BAGFILE topics:=$TOPICS
	TAGEXTRACTEDBAGFILE=${BAGFILE}_output.bag
    	roslaunch ${TAGSLAMLAUNCH} bag:=$TAGEXTRACTEDBAGFILE
    fi
    
    wait
)

# copy result
if [ -z ${RESULTDIR} ]; then exit 0; fi

mkdir -p ${RESULTDIR}

RESULTFILES="camera_poses.yaml calibration.yaml poses.yaml error_map.txt 
tag_corners.txt tag_diagnostics.txt time_diagnostics.txt out.bag"
for f in ${RESULTFILES}
do
    cp ~/.ros/$f ${RESULTDIR}
done

結果の確認

結果がtagslam_root/resultディレクトリに保存されています。
内部・外部パラメータの変換行列は　calibration.yamlに、外部パラメータの回転と並進は、camera_poses.yamlに保存されています。
キャリブレーション中のカメラの動きを確認するには、下記コマンドを実施すればOK。

source devel/setup.bash
roscore
rviz -d src/tagslam/example/tagslam_example.rviz
rosbag play --clock result/out.bag

するとrviz上でこんな感じで見れます。

まとめ

という訳で今回は、TagSLAMというソフトを使った、画角に重なりのないカメラのキャリブレーション手法についてご説明しました。
　一見簡単そうに見えて、実はオリジナルサイトにファイル編集方法の説明がなかったために結構ハマりました。
オリジナルサイトにはexampleがたくさんあり、画角に重なりのないカメラのキャリブレーションについても一見exampleがあるのですが(test6)、これは、既にキャリブレーションが終わったものの、カメラ位置を再現するだけのデータであって、実際のキャリブレーションをどう行うかの説明はありませんでした。
　しかし、TagSLAMのauthorのBernd Pfrommer氏は非常に親切な方で、質問をしたところ、上記解決手順を教えて下さいました。ここでお礼を申し上げます。ありがとうございました。