D455相机标定双目+imu（三）

最新推荐文章于 2025-04-28 15:02:40 发布

全日制一起混

最新推荐文章于 2025-04-28 15:02:40 发布

阅读量1.1k

点赞数 3

分类专栏： Inter Realsense vins-fusion 文章标签：数码相机计算机视觉 ubuntu

本文链接：https://blog.csdn.net/weixin_62952541/article/details/131657699

版权

Inter Realsense 同时被 2 个专栏收录

7 篇文章

订阅专栏

vins-fusion

7 篇文章

订阅专栏

三、双目+imu联合标定

3.1 准备文件

首先需要我们在前面已经对imu和双目进行标定后，需要以下文件：

①IMU标定结果：imu.yaml，参考IMU标定最后的结果

rostopic: /imu
update_rate: 200.0 #Hz
 
gyroscope_noise_density: 2.8260834273707305e-03
gyroscope_random_walk: 1.0524392205451450e-04

accelerometer_noise_density: 1.2291998294860730e-02
accelerometer_random_walk: 2.9433108833622326e-04

②双目标定结果：camchain-mult_cam_d455.yaml：

cam0:
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0.002673370630508643, -0.0010681327232295733, -0.00011528798569030352, 0.0010258669746751625]
  distortion_model: radtan
  intrinsics: [392.2211560299197, 392.77289841050305, 324.0902022905199, 243.14325635089884]
  resolution: [640, 480]
  rostopic: /infra_left
cam1:
  T_cn_cnm1:
  - [0.9999966624049934, -2.3483562378505667e-05, 0.0025835300252614437, -0.09512359120334894]
  - [2.373051383946311e-05, 0.9999999951529331, -9.555622327801639e-05, -6.826233219685323e-05]
  - [-0.002583527768738377, 9.561721284506207e-05, 0.999996658115222, -0.00012114470379891623]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [0.0049879872955638106, -0.006297264333934392, 2.7859317032933206e-05, 0.00021011678954185143]
  distortion_model: radtan
  intrinsics: [392.1074674941126, 392.6624570821124, 323.337632607508, 243.20458242799964]
  resolution: [640, 480]
  rostopic: /infra_right

③标定板文件checkerboard.yaml：

target_type: 'checkerboard' #gridtype
targetCols: 8               #number of internal chessboard corners
targetRows: 5               #number of internal chessboard corners
rowSpacingMeters: 0.030      #size of one chessboard square [m]
colSpacingMeters: 0.030      #size of one chessboard square [m]

3.2 打开相机录制话题

新建一个stereo-imu.launch文件，相关话题修改一下：

  <arg name="infra_width"         default="640"/>
  <arg name="infra_height"        default="480"/>
  <arg name="enable_infra"        default="true"/>
  <arg name="enable_infra1"       default="true"/>
  <arg name="enable_infra2"       default="true"/>
 
  <arg name="infra_fps"           default="30"/>
 
  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="100"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>
 
  <arg name="enable_sync"               default="true"/>

  <arg name="unite_imu_method" default="linear_interpolation"/>

然后打开相机：

roslaunch realsense2_camera stereo-imu.launch

关闭结构光

rosrun rqt_reconfigure rqt_reconfigure

将emitter_enable设置为off

改变话题hz

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

开始录制：

rosbag record -O imu_stereo.bag /infra_left /infra_right /imu

相机的移动如下：首先前后、左右、上下各两个来回，然后俯仰、滚转、偏航三个方向两个来回，注意移动不要太快、需要保证标定板始终出现在图像中

3.3 开始标定

将bag、checkerboard.yaml、camchain-mult_cam_d455.yaml 、 imu.yaml放在kalibr_ws路径下：

cd kalibr_ws
source devel/setup.bash
rosrun kalibr kalibr_calibrate_imu_camera --target checkerboard.yaml --cam camchain-mult_cam_d435i.yaml --imu imu.yaml --bag imu_stereo.bag

标定时间较长，耐心等待~

3.4 标定结果：

Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.0419733797326, median 0.0392126054829, std: 0.022383316784
Reprojection error (cam1):     mean 0.0406356612021, median 0.03807646421, std: 0.0214455082781
Gyroscope error (imu0):        mean 0.0520007330069, median 0.0448516098862, std: 0.0360617568009
Accelerometer error (imu0):    mean 0.0600520977099, median 0.0473350151808, std: 0.0691175307239

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.0419733797326, median 0.0392126054829, std: 0.022383316784
Reprojection error (cam1) [px]:     mean 0.0406356612021, median 0.03807646421, std: 0.0214455082781
Gyroscope error (imu0) [rad/s]:     mean 0.0020783057619, median 0.00179257779394, std: 0.00144127500921
Accelerometer error (imu0) [m/s^2]: mean 0.0104391628293, median 0.00822848742751, std: 0.0120150533471

Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0): 
[[ 0.99994541 -0.00427543 -0.00953449  0.02981493]
 [ 0.00421397  0.99997028 -0.00645616 -0.00113302]
 [ 0.00956181  0.00641563  0.9999337  -0.01486287]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[ 0.99994541  0.00421397  0.00956181 -0.02966641]
 [-0.00427543  0.99997028  0.00641563  0.00135581]
 [-0.00953449 -0.00645616  0.9999337   0.01513884]
 [ 0.          0.          0.          1.        ]]

timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.0018070987594530142


Transformation (cam1):
-----------------------
T_ci:  (imu0 to cam1): 
[[ 0.99996667 -0.00428232 -0.00695095 -0.06534713]
 [ 0.00423679  0.99996956 -0.00655194 -0.00119915]
 [ 0.00697879  0.00652227  0.99995438 -0.0150611 ]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam1 to imu0): 
[[ 0.99996667  0.00423679  0.00697879  0.06545514]
 [-0.00428232  0.99996956  0.00652227  0.00101751]
 [-0.00695095 -0.00655194  0.99995438  0.01459833]
 [ 0.          0.          0.          1.        ]]

timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
-0.0018085345916211656

Baselines:
----------
Baseline (cam0 to cam1): 
[[ 0.99999666 -0.00002348  0.00258353 -0.09512359]
 [ 0.00002373  1.         -0.00009556 -0.00006826]
 [-0.00258353  0.00009562  0.99999666 -0.00012114]
 [ 0.          0.          0.          1.        ]]
baseline norm:  0.0951236928383623 [m]


Gravity vector in target coords: [m/s^2]
[ 9.73473346 -0.10243336 -1.18020963]


Calibration configuration
=========================

cam0
-----
  Camera model: pinhole
  Focal length: [392.2211560299197, 392.77289841050305]
  Principal point: [324.0902022905199, 243.14325635089884]
  Distortion model: radtan
  Distortion coefficients: [0.002673370630508643, -0.0010681327232295733, -0.00011528798569030352, 0.0010258669746751625]
  Type: checkerboard
  Rows
    Count: 5
    Distance: 0.032 [m]
  Cols
    Count: 8
    Distance: 0.032 [m]

cam1
-----
  Camera model: pinhole
  Focal length: [392.1074674941126, 392.6624570821124]
  Principal point: [323.337632607508, 243.20458242799964]
  Distortion model: radtan
  Distortion coefficients: [0.0049879872955638106, -0.006297264333934392, 2.7859317032933206e-05, 0.00021011678954185143]
  Type: checkerboard
  Rows
    Count: 5
    Distance: 0.032 [m]
  Cols
    Count: 8
    Distance: 0.032 [m]



IMU configuration
=================

IMU0:
 ----------------------------
  Model: calibrated
  Update rate: 200.0
  Accelerometer:
    Noise density: 0.0122919982949 
    Noise density (discrete): 0.173835106973 
    Random walk: 0.000294331088336
  Gyroscope:
    Noise density: 0.00282608342737
    Noise density (discrete): 0.0399668551139 
    Random walk: 0.000105243922055
  T_ib (imu0 to imu0)
    [[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]