イントロ
はっきりいってはっちゃけ
ガッツリやってガッテン...
皆様こんばんは。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氏は非常に親切な方で、質問をしたところ、上記解決手順を教えて下さいました。ここでお礼を申し上げます。ありがとうございました。