D435i标定摄像头和IMU笔记四-2（双目摄像头与IMU联合标定篇）
参考视频
D435i标定摄像头和IMU笔记一（配置环境篇）
D435i标定摄像头和IMU笔记二（RGB摄像头标定篇）
D435i标定摄像头和IMU笔记二-2（RGB+双目多摄像头标定篇）
D435i标定摄像头和IMU笔记三（IMU标定篇)
D435i标定摄像头和IMU笔记四（RGB摄像头和IMU联合标定篇)
D435i标定摄像头和IMU笔记四-2（双目摄像头与IMU联合标定篇）
一、前提
已经完成了对IMU和双目摄像头的标定工作。
二、文件准备
2.1 标定板文件checkerboard.yaml（前面多次用到）：
target_type: 'checkerboard' #gridtype
targetCols: 5 #number of internal chessboard corners
targetRows: 8 #number of internal chessboard corners
rowSpacingMeters: 0.045 #size of one chessboard square [m]
colSpacingMeters: 0.045 #size of one chessboard square [m]
2.2 imu标定结果imu.yaml（见IMU标定）
rostopic: /imu
update_rate: 200.0 #Hz
gyroscope_noise_density: 0.00302857381362
gyroscope_random_walk: 3.00373059934e-05
accelerometer_noise_density: 0.0272600517598
accelerometer_random_walk: 0.000579817988633
2.3 双目标定文件camchain-mult_cam_d435i.yaml：
前面标定的结果。
cam0:
cam_overlaps: [1]
camera_model: pinhole
distortion_coeffs: [0.011747122541878768, -0.02234859898241525, 0.0020672667698299592,
-0.0008918010867833909]
distortion_model: radtan
intrinsics: [386.8409992331975, 388.24690742880813, 316.99396938084436, 241.81883369051474]
resolution: [640, 480]
rostopic: /infra_left
cam1:
T_cn_cnm1:
- [0.9999996711225254, -0.00023908758604951694, 0.0007749786892121749, -0.05020391636821169]
- [0.00024036158003728526, 0.9999986193621923, -0.0016442323503307573, 9.805698051707789e-05]
- [-0.0007745845037037506, 0.0016444180846820074, 0.9999983479526409, 0.0008601052990752862]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0]
camera_model: pinhole
distortion_coeffs: [0.01574357771516093, -0.027360749803476046, 0.0020895910198554654,
-0.000809614206569399]
distortion_model: radtan
intrinsics: [387.0690438082868, 388.49581611179184, 317.0317843214349, 242.59905600124543]
resolution: [640, 480]
rostopic: /infra_right
三、标定准备
- 启动realsense_ros
roslaunch realsense2_camera rs_camera.launch
如果是t265:
roslaunch realsense2_camera rs_t265.launch
- 关闭结构光
rosrun rqt_reconfigure rqt_reconfigure
将camera->stereo_module下的emitter_enable设置为off即可。
- 改变topic发布频率
分开设置：
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left
rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right
rosrun topic_tools throttle messages /camera/imu 200.0 /imu
或者合并：
rosrun topic_tools throttle messages /camera/infra1/image_rect_raw 20.0 /infra_left & rosrun topic_tools throttle messages /camera/infra2/image_rect_raw 20.0 /infra_right & rosrun topic_tools throttle messages /camera/imu 200.0 /imu
如果是t265:
rosrun topic_tools throttle messages /camera/fisheye1/image_raw 20.0 /fisheye1 & rosrun topic_tools throttle messages /camera/fisheye2/image_raw 20.0 /fisheye2 & rosrun topic_tools throttle messages /camera/imu 200.0 /imu
rviz查看
先把左上角的Fixed Frame设置为camera_link，然后左下角add>By topic>/infra1/imgage_rect_raw下双击camera，其他的同理。开始录制
cd ~/calibr_data/mii-d435i/imu_stereo_cali
rosbag record -O imu_stereo.bag /infra_left /infra_right /imu
如果是t265:
rosbag record -O imu_stereo.bag /fisheye1 /fisheye2 /imu
注意：标定板不动，相机围绕三个轴两个来回。
四、开始标定(时间很久)
kalibr_calibrate_imu_camera --target checkerboard.yaml --cam camchain-mult_cam_d435i.yaml --imu imu.yaml --bag imu_stereo.bag
如果是t265:
kalibr_calibrate_imu_camera --target apriltags.yaml --cam camchain-camer_t265.yaml --imu imu.yaml --bag imu_stereo.bag
五、标定结果
- camchain-imucam-imu_stereo.yaml
- imu-imu_stereo.yaml
- report-imucam-imu_stereo.pdf
- results-imucam-imu_stereo.txt
results-imucam-imu_stereo.txt:
Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.0439184893999, median 0.0404529025861, std: 0.0244014493806
Reprojection error (cam1): mean 0.0432418328153, median 0.0403828353072, std: 0.0231110306752
Gyroscope error (imu0): mean 0.10758968237, median 0.0803879270766, std: 0.110705523637
Accelerometer error (imu0): mean 0.149117631029, median 0.102847730683, std: 0.174658929541
Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.0439184893999, median 0.0404529025861, std: 0.0244014493806
Reprojection error (cam1) [px]: mean 0.0432418328153, median 0.0403828353072, std: 0.0231110306752
Gyroscope error (imu0) [rad/s]: mean 0.00460812006491, median 0.00344305524078, std: 0.00474157310933
Accelerometer error (imu0) [m/s^2]: mean 0.0574871355826, median 0.0396493787981, std: 0.067333698195
Transformation (cam0):
-----------------------
T_ci: (imu0 to cam0):
[[ 0.99998098 0.00447156 0.00424803 0.00057372]
[-0.00445265 0.99998019 -0.00444882 -0.02441941]
[-0.00426784 0.00442983 0.99998108 -0.02857855]
[ 0. 0. 0. 1. ]]
T_ic: (cam0 to imu0):
[[ 0.99998098 -0.00445265 -0.00426784 -0.00080441]
[ 0.00447156 0.99998019 0.00442983 0.02454296]
[ 0.00424803 -0.00444882 0.99998108 0.02846694]
[ 0. 0. 0. 1. ]]
timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.0082512730446
Transformation (cam1):
-----------------------
T_ci: (imu0 to cam1):
[[ 0.99997651 0.00445873 0.00520493 -0.04960042]
[-0.00443971 0.99998345 -0.00365951 -0.02425743]
[-0.00522116 0.00363632 0.99997976 -0.02789638]
[ 0. 0. 0. 1. ]]
T_ic: (cam1 to imu0):
[[ 0.99997651 -0.00443971 -0.00522116 0.04934591]
[ 0.00445873 0.99998345 0.00363632 0.02457962]
[ 0.00520493 -0.00365951 0.99997976 0.02806522]
[ 0. 0. 0. 1. ]]
timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
-0.00825230706853
Baselines:
----------
Baseline (cam0 to cam1):
[[ 0.99999954 -0.00001707 0.00095684 -0.05014721]
[ 0.00001631 0.99999969 0.00078926 0.00018452]
[-0.00095686 -0.00078924 0.99999923 0.00066342]
[ 0. 0. 0. 1. ]]
baseline norm: 0.0501519377303 [m]
Gravity vector in target coords: [m/s^2]
[ 0.08686023 9.76477809 -0.89999282]
Calibration configuration
=========================
cam0
-----
Camera model: pinhole
Focal length: [380.9803345158986, 382.154386188639]
Principal point: [318.93609497258376, 243.05040693225402]
Distortion model: radtan
Distortion coefficients: [0.00590432014031773, -0.0038710343988133944, 0.002729676970436722, 0.0012954618097941984]
Type: checkerboard
Rows
Count: 8
Distance: 0.045 [m]
Cols
Count: 5
Distance: 0.045 [m]
cam1
-----
Camera model: pinhole
Focal length: [381.2139619547049, 382.4422285681291]
Principal point: [318.87898121166836, 242.8507951458829]
Distortion model: radtan
Distortion coefficients: [0.005132541874724565, -0.0023236481550731013, 0.0020455150943863563, 0.0007867398316885759]
Type: checkerboard
Rows
Count: 8
Distance: 0.045 [m]
Cols
Count: 5
Distance: 0.045 [m]
IMU configuration
=================
IMU0:
----------------------------
Model: calibrated
Update rate: 200.0
Accelerometer:
Noise density: 0.0272600517598
Noise density (discrete): 0.385515349097
Random walk: 0.000579817988633
Gyroscope:
Noise density: 0.00302857381362
Noise density (discrete): 0.0428305016187
Random walk: 3.00373059934e-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]
camchain-imucam-imu_stereo.yaml:
cam0:
T_cam_imu:
- [0.9999809795447093, 0.004471557180217604, 0.0042480260342685995, 0.0005737184041461529]
- [-0.004452654569842776, 0.9999801907164122, -0.004448824794217874, -0.0244194144180026]
- [-0.004267835058368314, 0.004429825183010482, 0.9999810809374159, -0.028578553859411042]
- [0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
- [0.9999060579801039, 0.006187309239286005, -0.012230797973450003, -0.012504540979490943]
- [-0.006157939420205711, 0.9999780689386636, 0.0024375035998556514, 0.00041303794737816035]
- [0.012245611327613627, -0.002361958102862696, 0.9999222300544849, 0.008441266367516128]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0, 2]
camera_model: pinhole
distortion_coeffs: [0.00590432014031773, -0.0038710343988133944, 0.002729676970436722,
0.0012954618097941984]
distortion_model: radtan
intrinsics: [380.9803345158986, 382.154386188639, 318.93609497258376, 243.05040693225402]
resolution: [640, 480]
rostopic: /infra_left
timeshift_cam_imu: -0.008251273044604341
cam1:
T_cam_imu:
- [0.9999765139711361, 0.004458728157107288, 0.005204925489877297, -0.049600420389517876]
- [-0.004439711145172791, 0.9999834483339518, -0.0036595113179214724, -0.024257430395766763]
- [-0.005221156105843253, 0.0036363170048258303, 0.9999797581589133, -0.027896383573982254]
- [0.0, 0.0, 0.0, 1.0]
T_cn_cnm1:
- [0.9999995420794534, -1.7065963343667133e-05, 0.0009568435799698137, -0.050147210065992716]
- [1.6310769299760354e-05, 0.999999688403046, 0.0007892577334999778, 0.00018452290007793878]
- [-0.0009568567512638173, -0.0007892417652277561, 0.999999230761001, 0.0006634244462778601]
- [0.0, 0.0, 0.0, 1.0]
cam_overlaps: [0, 1]
camera_model: pinhole
distortion_coeffs: [0.005132541874724565, -0.0023236481550731013, 0.0020455150943863563,
0.0007867398316885759]
distortion_model: radtan
intrinsics: [381.2139619547049, 382.4422285681291, 318.87898121166836, 242.8507951458829]
resolution: [640, 480]
rostopic: /infra_right
timeshift_cam_imu: -0.008252307068533518
imu-imu_stereo.yaml:
imu0:
T_i_b:
- [1.0, 0.0, 0.0, 0.0]
- [0.0, 1.0, 0.0, 0.0]
- [0.0, 0.0, 1.0, 0.0]
- [0.0, 0.0, 0.0, 1.0]
accelerometer_noise_density: 0.0272600517598
accelerometer_random_walk: 0.000579817988633
gyroscope_noise_density: 0.00302857381362
gyroscope_random_walk: 3.00373059934e-05
model: calibrated
rostopic: /imu
time_offset: 0.0
update_rate: 200.0
版权声明：本文为qq_35616298原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接和本声明。