ZED2相机标定及运行VINS-mono
一、ZED2相机+IMU标定
1、标定工具安装
Kalibr的安装参见我的另外一篇博客Kalibr安装及相机+IMU标定
（需要注意的是Kalibr的安装需要网络能用google,否则网络问题会导致安装不成功，如需可以找我copy相关安装所需文件）
接下来是imu_utils的安装，参考github上的安装要求即可，且依赖于code_utils，需要先安装code_utils，然后创建catkin工作空间，必须先把code_utils放进去catkin_make,然后再把imu_utils文件放入工作空间中catkin_make,否则会报错找不到code_utils。
2、ZED2标定数据录制
我根据需求修改了ZED2的分辨率，在ZED2_WS/src/zed-ros-wrapper/zed_wrapper/params文件夹下找到common.yaml，设置resolution为3，即VGA模式，实际分辨率大小为672*376.
然后打开ZED2相机开启数据录制:
roslaunch zed_wrapper zed2.launch
启用左右摄像头可视化功能，以确保将标定板保持在相机范围内。
rosrun image_view image_view image:=/zed2/zed_node/left/image_rect_color & rosrun image_view image_view image:=/zed2/zed_node/right/image_rect_color
kalibr在处理标定数据的时候要求图像的频率不可过高，降低图像数据到20HZ，IMU数据至200HZ.
rosrun topic_tools throttle messages /zed2/zed_node/imu/data_raw 200 /zed2/zed_node/imu/data_raw2
rosrun topic_tools throttle messages /zed2/zed_node/left/image_rect_color 20 /zed2/zed_node/left/image_rect_color2
rosrun topic_tools throttle messages /zed2/zed_node/right/image_rect_color 20 /zed2/zed_node/right/image_rect_color2
录制数据
rosbag record -O Kalib_data_vga.bag /zed2/zed_node/imu/data_raw2 /zed2/zed_node/left/image_rect_color2 /zed2/zed_node/right/image_rect_color2
3、开始相机标定
这里使用的是棋盘格标定板，对应的checkboard.yaml文件内容为：
target_type: 'checkerboard' #gridtype
targetCols: 8 #number of internal chessboard corners
targetRows: 11 #number of internal chessboard corners
rowSpacingMeters: 0.03 #size of one chessboard square [m]
colSpacingMeters: 0.03 #size of one chessboard square [m]
执行标定：
单目情况
kalibr_calibrate_cameras --bag /home/ipsg/ZED2_WS/Kalib_data_vga.bag --topics /zed2/zed_node/left/image_rect_color2 --models pinhole-radtan --target /home/ipsg/dataset/checkboard.yaml --bag-from-to 5 140 --show-extraction --approx-sync 0.04
双目情况：
kalibr_calibrate_cameras --bag /home/ipsg/ZED2_WS/Kalib_data_vga.bag --topics /zed2/zed_node/left/image_rect_color2 /zed2/zed_node/right/image_rect_color2 --models pinhole-radtan pinhole-radtan --target /home/ipsg/dataset/checkboard.yaml --bag-from-to 5 140 --show-extraction --approx-sync 0.04
左目标定结果为：
cam0:
cam_overlaps: []
camera_model: pinhole
distortion_coeffs: [-0.04146142113155779, 0.040207379741452436, 0.00058191919155336,
2.6184399452903757e-05]
distortion_model: radtan
intrinsics: [264.1544476767924, 265.6303085861438, 357.12188690735223, 167.47999407549506]
resolution: [672, 376]
rostopic: /zed2/zed_node/left/image_rect_color2
注意：
在最初运行标定时有报错，一直卡在提取角点这一步
Initializing cam0:
Camera model: pinhole-radtan
Dataset: Kalib_data_vga.bag
Topic: /zed2/zed_node/left/image_rect_color2
Number of images: 1980
Extracting calibration target corners
Progress 19 / 1980 Time remaining: 9m
解决办法为：
单、双目：
修改kalibr_calibrate_cameras.py文件中的多线程标签multithreading=multithreading改为multithreading=False
observations = kc.extractCornersFromDataset(cam.dataset, cam.ctarget.detector,
multithreading=False, clearImages=False,
noTransformation=True)
联合标定：
修改IccSensors.py文件中的多线程标签multithreading=multithreading改为multithreading=False
self.targetObservations = kc.extractCornersFromDataset(self.dataset, self.detector, multithreading=False)
在执行单目标定、单目+IMU联合表定时会报错如下：
Cameras are not connected through mutual observations, please check the dataset. Maybe adjust the approx. sync. tolerance.
Traceback (most recent call last):
File "/home/ipsg/tool/kalibr_ws/devel/bin/kalibr_calibrate_cameras", line 15, in
exec(compile(fh.read(), python_script, 'exec'), context)
File "/home/ipsg/tool/kalibr_ws/src/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras", line 447, in
main()
File "/home/ipsg/tool/kalibr_ws/src/aslam_offline_calibration/kalibr/python/kalibr_calibrate_cameras", line 204, in main
graph.plotGraph()
File "/home/ipsg/tool/kalibr_ws/src/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/MulticamGraph.py", line 311, in plotGraph
edge_label=self.G.es["weight"],
KeyError: 'Attribute does not exist'
解决办法：
找到工作空间下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)
注释掉之后便可以得出标定结果。
修改以上信息之后，便可以顺利运行。
4、IMU参数标定
单独录制IMU数据，数据包录制我录制了两个多小时，录制过程中必须保持相机静止不动。
rosbag record -O imu_calibration /zed2/zed_node/imu/data_raw
根据imu_utils文件夹里面的A3.launch改写ZED2标定启动文件：ZED2_calibration.launch注意，max_time_min对应的参数，默认是120，意味着两个小时，如果数据录制时间超过两小时可以不用修改，如果不足，这个时间值要改为略小于真实时间。我的内容如下：
启动标定：
roslaunch imu_utils ZED2_calibration.launch
回访数据包，以200Hz的速率回放：
rosbag play -r 200 imu_calibration.bag
最后可以得到标定结果文件：/home/ipsg/tool/utils_ws/src/imu_utils/data/ZED2_imu_param.yaml
%YAML:1.0
---
type: IMU
name: ZED2
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 1.8222305208357593e-03
gyr_w: 3.7133721747382378e-05
x-axis:
gyr_n: 2.1821037744825752e-03
gyr_w: 4.3929681760916831e-05
y-axis:
gyr_n: 1.6623543812424751e-03
gyr_w: 4.1416410773020793e-05
z-axis:
gyr_n: 1.6222334067822277e-03
gyr_w: 2.6055072708209496e-05
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 1.9690445544535126e-02
acc_w: 5.2966882234280319e-04
x-axis:
acc_n: 2.0618609665773655e-02
acc_w: 5.4447136705058940e-04
y-axis:
acc_n: 1.7485637447877407e-02
acc_w: 6.3371577751311896e-04
z-axis:
acc_n: 2.0967089519954317e-02
acc_w: 4.1081932246470108e-04
5、执行相机+IMU联合标定
利用步骤2中录制的数据包，执行标定，需要准备cam.yaml及imu.yaml文件，cam.yaml为单双目输出的标定文件，本次标定仅对左目标定，使用如下：
cam0:
cam_overlaps: []
camera_model: pinhole
distortion_coeffs: [-0.04146142113155779, 0.040207379741452436, 0.00058191919155336,
2.6184399452903757e-05]
distortion_model: radtan
intrinsics: [264.1544476767924, 265.6303085861438, 357.12188690735223, 167.47999407549506]
resolution: [672, 376]
rostopic: /zed2/zed_node/left/image_rect_color2
imu.yaml信息由步骤4中的IMU标定结果得出，取标定结果Acc及Gyr的平均值填入imu.yaml文件，得如下内容：
#Accelerometers
accelerometer_noise_density: 1.96e-02 #Noise density (continuous-time)
accelerometer_random_walk: 5.29e-04 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 1.82e-03 #Noise density (continuous-time)
gyroscope_random_walk: 3.71e-05 #Bias random walk
rostopic: /zed2/zed_node/imu/data_raw2 #the IMU ROS topic
update_rate: 200.0 #Hz (for discretization of the values above)
除了使用自己标定的IMU参数信息，也可以直接使用官网上提供的该参数，经验证，标定过后也是可以在系统中运行的，其给定的参数为（IMU rate及topic需要修改）：
#Accelerometers
accelerometer_noise_density: 1.4e-03 #Noise density (continuous-time)
accelerometer_random_walk: 8.0e-05 #Bias random walk
#Gyroscopes
gyroscope_noise_density: 8.6e-05 #Noise density (continuous-time)
gyroscope_random_walk: 2.2e-06 #Bias random walk
rostopic: /zed2/zed_node/imu/data_raw #the IMU ROS topic
update_rate: 400.0 #Hz (for discretization of the values above)
执行联合标定：
kalibr_calibrate_imu_camera \
--target /home/ipsg/dataset/checkboard.yaml \
--bag /home/ipsg/ZED2_WS/Kalib_data_vga.bag \
--bag-from-to 10 130 \
--cam /home/ipsg/tool/kalibr_ws/cam.yaml \
--imu /home/ipsg/tool/kalibr_ws/imu.yaml \
--imu-models scale-misalignment \
--timeoffset-padding 0.1
可得标定结果为camchain-imucam-homeipsgZED2_WSKalib_data_vga.yaml，如下：
cam0:
T_cam_imu:
- [0.010461959926441555, -0.9997613415836004, -0.019178302048279805, 0.0261061220321363]
- [-0.04841582739273925, 0.01865039796109419, -0.9986531281249599, 0.004580655437468762]
- [0.9987724741162884, 0.011376402368514227, -0.04820915283196389, -0.06740069267783835]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: []
camera_model: pinhole
distortion_coeffs: [-0.04146142113155779, 0.040207379741452436, 0.00058191919155336,
2.6184399452903757e-05]
distortion_model: radtan
intrinsics: [264.1544476767924, 265.6303085861438, 357.12188690735223, 167.47999407549506]
resolution: [672, 376]
rostopic: /zed2/zed_node/left/image_rect_color2
timeshift_cam_imu: 0.001248879099353284
二、使用ZED2运行VINS-mono
修改realsense_color_config.yaml文件
1、订阅topics修改
imu_topic: "/zed2/zed_node/imu/data_raw"
image_topic: "/zed2/zed_node/left/image_rect_color"
2、左目相机内参修改
model_type: PINHOLE
camera_name: camera
image_width: 672
image_height: 376
distortion_parameters:
k1: 0
k2: 0
p1: 0
p2: 0
projection_parameters:
fx: 264.154447
fy: 265.630308
cx: 357.121886
cy: 167.479994
这里使用的是校正后的图像，故设置畸变系数均为0；
3、IMU至cam的变换矩阵，参数修改为2，使用在线标定（设置为0，使用已有的标定参数也是可以运行的）：
# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 2 # 0 Have an accurate extrinsic parameters. We will trust the following imu^R_cam, imu^T_cam, don't change it.
# 1 Have an initial guess about extrinsic parameters. We will optimize around your initial guess.
# 2 Don't know anything about extrinsic parameters. You don't need to give R,T. We will try to calibrate it. Do some rotation movement at beginning.
#If you choose 0 or 1, you should write down the following matrix.
4、IMU参数，使用VINS-mono中给定的参数
#imu parameters The more accurate parameters you provide, the better performance
acc_n: 0.2 # accelerometer measurement noise standard deviation. #0.2
gyr_n: 0.05 # gyroscope measurement noise standard deviation. #0.05
acc_w: 0.02 # accelerometer bias random work noise standard deviation. #0.02
gyr_w: 4.0e-5 # gyroscope bias random work noise standard deviation. #4.0e-5
g_norm: 9.80 # gravity magnitude
5、不需要在线估计同步时差
#unsynchronization parameters
estimate_td: 0 # online estimate time offset between camera and imu
td: 0.000 # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)
6、相机改为全局曝光
#rolling shutter parameters
rolling_shutter: 0 # 0: global shutter camera, 1: rolling shutter camera
rolling_shutter_tr: 0 # unit: s. rolling shutter read out time per frame (from data sheet).
7、开始运行
roslaunch zed_wrapper zed2.launch
roslaunch vins_estimator realsense_live.launch
roslaunch vins_estimator vins_rviz.launch
结果如下：
参考链接：
https://blog.csdn.net/u011178262/article/details/83316968
https://blog.csdn.net/fb_941219/article/details/104709049
https://blog.csdn.net/u010590316/article/details/89297324
https://zhuanlan.zhihu.com/p/268825840
https://blog.csdn.net/weixin_44580210/article/details/89789416