视觉SLAM | RealsenseD435i相机标定
在运行VINS-MONO、VINS-Fusion等SLAM方案的时候，需要很准确的相机参数，否则很容易漂移。本文是RealsenseD435i相机标定过程的记录，标定主要有三个步骤
- IMU标定
- 相机标定
- IMU与相机联合标定
IMU标定
这里使用imu_utils标定imu，在安装imu_utils之前，要先安装code_utils，注意要安装code_utils后再下载imu_utils
cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/code_utils
cd ~/catkin_ws
catkin_make
编译时出现错误：
code_utils/backward.hpp:216:30: fatal error: elfutils/libdw.h: No such file or directory
执行apt-get install libdw-dev 安装对应的库即可解决
接下来安装imu_utils
cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/imu_utils
cd ~/catkin_ws
catkin_make
安装完成后开始标定，我没有采用录制imu话题再回放bag文件的方式，而是直接现场发布imu的话题，同时运行标定的节点。执行下面的命令可以发布带有imu的话题
roslaunch realsense2_camera rs_camera.launch unite_imu_method:="linear_interpolation"
编写一个标定的launch文件，命名为my_calibration_imu_d435i.launch，执行该launch文件，将RealsenseD435i静止摆放两个小时
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
#imu topic的名字
#标定的时长
node>
launch>
完成后就会生成d435i_imu_param.yaml文件
相机标定
相机的参数分为内参和外参，内参可以通过读取/camera/color/camera_info话题获得，但是外参就需要借助别的工具，这里我使用kalibr来获取相机的外参。
安装Kalibr
首先安装依赖项
sudo apt-get install python-setuptools python-rosinstall ipython libeigen3-dev libboost-all-dev doxygen libopencv-dev
sudo apt-get install ros-melodic-vision-opencv ros-melodic-image-transport-plugins ros-melodic-cmake-modules
sudo apt-get install python-software-properties software-properties-common libpoco-dev python-matplotlib python-scipy python-git python-pip ipython
sudo apt-get install libtbb-dev libblas-dev liblapack-dev python-catkin-tools libv4l-dev
sudo pip install python-igraph --upgrade
然而在执行sudo pip install python-igraph时报错
Could not compile the C core of igraph.
本来想忽略这个问题，但是后续步骤会继续报错，没办法必须解决这个问题才行。网上的解决方法是先执行
sudo apt-get install -y libigraph0-dev
再继续安装python-igraph
但是我执行完后还是报一样的错，继续搜索，先执行
sudo apt install build-essential python-dev libxml2 libxml2-dev zlib1g-dev bison flex
再执行
pip install -i https://pypi.tuna.tsinghua.edu.cn/simple python-igraph
中间的-i https://pypi.tuna.tsinghua.edu.cn/simple作用是使用国内的清华源，这样下载会快一点，然后莫名其妙就好了，应该是少安装了依赖项，结果如下：
Successfully built python-igraph
Installing collected packages: texttable, python-igraph
Successfully installed python-igraph-0.8.2 texttable-1.6.2
之后就开始下载安装kalibr，先另外创建一个工作空间，防止损坏原来工作空间的程序
mkdir -p ~/kalibr_workspace/src
cd ~/kalibr_workspace
source /opt/ros/melodic/setup.bash
catkin init
catkin config --extend /opt/ros/melodic
catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release
然后下载并使用catkin build编译kalibr
cd ~/kalibr_workspace/src
git clone https://github.com/ethz-asl/Kalibr.git
cd ~/kalibr_workspace
catkin build -DCMAKE_BUILD_TYPE=Release -j2
在使用catkin build之前，建议先将suitesparse下载下来，并修改CMakelist.txt，因为这个东西可能会由于一些原因下载时间过长，导致编译失败，详见：Camera-IMU标定工具Kalibr的编译
最后执行source ~/kalibr_workspace/devel/setup.bash
标定过程
- 准备标定材料
首先可以从这里下载标定材料，但是普通网友(比如我)似乎下载不了，不过还好它提供了本地生成标定材料的方法，使用kalibr_create_target_pdf --h命令，参照这里
但是在使用时报错ImportError: No module named pyx，解决方法：使用udo apt-get install python-pyx安装pyx即可
接下来生成标定材料，我选择生成官方推荐的Aprilgrid
kalibr_create_target_pdf --type apriltag --nx [NUM_COLS] --ny [NUM_ROWS] --tsize [TAG_WIDTH_M] --tspace [TAG_SPACING_PERCENT]
其中：NUM_COLS、NUM_ROWS、TAG_WIDTH_M、TAG_SPACING_PERCENT是要设置的参数，含义见参考链接，我输入的指令为：
kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.02 --tspace 0.3
然后当前路径下就会出现target.pdf tmpENRhM0.tex tmpGUqWqP.tex这几个文件
标定时还需要编写target.yaml文件，如下，用来描述标定板的参数
target_type: 'aprilgrid' #gridtype
tagCols: 6 #number of apriltags
tagRows: 6 #number of apriltags
tagSize: 0.02 #size of apriltag, edge to edge [m]
tagSpacing: 0.3 #ratio of space between tags to tagSize
#example: tagSize=2m, spacing=0.5m --> tagSpacing=0.25[-]
- 录制相机的标定数据
使用如下命令发布图像话题，其中throttle用于将图像帧率降低到4FPS，再将新的图像话题改名为/color进行发布
roslaunch realsense2_camera rs_camera.launch
rosrun topic_tools throttle messages /camera/color/image_raw 4.0 /color
接下来录制rosbag，-O之后可以设置.bag文件的名字，使用Ctrl+C结束录制
mkdir bagfiles
cd bagfiles
rosbag record -O camd435i /color
- 标定单个相机
直接进行标定：
kalibr_calibrate_cameras --target ~/bagfiles/target.yaml --bag ~/bagfiles/camd435i.bag --models pinhole-radtan --topics /color --show-extraction
然后可能报错
Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance
百度找到一个解决方法，直接在末尾加上--approx-sync 0.04
但还是报错，继续百度
最终解决方法：https://github.com/ethz-asl/kalibr/issues/332
将src/Kalibr/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras中如下的代码注释掉
if not graph.isGraphConnected():
obsdb.printTable()
print "Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance."
graph.plotGraph()
sys.exit(-1)
虽然感觉有点不妥。。但没报错了，标定完成后当前路径下会出现一个.yaml文件
IMU与相机联合标定
- 录制标定数据
仍然使用之前的标定板，运行下面的命令发布图像和IMU话题
roslaunch realsense2_camera rs_camera.launch unite_imu_method:="linear_interpolation"
将图像频率设置为20Hz，IMU频率设置为200Hz：
rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
rosrun topic_tools throttle messages /camera/imu 200.0 /imu
录制rosbag
rosbag record -O dynamic /color /imu
- 开始标定
新建一个camchain.yaml文件，内容如下，将intrinsics和distortion_coeffs的数值改成之前标定的结果
cam0:
camera_model: pinhole
intrinsics: [637.1779587247433, 633.7804879350356, 317.29745787695197, 222.79306023564143]
distortion_model: equidistant
distortion_coeffs: [0.12699077558547853, -0.2663283480947607, -0.003966617209275872, 0.002919700067058629]
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文件，内容如下，将下面的四个参数的值设置为imu标定的结果。
rostopic: /imu
update_rate: 200.0 #Hz
accelerometer_noise_density: 0.030017 #continous
accelerometer_random_walk: 0.000876
gyroscope_noise_density: 0.00294 #continous
gyroscope_random_walk: 3.14268e-05
进行标定
kalibr_calibrate_imu_camera --target ~/bagfiles/target.yaml --cam ~/bagfiles/camchain.yaml --imu ~/bagfiles/imu.yaml --bag ~/bagfiles/dynamic.bag --show-extraction
标定结果如下：
Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.645979524406, median 0.55076768161, std: 0.441172957301
Gyroscope error (imu0): mean 0.632165457077, median 0.51915755434, std: 0.418834270707
Accelerometer error (imu0): mean 0.315519030434, median 0.26144132228, std: 0.220137274831
Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.645979524406, median 0.55076768161, std: 0.441172957301
Gyroscope error (imu0) [rad/s]: mean 0.026284098714, median 0.0215854698381, std: 0.017414240517
Accelerometer error (imu0) [m/s^2]: mean 0.133939243527, median 0.110983013877, std: 0.0934492604853
Transformation (cam0):
-----------------------
T_ci: (imu0 to cam0):
[[ 0.99914747 -0.03946207 -0.01212755 0.04307462]
[ 0.03965755 0.99907997 0.01632454 -0.01789784]
[ 0.0114722 -0.01679157 0.99979319 -0.01402533]
[ 0. 0. 0. 1. ]]
T_ic: (cam0 to imu0):
[[ 0.99914747 0.03965755 0.0114722 -0.04216722]
[-0.03946207 0.99907997 -0.01679157 0.01934568]
[-0.01212755 0.01632454 0.99979319 0.01483699]
[ 0. 0. 0. 1. ]]
timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.0302255306475
Gravity vector in target coords: [m/s^2]
[-0.48824837 -9.78365493 -0.45840216]
Calibration configuration
=========================
cam0
-----
Camera model: pinhole
Focal length: [637.1779587247433, 633.7804879350356]
Principal point: [317.29745787695197, 222.79306023564143]
Distortion model: equidistant
Distortion coefficients: [0.12699077558547853, -0.2663283480947607, -0.003966617209275872, 0.002919700067058629]
Type: aprilgrid
Tags:
Rows: 6
Cols: 6
Size: 0.02 [m]
Spacing 0.006 [m]
IMU configuration
=================
IMU0:
----------------------------
Model: calibrated
Update rate: 200.0
Accelerometer:
Noise density: 0.030017
Noise density (discrete): 0.424504485018
Random walk: 0.000876
Gyroscope:
Noise density: 0.00294
Noise density (discrete): 0.0415778787338
Random walk: 3.14268e-05
T_i_b
[[ 1. 0. 0. 0.]
[ 0. 1. 0. 0.]
[ 0. 0. 1. 0.]
[ 0. 0. 0. 1.]]
time offset with respect to IMU0: 0.0 [s]
参考
用imu_utils标定IMU
Ubuntu18.04+ROS-melodic下的kalibr安装编译
Kalibr标定Intel D435i相机