d435i 标定 imu和相机 用来复现vins_fusion

本文主要为自用，会省略一些众所周知的细节。标定程序的安装可以参考下面我的步骤，vins_fusion暂时只复现了双目+IMU，可跳转至"双目相机标定"部分进行参考

一、安装imu_utils

1、安装absl

git clone https://github.com/abseil/abseil-cpp
cd abseil-cpp
mkdir build && cd build
cmake -DCMAKE_INSTALL_PREFIX=/usr/local/absl ..  # 指定安装路径
make -j8

下面这步可能有问题，提示

cmake -DCMAKE_INSTALL_PREFIX=/usr/local/absl ...
解决：在CMakeLists.txt加入

set(CMAKE_CXX_STANDARD 17)

加入后先把build文件夹中的内容清空，再cmake

也有不少博主采用如下方案解决，我这么处理会报错
其他解决方案

2、安装ceres

参考链接：解决方案

#1. 下载ceres2.0版本
# 地址为 http://ceres-solver.org/ceres-solver-2.0.0.tar.gz
#2. 解压
#3. 进入目录，然后编译
cd ceres-solver
mkdir build && cd build 
cmake ..
make -j3
sudo make install

3、安装code_util

git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make

1、报backward.hpp错误

修改code_utils功能包下sumpixel_test.cpp中 #include "backward.hpp"为 #include <code_utils/backward.hpp>

2、elfutils/libdw.h 找不到

报错

/home/lzy/catkin_ws/src/code_utils/include/code_utils/backward.hpp:216:12: fatal error: elfutils/libdw.h: 没有那个文件或目录

216 | # include <elfutils/libdw.h>
解决：

sudo apt-get install libdw-dev

3、报一堆错误

进入CMakeLists.txt中将

set(CMAKE_CXX_FLAGS "-std=c++11")

修改成：

set(CMAKE_CXX_FLAGS "-std=c++14")

4、CV方面报错
CV_MINMAX修改为NORM_MINMAX

CV_LOAD_IMAGE_GRAYSCALE改为IMREAD_GRAYSCALE

CV_LOAD_IMAGE_UNCHANGED改为IMREAD_UNCHANGED

参考链接：参考

4、安装imu_utils

git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make

1、报错一堆，同code_utils一样

进入CMakeLists.txt中将

set(CMAKE_CXX_FLAGS "-std=c++11")

修改成：

set(CMAKE_CXX_FLAGS "-std=c++14")

2、报错如下

/home/nvidia/catkin_ws/src/imu_utils/src/imu_an.cpp: In function 'void writeData1(std::string, const std::vector&, const std::vector&)':

/home/nvidia/catkin_ws/src/imu_utils/src/imu_an.cpp:68:19: error: aggregate 'std::ofstream out_t' has incomplete type and cannot be defined

68 | std::ofstream out_t;

| ^~~~~

解决：在imu_an.cpp中加入

#include <fstream>

5、imu_utils标定流程

标定参考流程

二、相机标定

1、Kalibr安装

官方安装：https://github.com/ethz-asl/kalibr/wiki/installation

采用源码方式安装，

报错：

解决：

pip install --upgrade importlib_metadata

生成棋盘

rosrun kalibr kalibr_create_target_pdf --type checkerboard --nx 5 --ny 8 --csx 0.045 --csy 0.045

运行单目标定

rosrun camera_calibration cameracalibrator.py --size 5x8 --square 0.045 image:=/camera/color/image_raw

标定结果

image_height: 480
camera_name: narrow_stereo
camera_matrix:
  rows: 3
  cols: 3
  data: [613.56325,   0.     , 327.91613,
           0.     , 614.50013, 241.53748,
           0.     ,   0.     ,   1.     ]
# fx = 613.56325: x轴焦距（像素单位）
# fy = 614.50013: y轴焦距（像素单位）
# cx = 327.91613: 主点x坐标（光学中心）
# cy = 241.53748: 主点y坐标（光学中心）

distortion_model: plumb_bob

#畸变系数
distortion_coefficients:
  rows: 1
  cols: 5
  data: [0.129330, -0.259850, 0.000048, 0.001920, 0.000000]
# k1 = 0.129330: 径向畸变系数1
# k2 = -0.259850: 径向畸变系数2
# p1 = 0.000048: 切向畸变系数1
# p2 = 0.001920: 切向畸变系数2
# k3 = 0.000000: 径向畸变系数3

rectification_matrix:
  rows: 3
  cols: 3
  data: [1., 0., 0.,
         0., 1., 0.,
         0., 0., 1.]

# 投影矩阵         
projection_matrix:
  rows: 3
  cols: 4
  data: [623.26678,   0.     , 328.88422,   0.     ,
           0.     , 625.06627, 241.5653 ,   0.     ,
           0.     ,   0.     ,   1.     ,   0.     ]

单目相机和IMU联合标定

参考链接

生成棋盘

rosrun kalibr kalibr_create_target_pdf --type apriltag --nx 6 --ny 6 --tsize 0.022 --tspace 0.3

标定

rosrun kalibr kalibr_calibrate_imu_camera --target checkerboard.yaml --cam camchain.yaml --imu imu.yaml --bag d435i.bag --bag-from-to 5 45 --show-extraction

标定时的图像

双目相机标定

标定参考链接

为了复现VINS_Fusion，标定时需要使用pinhole-radtan模型进行标定，我一开始使用pinhole-equi，导致误差大了一个量级，在运行双目VINS_Fusion时会出现极快速度的"瞬移"，都不能称之为漂移了。

1、生成标定板，可以参考上方生成棋盘标定板的指令。这里我用实验室现成的标定板

新建new_checkboard.yaml文件，根据生成棋盘格子情况设置，0.08单位为米，表示一个格子的宽度

target_type: 'checkerboard' #gridtype
targetCols: 11               #number of apriltags
targetRows: 10               #number of apriltags
colSpacingMeters: 0.08
rowSpacingMeters: 0.08

2、录制包含双目相机话题的bag包

rosbag record -o stere_calibr.bag /camera/infra1/image_rect_raw /camera/infra2/image_rect_raw /camera/imu

我这里录制出来包名为stere_calibr_2025-10-03-16-12-13.bag

3、开始标定，注意 --models 是pinhole-equi ，后面--bag-from-to 5 130是包的秒数么？我觉得可以缩短一些，如5 45。

rosrun kalibr kalibr_calibrate_cameras --target new_checkboard.yaml --bag  stere_calibr_2025-10-03-16-12-13.bag --models pinhole-equi pinhole-equi --topics /camera/infra1/image_rect_raw /camera/infra2/image_rect_raw --show-extraction --approx-sync 0.03 --bag-from-to 5 130

标定结束出来三个文件
根据标定结果（右）配置两个双目相机的vins_fusion参数文件（左）

双目相机+IMU联合标定并运行vins_fusion

这一步要先完成上面双目相机标定

新建imu.yaml

#Accelerometers
accelerometer_noise_density: 1.2594292202442568e-02  #Noise density (continuous-time)
accelerometer_random_walk:   4.0715325965827638e-04  #Bias random walk
#Gyroscopes
gyroscope_noise_density:     1.8405334687656174e-03   #Noise density (continuous-time)
gyroscope_random_walk:       2.7769948094869358e-05  #Bias random walk
rostopic:                    /camera/imu      #the IMU ROS topic
update_rate:          200.0      #Hz (for discretization of the values above)

新建camchain.yaml

  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0.003097819545817662, -0.0032041077484471782, -0.00046054931488865783, 0.0007623562955000076]
  distortion_model: radtan
  intrinsics: [392.7305054875349, 392.9928238710379, 317.47649451194974, 241.34878845869625]
  resolution: [640, 480]
  rostopic: /camera/infra1/image_rect_raw
cam1:
  T_cn_cnm1:
  - [0.9999742301363518, -1.826255383145886e-05, 0.007179047964823705, -0.050003170014655964]
  - [1.4670739012890657e-05, 0.9999998747065819, 0.0005003714435306187, 8.17257185565296e-06]
  - [-0.007179056203396701, -0.0005002532270876947, 0.9999741051140919, 0.0002995230811644902]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [0.0008076080004856392, 0.0031308345192685395, -0.0005939542319836549, -0.00024727311963091705]
  distortion_model: radtan
  intrinsics: [392.84475365843866, 393.16321985549973, 309.0269177286958, 241.14311312243154]
  resolution: [640, 480]
  rostopic: /camera/infra2/image_rect_raw

新建new_checkboard.yaml

target_type: 'checkerboard' #gridtype
targetCols: 11               #number of apriltags
targetRows: 10               #number of apriltags
colSpacingMeters: 0.08
rowSpacingMeters: 0.08

运行标定

rosrun kalibr kalibr_calibrate_imu_camera --bag stere_calibr_2025-10-03-16-12-13.bag --cam camchain.yaml --imu imu.yaml --target new_checkboard.yaml --bag-from-to 10 130 --show-extraction

标定结果

cam0:
  T_cam_imu:
  - [0.9999669967015586, -0.0070883530455097335, -0.0039699822123431434, 0.0045829622209191965]
  - [0.0070734118936428, 0.9999678952515855, -0.003765011327503313, -0.0062613263130902675]
  - [0.003996542486572694, 0.0037368057503125, 0.9999850318534456, -0.020593280097173598]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0.003097819545817662, -0.0032041077484471782, -0.00046054931488865783, 0.0007623562955000076]
  distortion_model: radtan
  intrinsics: [392.7305054875349, 392.9928238710379, 317.47649451194974, 241.34878845869625]
  resolution: [640, 480]
  rostopic: /camera/infra1/image_rect_raw
  timeshift_cam_imu: 0.0012947445934955047
cam1:
  T_cam_imu:
  - [0.9999697898800448, -0.0070796056394183335, 0.0032091293600608494, -0.0455680516938101]
  - [0.007090081017954129, 0.9999695357617046, -0.0032647051444475938, -0.006263390010578269]
  - [-0.0031859187714276343, 0.003287359504473488, 0.9999895215396362, -0.020322992850038652]
  - [0.0, 0.0, 0.0, 1.0]
  T_cn_cnm1:
  - [0.9999742301363538, -1.8262553831458863e-05, 0.007179047964823707, -0.050003170014655964]
  - [1.4670739012890667e-05, 0.9999998747065839, 0.0005003714435306187, 8.17257185565296e-06]
  - [-0.007179056203396703, -0.0005002532270876947, 0.9999741051140939, 0.0002995230811644902]
  - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [0]
  camera_model: pinhole
  distortion_coeffs: [0.0008076080004856392, 0.0031308345192685395, -0.0005939542319836549, -0.00024727311963091705]
  distortion_model: radtan
  intrinsics: [392.84475365843866, 393.16321985549973, 309.0269177286958, 241.14311312243154]
  resolution: [640, 480]
  rostopic: /camera/infra2/image_rect_raw
  timeshift_cam_imu: 0.001293837851146846

运行VINS_fusion
这是我的yaml文件，暂时还没把imu标定结果放上

%YAML:1.0

#common parameters
#support: 1 imu 1 cam; 1 imu 2 cam: 2 cam; 
imu: 1         
num_of_cam: 2  

imu_topic: "/camera/imu"
image0_topic: "/camera/infra1/image_rect_raw"
image1_topic: "/camera/infra2/image_rect_raw"
output_path: "~/output/"

cam0_calib: "cam0_d435i.yaml"
cam1_calib: "cam1_d435i.yaml"
image_width: 640
image_height: 480
   

# Extrinsic parameter between IMU and Camera.
estimate_extrinsic: 1   # 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.

body_T_cam0: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
#   data: [0.9999883187624422, -5.930930785918539e-05, 0.00483309642633039, -0.05045129504417945,
#          5.82201817184673e-05, 0.9999999728828132, 0.00022548786948520963, -0.00012769957821977544,
#          -0.004833109668799891, -0.00022520385175563642, 0.9999882950985729, 0.0003684415788260392,
#           0, 0, 0, 1]
   data: [0.9999697898800448, -0.0070796056394183335, 0.0032091293600608494, -0.0455680516938101,
          0.007090081017954129, 0.9999695357617046, -0.0032647051444475938, -0.006263390010578269,
          -0.0031859187714276343, 0.003287359504473488, 0.9999895215396362, -0.020322992850038652,
           0, 0, 0, 1]

body_T_cam1: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [0.9999669967015586, -0.0070883530455097335, -0.0039699822123431434, 0.0045829622209191965,
           0.0070734118936428, 0.9999678952515855, -0.003765011327503313, -0.0062613263130902675,
           0.003996542486572694, 0.0037368057503125, 0.9999850318534456, -0.020593280097173598,
           0, 0, 0, 1]

#Multiple thread support
multiple_thread: 1

#feature traker paprameters
max_cnt: 150            # max feature number in feature tracking
min_dist: 30            # min distance between two features 
freq: 10                # frequence (Hz) of publish tracking result. At least 10Hz for good estimation. If set 0, the frequence will be same as raw image 
F_threshold: 1.0        # ransac threshold (pixel)
show_track: 1           # publish tracking image as topic
flow_back: 1            # perform forward and backward optical flow to improve feature tracking accuracy

#optimization parameters
max_solver_time: 0.04  # max solver itration time (ms), to guarantee real time
max_num_iterations: 8   # max solver itrations, to guarantee real time
keyframe_parallax: 10.0 # keyframe selection threshold (pixel)

#imu parameters       The more accurate parameters you provide, the better performance
acc_n: 0.1          # accelerometer measurement noise standard deviation. 
gyr_n: 0.01         # gyroscope measurement noise standard deviation.     
acc_w: 0.001        # accelerometer bias random work noise standard deviation.  
gyr_w: 0.0001       # gyroscope bias random work noise standard deviation.     
g_norm: 9.81007     # gravity magnitude

#unsynchronization parameters
estimate_td: 0                      # online estimate time offset between camera and imu
td: 0.0                             # initial value of time offset. unit: s. readed image clock + td = real image clock (IMU clock)

#loop closure parameters
load_previous_pose_graph: 0        # load and reuse previous pose graph; load from 'pose_graph_save_path'
pose_graph_save_path: "~/output/pose_graph/" # save and load path
save_image: 1                   # save image in pose graph for visualization prupose; you can close this function by setting 0