D435i 相机IMU联合标定实战：VINS-Fusion 复现避坑指南

视觉惯性里程计（VINS）的精度很大程度上依赖于高质量的相机与 IMU 联合标定。最近在尝试使用 D435i 相机复现 VINS-Fusion 时，我遇到了不少标定问题，走了不少弯路。D435i 的 IMU 和相机外参精度直接影响 VINS-Fusion 的定位和建图效果。本文将分享我的实战经验，从底层原理到具体代码，帮助大家避开这些坑，成功复现 VINS-Fusion。

问题场景重现：标定精度不足导致 VINS 算法失效

在使用 D435i 运行 VINS-Fusion 时，我遇到的最常见问题是：

1. 定位漂移严重： 尤其是在快速移动或旋转时，轨迹迅速偏离真实路径。
2. 建图质量差： 地图点云稀疏、噪声大，甚至出现重影。
3. 算法直接崩溃： 在某些剧烈运动场景下，VINS-Fusion 无法正常初始化或运行。

这些问题最终都指向一个根源：**D435i 的相机和 IMU 之间的外参标定精度不足。**即使使用了官方提供的标定参数，在实际应用中仍然可能存在偏差，尤其是在高动态场景下。

底层原理深度剖析：联合标定的本质与难点

相机和 IMU 联合标定的本质是求解相机坐标系与 IMU 坐标系之间的转换关系，包括旋转矩阵 R 和平移向量 t。这个转换关系将 IMU 的测量数据（角速度和加速度）与相机捕捉到的图像特征点联系起来。常用的标定方法包括：

1. 基于棋盘格的标定： 通过同时观测棋盘格在相机和 IMU 中的运动，利用优化算法求解外参。
2. 基于VIO的在线标定： 在 VINS 系统运行时，同时估计相机姿态、IMU 偏差和相机-IMU 外参。

联合标定的难点在于：

• 数据同步： 必须精确同步相机图像和 IMU 数据，任何时间戳偏差都会影响标定精度。可以使用硬件触发或者软件同步，但要保证精度。
• 运动激励： 需要提供充分的运动激励，包括平移和旋转，才能保证标定结果的准确性。纯粹的平移或者纯粹的旋转都会导致标定结果不稳定。
• 优化算法： 标定问题通常是非线性的，需要选择合适的优化算法（如 Ceres Solver, g2o）并进行参数调整，以获得最佳结果。在使用 Ceres Solver 时，需要注意损失函数的选择，Huber Loss 可以有效地抑制 outlier 的影响。

代码/配置解决方案：Kalibr工具与标定流程

这里推荐使用 Kalibr 工具进行 D435i 的相机 IMU 联合标定。Kalibr 是一个开源的、功能强大的标定工具箱，支持多种相机和 IMU 模型。

1. 数据采集：

• 准备一个棋盘格，尺寸建议为 8x6 或 9x7，格子大小根据实际情况调整。
• 使用 rosbag 录制 D435i 的相机图像和 IMU 数据。确保 rosbag 包包含以下 topic：

```bash
/camera/color/image_raw # 彩色图像
/camera/depth/image_rect_raw # 深度图像 (可选，但建议)
/camera/imu/data # IMU 数据
```

• 录制时，手持 D435i 缓慢移动，覆盖棋盘格的不同角度和位置。尽可能提供充分的运动激励，包括平移、旋转、Z轴运动等等。录制时间建议 5-10 分钟。

2. 相机内参标定：

• Kalibr 可以同时标定相机内参和外参。如果已经有相机内参，可以跳过这一步。如果没有，可以使用 Kalibr 的相机内参标定功能。这里假设已经知道D435i的相机内参。

3. IMU 标定：

• Kalibr 可以标定 IMU 的噪声参数 (噪声密度、随机游走)。建议先进行 IMU 单独标定，获取 IMU 的精确噪声参数，然后再进行相机-IMU 联合标定。

4. 相机-IMU 联合标定：

• 创建 Kalibr 的标定配置文件 d435i.yaml。以下是一个示例：

```yaml
cam0_sensor: camera
cam0_topic: /camera/color/image_raw
cam0_resolution: [848, 480] # 根据实际图像分辨率修改
cam0_rostopic_camera_info: /camera/color/camera_info

imu0_sensor: imu
imu0_topic: /camera/imu/data

target: # 棋盘格配置
  type: checkerboard
  rows: 6 # 实际棋盘格行数
  cols: 8 # 实际棋盘格列数
  size: 0.025 # 棋盘格格子大小，单位米

output_path: /tmp/kalibr_results # 输出路径
time_calibration: False # 是否进行时间校准

# IMU 的噪声参数 (从 IMU 单独标定中获取)
imu0_noise_density: 2.0e-05   # gyro noise density [rad/s/sqrt(Hz)]
imu0_noise_random_walk: 2.0e-06 # gyro bias random walk [rad/s^2/sqrt(Hz)]
imu0_gyro_bias_inital: [0.0, 0.0, 0.0] # Initial gyro bias (optional)
imu0_accelerometer_noise_density: 2.0e-03 # accelerometer noise density [m/s^2/sqrt(Hz)]
imu0_accelerometer_noise_random_walk: 2.0e-04 # accelerometer bias random walk [m/s^3/sqrt(Hz)]
imu0_accelerometer_bias_inital: [0.0, 0.0, 0.0] # Initial accelerometer bias (optional)
```

• 运行 Kalibr 标定命令：

```bash
rosrun kalibr kalibr_calibrate --target /path/to/your/checkerboard.yaml --cam-def /path/to/your/d435i.yaml --imu-def /path/to/your/d435i.yaml --bag /path/to/your/data.bag --bag-from-to 5 45 # 从 rosbag 的第 5 秒开始到第 45 秒结束
```

5. 将标定结果应用到 VINS-Fusion：

• Kalibr 会生成一个标定结果文件，其中包含相机内参、IMU 噪声参数和相机-IMU 外参。将这些参数更新到 VINS-Fusion 的配置文件中。VINS-Fusion 的配置文件通常是 YAML 格式，需要仔细核对每个参数的单位和坐标系。

实战避坑经验总结

1. 充分的运动激励是关键： 录制数据时，务必保证相机在三维空间内都有充分的运动，特别是围绕三个轴的旋转。纯粹的平移运动无法准确标定旋转矩阵。
2. 同步精度至关重要： 检查相机图像和 IMU 数据的时间戳是否对齐。如果存在偏差，可以使用 Kalibr 的时间校准功能进行校正。
3. 仔细检查标定结果： Kalibr 会输出标定结果的误差评估。如果误差过大，说明标定结果不可靠，需要重新录制数据或调整标定参数。
4. 标定结果的坐标系转换： Kalibr 标定得到的坐标系可能与 VINS-Fusion 使用的坐标系不同。需要进行坐标系转换，确保标定结果的正确性。
5. 动态标定与在线优化： 如果对精度要求较高，可以考虑使用基于 VIO 的在线标定方法，或者在 VINS-Fusion 运行时进行外参优化。 Ceres Solver 和 g2o 都是不错的选择，但需要根据实际情况选择合适的优化算法和参数。

通过以上方法，可以显著提高 D435i 相机 IMU 联合标定的精度，从而改善 VINS-Fusion 的定位和建图效果。希望这些经验能帮助大家成功复现 VINS-Fusion，并在实际应用中取得更好的效果。