Kalibr標定Intel D435i相機
系統環境：ubuntu16.04+roskinetic
需要的基礎知識基礎：ROS基本知識（如何使當前終端生效，roslaunch文件的編寫）
源碼安裝：kalibr https://github.com/ethz-asl/kalibr.git
Realsense SDK: https://github.com/IntelRealSense/librealsense.git
realsense-ROS: https://github.com/IntelRealSense/realsense-ros.git
1、相機標定
(1)realsense d435i可以直接讀取相機的內參
roslaunch realsense2_camera rs_camera.launch //打開相機節點
rostopic echo /camera/color/camera_info //查看相機內參
可以根據官網提供的相機內參驗證自己的標定結果。
具體標定流程：
a、準備標定板和標定板的參數文件（以棋盤格標定爲例）
具體可參考https://github.com/ethz-asl/kalibr/wiki/downloads
下載Checkerboard 7×6 0.5×0.5m(unscaled)的pdf以及yaml文件。
b、錄製camera標定數據：rosbag
爲了查看錄製照片的好壞，可以通過ROS中的Rviz界面查看具體命令如下：分別在兩個終端實現。
roslaunch realsense2_camera rs_camera.launch//打開相機節點
rviz，選camera_link,add topic->camera->color->image_raw//打開可視化窗口
將圖像頻率降低爲4HZ，這裏可以用throttle方法，不會出錯，併發布新的topic，不會修改原topic：
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
開始錄製rosbag包，最好新建一個文件夾，然後在當前終端輸入以下命令：
mkdir bagfiles
cd bagfiles
rosbag record -O camd435i /color
其中-O表示設置的bag包名字，/color爲錄製的話題的名稱。
ctrl+c結束錄製，bag文件會自動保存到錄製時所在的目錄。
c、標定相機
注意：在使用之前對kalibr_workspace source一下，就是之前我提到的需要的基礎知識。
單個相機標定（測試成功）
kalibr_calibrate_cameras --target /home/sunshine/d435ibagfiles/checkerboard_7x6_50x50cm.yaml --bag /home/sunshine/d435ibagfiles/camd435i.bag --bag-from-to 26 100 --models pinhole-radtan --topics /color --show-extraction
具體參數解釋：
--target //棋盤格yaml參數文件路徑
--bag //錄製rosbag包路徑
--models //相機模型
--topics //話題名稱
多個相機聯合標定（沒有測試）
kalibr_calibrate_cameras --target src/kalibr/checkerboard.yaml --bag src/kalibr/bag/fisheye_2019-07-15-21-52-12.bag --models pinhole-radtan pinhole-radtan --topics /image_raw1_th /image_raw2_th
可能遇到的問題，在啓動這個命令的時候可能遇到缺少一些庫，具體要把這些依賴項都安裝好，如果你之前就按照官方提供的安裝說明應該就沒問題。ROS系統是kinetic的話要把下面的indigo改爲kinetic.
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev ros-indigo-vision-opencv ros-indigo-image-transport-plugins ros-indigo-cmake-modules python-software-properties 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
標定結果保存在kalibr_workspace目錄下，生成了三個文件
camchain-homesunshined435ibagfilescamd435i.yaml //主要作用爲了後期imu+相機聯合標定
report-cam-homesunshined435ibagfilescamd435i.pdf //以圖的方式顯示效果
results-cam-homesunshined435ibagfilescamd435i.txt //含有相機的內參以及重投影誤差
results-cam-homesunshined435ibagfilescamd435i.txt具體內容
Calibration results
====================
Camera-system parameters:
cam0 (/color):
type: <class 'aslam_cv.libaslam_cv_python.DistortedPinholeCameraGeometry'>
distortion: [ 0.10680349 0.23330173 0.0007527 -0.01424711] +- [0.06332829 0.36042001 0.00826051 0.00888244]
projection: [636.2812933 633.32021 301.51496978 244.06377452] +- [ 1.43331881 3.59650713 15.12620104 14.66812844]
reprojection error: [0.000011, 0.000030] +- [0.128319, 0.113157]
Target configuration
====================
Type: checkerboard
Rows
Count: 7
Distance: 0.05 [m]
Cols
Count: 6
Distance: 0.05 [m]
2、IMU標定
1、配置realsense的launch文件，在realsense-ros/realsense2_camera/launch文件夾下，複製rs_camera.launch文件，重命名爲rs_imu_calibration.launch文件。修改配置參數
<arg name="enable_sync" default="true"/>
<arg name="unite_imu_method" value="linear_interpolation"/> //也可以把“linear_interpolation”修改成"copy"
參考博客：https://blog.csdn.net/qq_41839222/article/details/86552367
https://blog.csdn.net/u012177641/article/details/92824014#commentBox
2、標定imu時需要配置imu_utils和code_utils,下載鏈接爲：
https://github.com/gaowenliang/imu_utils
https://github.com/gaowenliang/code_utils
mkdir catkin_imu_utils
cd catkin_imu_utils
mkdir src
git clone https://github.com/gaowenliang/imu_utils.git
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make
source devel/setup.bash
可能遇到的問題
code_utils-master/src/sumpixel_test.cpp:2:24: fatal error: backward.hpp: 沒有那個文件或目錄 compilation terminated.
這個sumpixel_test.hpp 裏可以直接換成：
#include "code_utils/backward.hpp"
3、編寫d435i_imu_calibration.launch文件，在imu_utils/launch文件夾下
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<param name="imu_topic" type="string" value= "/camera/imu"/>
<param name="imu_name" type="string" value= "d435i_imu_calibration"/>
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
<param name="max_time_min" type="int" value= "10"/>
<param name="max_cluster" type="int" value= "100"/>
</node>
</launch>
具體參數注意
標定imu時的launch文件記得修改max_time_min對應的參數，默認是120，意味着兩個小時，要是你的ros包裏的imu數據長度沒有兩個小時，就進行不下去，始終停留在wait for imu data這裏。
標定結果保存在 imu_utils/data/文件夾下
4、具體步驟
step1：進入realsense-ros包，開啓d435i，發佈IMU話題
step2: 錄製imu數據包 rosbag record -O imu_calibration /camera/imu
step3: 運行校準程序 roslaunch imu_utils d435i_imu_calibration.launch
step4: 回放數據包 rosbag play -r 20 imu_calibration.bag
可以直接step1->step3在線標定
5、標定結果
在imu_utils/data/d435i_imu_calibration_imu_param.yaml
%YAML:1.0
---
type: IMU
name: d435i_imu_calibration
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 5.7641536836437718e-02
gyr_w: 1.6660473467600197e-03
x-axis:
gyr_n: 1.6392774317631395e-01
gyr_w: 4.8477797168896648e-03
y-axis:
gyr_n: 6.1062818273645578e-03
gyr_w: 1.2128782830293989e-04
z-axis:
gyr_n: 2.8905855056346572e-03
gyr_w: 2.9074495087454760e-05
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 1.6327198936085560e-01
acc_w: 4.5072862611306866e-03
x-axis:
acc_n: 5.0218265479090460e-02
acc_w: 5.9343436346130033e-04
y-axis:
acc_n: 6.4732875717275387e-02
acc_w: 9.4211033888535437e-04
z-axis:
acc_n: 3.7486482688620093e-01
acc_w: 1.1986314081045406e-02
然後將Acc和Gyr的第一組平均數據拷貝到kalibr對應的imu.yml文件中，
rostopic: /camera/imu
update_rate: 200.0 #Hz
accelerometer_noise_density: 2.89e-01 #continous
accelerometer_random_walk: 4.55e-04
gyroscope_noise_density: 3.02e-03 #continous
gyroscope_random_walk: 2.29e-05
3、相機+IMU聯合標定
3.1準備條件：根據自己標定的camera結果替換下面的數值，建立camchain.yaml文件
cam0:
camera_model: pinhole
intrinsics: [632.9640658678117, 638.2668942402212, 339.9807921782614, 243.68020465500277]
distortion_model: equidistant
distortion_coeffs: [0.366041713382057, 1.1433178097591097, -3.008125411486294, -3.1186836086733227]
T_cam_imu:
- [0.01779318, 0.99967549,-0.01822936, 0.07008565]
- [-0.9998017, 0.01795239, 0.00860714,-0.01771023]
- [0.00893160, 0.01807260, 0.99979678, 0.00399246]
- [0.0, 0.0, 0.0, 1.0]
timeshift_cam_imu: -8.121e-05
rostopic: /color
resolution: [640, 480]
imu.yaml文件
rostopic: /imu
update_rate: 200.0 #Hz
accelerometer_noise_density: 0.01 #continous
accelerometer_random_walk: 0.0002
gyroscope_noise_density: 0.005 #continous
gyroscope_random_walk: 4.0e-06
3.2標定步驟
1、將圖像頻率降低爲20HZ，imu頻率設置爲200Hz,這裏可以用throttle方法，不會出錯，併發布新的topic，不會修改原topic：
rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/gyro/image_info 200.0 /imu
2、錄製rosbag包
rosbag record -O dynamic /color /imu
3、標定
kalibr_calibrate_imu_camera --target ~/bagfiles/checkerboard.yaml --cam ~/bagfiles/camchain.yaml --imu ~/bagfiles/imu.yaml --bag ~/bagfiles/dynamic.bag --bag-from-to 5 45 --show-extraction
4、標定結果評價
4.1 多目標定評估
kalibr標定多目後會生成標定報告以及標定出的外參值，標定報告會直接給出相機座標的空間位置示意圖，結合標定結果和實際位置比較可以較爲直觀的判斷結果是否可靠；另外就是看重投影誤差，這個值越小越好，我的在0.15以下已經是很好的了。內參數的話也可以關注一下，我使用的d435i，標定後雙面的內參基本一樣，不過和自帶/camera_info還是有所區別的，但是不是很大。
4.2 IMU+相機聯合標定評估
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.116388852487, median 0.100494263466, std: 0.115779726566
Reprojection error (cam1): mean 0.118837162459, median 0.102707273927, std: 0.118032053496
Gyroscope error (imu0): mean 0.566382365906, median 0.424566309149, std: 0.541174462645
Accelerometer error (imu0): mean 0.303879126638, median 0.257914882481, std: 0.20381924896
Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.116388852487, median 0.100494263466, std: 0.115779726566
Reprojection error (cam1) [px]: mean 0.118837162459, median 0.102707273927, std: 0.118032053496
Gyroscope error (imu0) [rad/s]: mean 0.0221873017669, median 0.016631839881, std: 0.0211998145317
Accelerometer error (imu0) [m/s^2]: mean 0.128924994664, median 0.109424017423, std: 0.0864731838454
而kalibr提供的例程是：
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.142975755445, median 0.130686487357, std: 0.0817972217348
Reprojection error (cam1): mean 0.139113383779, median 0.129061976876, std: 0.0768758604603
Gyroscope error (imu0): mean 0.0947751091434, median 0.082773649866, std: 0.0609365033876
Accelerometer error (imu0): mean 0.363001590633, median 0.285683155402, std: 0.341485432021
Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.142975755445, median 0.130686487357, std: 0.0817972217348
Reprojection error (cam1) [px]: mean 0.139113383779, median 0.129061976876, std: 0.0768758604603
Gyroscope error (imu0) [rad/s]: mean 0.0067016122363, median 0.00585298091238, std: 0.00430886147672
Accelerometer error (imu0) [m/s^2]: mean 0.0513361772636, median 0.0404016992912, std: 0.0482933329317
參考博客：https://blog.csdn.net/u012177641/article/details/92824014#commentBox
https://blog.csdn.net/kongdaqing1290/article/details/97524087
https://blog.csdn.net/u012177641/article/details/92824014
https://cloud.tencent.com/developer/article/1438368
https://blog.csdn.net/qq_41839222/article/details/86552367
https://blog.csdn.net/weixin_40628128/article/details/95945945
https://blog.csdn.net/weixin_40830684/article/details/88768225