一、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 <module>
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 <module>
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,意味着两个小时,如果数据录制时间超过两小时可以不用修改,如果不足,这个时间值要改为略小于真实时间。我的内容如下:
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<param name="imu_topic" type="string" value= "/zed2/zed_node/imu/data_raw"/>
<param name="imu_name" type="string" value= "ZED2"/>
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
<param name="max_time_min" type="int" value= "120"/>
<param name="max_cluster" type="int" value= "200"/>
</node>
</launch>
启动标定:
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