はじめに
本記事は3D Sensor Adevent Calenderの第14日目です。
https://qiita.com/advent-calendar/2019/3d-sensor
最初にお詫びを
Realsense D435iをベースに作業を行っていたため、勘違いしてました。。。
というわけで、3Dとはほとんど関係ない、IMUとカメラのキャリブレーション についてのお話です。
ご存知の通り、RealsenseやKinect などの3Dセンサーだけでなく、スマートフォン などにはInertial Measurement Unit (IMU)というものが入っています。これは、ジャイロスコープ と加速度計からなる装置（GPS などのセンサが付く場合もある）で、3軸の角速度および3方向の加速度を取得することができます。
例えばARなどではVisual SLAMという技術を用いて、カメラの映像から三次元のマップとカメラの位置姿勢を同時推定することで、カメラ映像にリアルタイムで3Dオブジェクトをマッピング することができますが、この技術の欠点として、撮影物体の形はわかっても大きさまではわからなかったり、テクスチャのないのっぺりした画像に対してはうまくトラッキング できないなどの欠点があります。
Visual Inertial SLAMは、カメラ映像にIMUの情報を加えることで、これらの欠点を補うことが可能です。
Visual SLAMやVisual Inertial SLAMについては、こんな記事書いてます。
takmin.hatenablog.com
Visual Inertial SLAMを使うためにはカメラやIMUについてキャリブレーション を行い、以下のパラメータを取得する必要があります。
カメラのレンズ歪や焦点距離 等の内部パラメータ
IMUのノイズの分散
カメラとIMUとの位置関係
このカメラとIMU間のキャリブレーション を行うツールとして、近年良く使用されているのがKalibrです。
github.com
Kalibrはチューリッヒ工科大学 のPaul Furgaleらによって開発された、複数カメラ、カメラ/IMU、ローリングシャッターのキャリブレーション を行うツールキットです。
この記事では、このKalibrの使い方について日本語で簡単に解説をしたいと思います。
Kalibrでキャリブレーション する方法については、以下の動画を見ていただくと、すぐイメージが出来ると思います。
VIDEO www.youtube.com
インストール手順
インストールはこちらの公式ドキュメントに基づいて行いました。
https://github.com/ethz-asl/kalibr/wiki/installation
こちらのドキュメントではUbuntu 14.04とROS Indigoを使用していますが、私はUbuntu18.04とROS Melodicで行いました。
インストール方法にはCDEパッケージというのを用いて依存関係も含めて一括で入れる方法と、ソースからビルドする方法の２つがあり、今回はソースからビルドする方法で行います。
私の場合、すでにROS Melodicがインストール済みだったため、以下の手順でインストールを行いました。
1. 依存関係のあるライブラリのインストール
# sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-melodic-vision-opencv ros-melodic-image-transport-plugins ros-melodic-cmake-modules software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev
# sudo pip install python-igraph --upgrade
また、Realsenseを使用する場合は、別途Realsense SDK とrealsense-rosをインストールする必要があります。
Realsense SDK とrealsense-rosのインストール方法についてはここでは説明しません。以下のサイトを参考にしてください。
Realsense SDK
https://www.intelrealsense.com/developers/
Realsense ROS
https://github.com/IntelRealSense/realsense-ros
KlibrをソースからビルドするためにROSのワークスペース を作成します。
# mkdir -p ~/kalibr_ws/src
# cd ~/kalibr_ws
# source /opt/ros/melodic/setup.bash
# catkin init
# catkin config --extend /opt/ros/melodic
# catkin config --merge-devel
3. リポジトリ をcloneしてビルド
最後にKalibrのソースコード をgithub よりダウンロードしてビルドします。
# cd ~/kalibr_ws/src
# git clone https://github.com/ethz-asl/kalibr.git
# cd ../
# catkin build -DCMAKE_BUILD_TYPE=Release -j4
と、ここまではインストールドキュメントの手順通りだったのですが、私の場合ビルド中で以下のようなEigenとOpenCV に関するエラーが出力されました。
Errors << ethz_apriltag2:cmake /home/takuya/kalibr_ws/logs/ethz_apriltag2/build.cmake.002.log
CMake Warning at /opt/ros/melodic/share/cmake_modules/cmake/Modules/FindEigen.cmake:62 (message):
The FindEigen.cmake Module in the cmake_modules package is deprecated.
Please use the FindEigen3.cmake Module provided with Eigen. Change
instances of find_package(Eigen) to find_package(Eigen3). Check the
FindEigen3.cmake Module for the resulting CMake variable names.
Call Stack (most recent call first):
CMakeLists.txt:8 (find_package)
CMake Warning at /opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:166 (message):
catkin_package() DEPENDS on 'opencv' but neither 'opencv_INCLUDE_DIRS' nor
'opencv_LIBRARIES' is defined.
Call Stack (most recent call first):
/opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:102 (_catkin_package)
CMakeLists.txt:10 (catkin_package)
実は既にOpenCV をソースからビルドしていたのに、別にOpenCV をaptでインストールしてしまったために、OpenCV の参照先関係でエラーが起きたようです。
そこで、以下のようにOpenCV とEIGENのパスを指定したらうまくいきました。
# catkin build -DCMAKE_BUILD_TYPE=Release -j4 -Deigen_INCLUDE_DIRS=/usr/include/eigen3 -DCMAKE_MODULE_PATH=/usr/share/OpenCV
4. catkinワークスペース のsetupをsourceするスクリプト を.bashrcに追加
# echo "source /home/takuya/kalibr_ws/devel/setup.bash" >> ~/.bashrc
# source /home/takuya/kalibr_ws/devel/setup.bash
これでKalibrを使用する準備が整いました。
では、実際にKalibrを使用してカメラとIMUのキャリブレーション を行います。
キャリブレーション の手順については、以下の公式ドキュメントを参考にしてください。
Camera IMU calibration · ethz-asl/kalibr Wiki · GitHub
大まかな手順としては
IMUの初期パラメータを記載したYAML ファイルの準備
チェッカーパターンを壁に貼り付けて、センサーで撮影し、データをRos bagの形で保存
キャリブレーション プログラムを起動
となります。
1. IMUの初期パラメータを記載したYAML ファイルの準備
ここでは、以下の３つのYAML ファイルを用意します。
チェッカーパターンについて記述したtarget.yaml
カメラパラメータの初期値を記述したchain.yaml
IMUパラメータの初期値を記述したimu.yaml
target.yaml の編集
まず、以下のサイトからチェッカーパターンとyaml ファイルを取得します。
https://github.com/ethz-asl/kalibr/wiki/downloads
ここでは
Aprilgrid 6x6 0.8x0.8 m (A0 page)
をダウンロードしました。
この"april_6x6_80x80cm_A0.pdf"を印刷し、以下の図のようにAとBの実際の長さをメジャー等で測ります。
"april_6x6_80x80cm.yaml "をコピーし、"target.yaml "とファイル名を変更します。
測った長さは"target.yaml "の中の"tagSize"にAをメートル単位で、"tagSpacing"にAとBの比率（=B/A）を記載します。
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.0271 #size of apriltag, edge to edge [m]
tagSpacing: 0.2214 #ratio of space between tags to tagSize
chain.yaml の編集
chain.yaml には、カメラの内部パラメータやレンズ歪、カメラとIMU間の相対位置を記載します。
ここで記載したものを初期パラメータとしてキャリブレーション を行い、パラメータを更新します。
詳しい説明はこちらのリンクを参照してください。
https://github.com/ethz-asl/kalibr/wiki/yaml-formats
このカメラパラメータの取得は、私はRealsense SDK を用いてセンサーが内部的に保有 しているパラメータを取得しました。
// Create a Pipeline - this serves as a top-level API for streaming and processing frames
rs2::pipeline p;
// Configure and start the pipeline
rs2::pipeline_profile selection = p.start();
// Extrinsic Paramter
rs2::stream_profile color_stream = selection.get_stream(RS2_STREAM_COLOR);
rs2::stream_profile accel_stream = selection.get_stream(RS2_STREAM_ACCEL);
rs2_extrinsics a2c = accel_stream.get_extrinsics_to(color_stream);
printf("T_cam_imu:");
//print extrinsic parameter
printf("[");
int k = 0;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
printf("%f,",e.rotation[k]);
k++;
}
printf("%f,\n", e.translation[i]);
}
printf("0.0,0.0,0.0,1.0]\n");
// Intrinsic Parameter
rs2::video_stream_profile video_stream = color_stream.as<rs2::video_stream_profile>();
printf("resolution: (%d,%d)\n", video_stream.width(), video_stream.height());
rs2_intrinsics color_intrinsic = video_stream.get_intrinsics();
printf("distortion:");
for (int i = 0; i < 5; i++) {
printf("%f,", color_intrinsic.coeffs[i]);
}
printf("\n");
printf("intrinsics: [%f,%f,%f,%f]\n", color_intrinsic.fx, color_intrinsic.fy, color_intrinsic.ppx, color_intrinsic.ppy);
chain.yaml は以下のようなフォーマットです。
上記のコードで得られたパラメータを記載します。
cam0:
camera_model: pinhole
intrinsics: [610.993591, 610.823059, 323.637970, 238.294464]
distortion_model: radtan
distortion_coeffs: [0.0, 0.0, 0.0, 0.0]
T_cam_imu:
- [0.999945,0.009040,-0.005378,0.020510]
- [-0.009021,0.999953,0.003476,-0.004948]
- [0.005409,-0.003428,0.999979,-0.011323]
- [0.0, 0.0, 0.0, 1.0]
timeshift_cam_imu: -8.121e-05
rostopic: /camera/color/image_raw
resolution: [640, 480]
次にimu.yaml を編集します。
IMUが測定する角速度および加速度は以下の式のように、真値に対しガウス ノイズとバイアスを加えたものとして表現されます。
バイアスは微分 値の変動がガウス分布 に従っており、このガウス ノイズの分散(noise_density)およびバイアスの微分 値の分散(random_walk)をそれぞれyaml ファイルに記載する必要があります。
私はアラン分散（Allan Variance）というものを用いて、これらの値を算出する方法を用いました。ただ、もしRealsense SDK 経由でこれらの値が取得できるなら、その方が良いと思います。（私は調べてもわかりませんでした）
アラン分散について書くと長くなるので、ここでは割愛します。各々ググってください。
#Accelerometers
accelerometer_noise_density: 0.17503487119062797 #Noise density (continuous-time)
accelerometer_random_walk: 0.11540370899319194 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 0.02955356570015007 #Noise density (continuous-time)
gyroscope_random_walk: 0.0038742360784085003 #Bias random walk
rostopic: /camera/imu #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)
2. ROS Bagの取得
キャリブレーション のためのデータをROS Bagという形式で取得します。
Realsenseで撮影した画像やIMUのデータは、realsense-rosを通してROS Topicという形式で配信されます。
ROS Bagはリアルタイムに配信されるROS Topicのメッセージをキャプチャしてファイルとして保存したものです。保存したROS Bagを再生すると、キャプチャ時の信号をそのまま復元できるので、実際のセンサーがなくてもその時のデータの流れを何度でも再現できるようになります。
Kalibrでは、カメラ映像とIMUの信号をROS Bagとして保存する必要があります。
ROS Bagを取得するには、まず印刷したチェッカーパターンを壁に貼り付けます。
次にrealsense-rosを起動しますが、その前に/src/realsense-ros/realsense2_camera/launch/rs_camera.launchファイル内のパラメータを修正します。
今回はcolor_fps 、gyro_fps 、accel_fps を修正し（realsense viewerを起動して確認）、enable_syncをtrueにしました。
以下のコマンドでrealsense-rosが起動します。
# roslaunch realsense2_camera rs_camera.launch
rvisというROSのVisualizationツールを立ち上げておくと、実際にセンサーが取得しているデータを目視で確認することが可能です。
# rosluanch rviz rviz
rosbagのキャプチャを開始します。
引数には、カメラ映像、IMUデータのトピック名を指定します。
# rosbag record /camera/color/image_raw /camera/color/camera_info /camera/accel/imu_info /camera/gyro/imu_info /camera/imu
あとは、 最初の動画で紹介したようにセンサーをチェッカーパターンの前で、x,y,z軸まわりの回転及び、x,y,z軸方向の平行移動を行うように動かします。
ROS Bagを取得できれば、後は実際にキャリブレーション を行うだけです。
編集したyaml ファイルとrosbagを用いてkalibrを実行します。
kalibr_calibrate_imu_camera --bag 2019-10-25-16-04-59.bag --cam chain.yaml --imu imu.yaml --target target.yaml
キャリブレーション が終わるとレポート画面が立ち上がります。
レポート画面
また上記の例では、以下のようなキャリブレーション 済みファイルが出力されます。
camchain-imucam-2019-10-25-16-04-59.yaml
imu-2019-10-25-16-04-59.yaml
results-imucam-2019-10-25-16-04-59.txt
results-imucam-2019-10-25-16-04-59.pdf
camchain-imucam-2019-10-25-16-04-59.yaml はカメラのキャリブレーション 結果で、フォーマットはchain.yaml と同じです。
同様にimu-2019-10-25-16-04-59.yaml はIMUのキャリブレーション 結果でフォーマットは上記のimu.yaml と同じです。
results-imucam-2019-10-25-16-04-59.txtとresults-imucam-2019-10-25-16-04-59.pdfは同じ内容を表していて、キャリブレーション の残差等の結果が記載されています。
これで、キャリブレーション は完了です。
最後に
というわけで、カメラとIMUのキャリブレーション ツールであるKalibrの使い方について解説しました。
Kalibrについては日本語の資料がほとんど存在しないため、こうやってブログに残しておけば誰かの役に立つかなと思っていたのですが、なかなかまとめる時間が取れませんでした。結局、今回3Dは関係ありませんでしたが、アドベントカレンダー という締め切りを使って書かせてもらいました。
私自身、そこまでKalibrを使いまくっているわけではないので、間違っている個所などあるかもしれません。できれば公式ドキュメントを見た上で、この記事が理解の手助けになれば良いなと思います。
最後までお読みいただきありがとうございました。
