D435i标定摄像头和IMU教程四(RGB摄像头和IMU联合标定篇)
- 一、前提
- 二、准备标定文件
- 2.1 标定板yaml文件
- 2.2 相机标定yaml文件
- 2.3 imu标定yaml文件
- 2.4 录制rosbag文件
- 2.5 标定(运行时间很久很久)
- 2.6 结果
参考视频
D435i标定摄像头和IMU笔记一(配置环境篇)
D435i标定摄像头和IMU笔记二(RGB摄像头标定篇)
D435i标定摄像头和IMU笔记二-2(RGB+双目多摄像头标定篇)
D435i标定摄像头和IMU笔记三(IMU标定篇)
D435i标定摄像头和IMU笔记四(RGB摄像头和IMU联合标定篇)
D435i标定摄像头和IMU笔记四-2(双目摄像头与IMU联合标定篇)
一、前提
我们已经对RGB摄像头和IMU进行了单独标定。Kalibr官方WIKI。
二、准备标定文件
2.1 标定板yaml文件
前面用到的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 相机标定yaml文件
前面标定自动产生的camchain-camd435i.yaml,可以直接用。内容为:
cam0:
cam_overlaps: []
camera_model: pinhole
distortion_coeffs: [0.16212545911017717, -0.24080691480176616, -0.004308195576417379,
-0.0030299905015934016]
distortion_model: radtan
intrinsics: [611.3040953235151, 612.2272911356274, 313.980911449739, 237.49071739181224]
resolution: [640, 480]
rostopic: /color
2.3 imu标定yaml文件
需要根据IMU标定新建,参考D435i标定摄像头和IMU教程三(IMU标定篇)中的2.4。内容为:
rostopic: /imu
update_rate: 200.0 #Hz
gyroscope_noise_density: 3.0285738136227973e-03
gyroscope_random_walk: 3.0037305993425518e-05
accelerometer_noise_density: 2.7260051759783317e-02
accelerometer_random_walk: 5.7981798863344241e-04
2.4 录制rosbag文件
- 启动D435i:Ctrl+Alt+t:终端1
roslaunch realsense2_camera rs_camera.launch
- rviz查看视野:Ctrl+Shift+t:终端2
- 更改图像频率并转发为一个新话题:Ctrl+Shift+t:终端3
rosrun topic_tools throttle messages /camera/color/image_raw 20.0 /color
- 更改IMU频率并转发为一个新话题:Ctrl+Shift+t:终端4
rosrun topic_tools throttle messages /camera/imu 200.0 /imu
- 录制rosbag:Ctrl+Shift+t:终端5
rosbag record -O camer_imu /color /imu
PS:注意要对着标定板,确保能检测到角点,各个角度都录一下。三个轴,每个轴都来回三次。
2.5 标定(运行时间很久很久)
kalibr_calibrate_imu_camera --target checkerboard.yaml --cam camchain-camd435i.yaml --imu imu.yaml --bag camer_imu.bag
问题一:提示OverflowError: bad numeric conversion: positive overflow
解决办法:IMU数据获取有问题,我的是使用长线或者USB供电不足会出现这种问题。建议使用20cm左右的,好一点的或者原装数据线。
2.6 结果
生成了4个文件:
- (相机校正结果)Saving camera chain calibration to file: camchain-imucam-camer_imu_2.yaml
- (IMU校正结果)Saving imu calibration to file: imu-camer_imu_2.yaml
- (详细数据)Detailed results written to file: results-imucam-camer_imu_2.txt
- (联和标定报告)Report written to report-imucam-camer_imu_2.pdf
展示详细数据:
Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0): mean 0.283157878598, median 0.21921505662, std: 0.252071973755
Gyroscope error (imu0): mean 0.196325541059, median 0.0913923426892, std: 0.324390653502
Accelerometer error (imu0): mean 0.189152781802, median 0.16916940251, std: 0.118189680733
Residuals
----------------------------
Reprojection error (cam0) [px]: mean 0.283157878598, median 0.21921505662, std: 0.252071973755
Gyroscope error (imu0) [rad/s]: mean 0.00840872140414, median 0.00391437988149, std: 0.0138938144099
Accelerometer error (imu0) [m/s^2]: mean 0.0729213007089, median 0.065217401265, std: 0.0455639360274
Transformation (cam0):
-----------------------
T_ci: (imu0 to cam0):
[[ 0.99943583 0.03338307 -0.0036876 0.02106939]
[-0.03356636 0.9965747 -0.07557888 0.02139301]
[ 0.00115191 0.07566002 0.99713301 0.03351284]
[ 0. 0. 0. 1. ]]
T_ic: (cam0 to imu0):
[[ 0.99943583 -0.03356636 0.00115191 -0.02037802]
[ 0.03338307 0.9965747 0.07566002 -0.02455868]
[-0.0036876 -0.07557888 0.99713301 -0.03172221]
[ 0. 0. 0. 1. ]]
timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.00683194058101
Gravity vector in target coords: [m/s^2]
[-0.59332672 8.88845436 -4.10021528]
Calibration configuration
=========================
cam0
-----
Camera model: pinhole
Focal length: [611.3040953235151, 612.2272911356274]
Principal point: [313.980911449739, 237.49071739181224]
Distortion model: radtan
Distortion coefficients: [0.16212545911017717, -0.24080691480176616, -0.004308195576417379, -0.0030299905015934016]
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]
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)