はじめに
本記事は3D Sensor Adevent Calenderの第14日目です。
https://qiita.com/advent-calendar/2019/3d-sensor
最初にお詫びを
申し込んでみたけど、よくよく考えたら3D関係なかった。。。/3D Sensor Advent Calendar 2019 #Qiita https://t.co/e7GuoWIEAC
— Minagawa Takuya (高度でないAI人材) (@takmin) 2019年11月22日
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については、こんな記事書いてます。
Visual Inertial SLAMを使うためにはカメラやIMUについてキャリブレーションを行い、以下のパラメータを取得する必要があります。
- カメラのレンズ歪や焦点距離等の内部パラメータ
- IMUのノイズの分散
- カメラとIMUとの位置関係
このカメラとIMU間のキャリブレーションを行うツールとして、近年良く使用されているのがKalibrです。
Kalibrはチューリッヒ工科大学のPaul Furgaleらによって開発された、複数カメラ、カメラ/IMU、ローリングシャッターのキャリブレーションを行うツールキットです。
この記事では、このKalibrの使い方について日本語で簡単に解説をしたいと思います。
Kalibrでキャリブレーションする方法については、以下の動画を見ていただくと、すぐイメージが出来ると思います。
インストール手順
インストールはこちらの公式ドキュメントに基づいて行いました。
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
2. catkinワークスペースの作成
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
大まかな手順としては
となります。
1. IMUの初期パラメータを記載したYAMLファイルの準備
ここでは、以下の３つの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.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を起動しますが、その前に
以下のコマンドで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軸方向の平行移動を行うように動かします。
3. キャリブレーション
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を使いまくっているわけではないので、間違っている個所などあるかもしれません。できれば公式ドキュメントを見た上で、この記事が理解の手助けになれば良いなと思います。
最後までお読みいただきありがとうございました。
