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
来源：CSDN
作者：纷繁中淡定
链接：https://blog.csdn.net/qq_36170626/article/details/102998942