📚 课程目录第4章相机-IMU外参标定

4. 相机-IMU外参标定

任务目标

本案例使用 Kalibr 工具对相机与 IMU 之间的外参（旋转 T_ci 和平移 p_ci）进行联合标定。输入为相机拍摄的 Aprilgrid 标定板图像序列及同步的 IMU 测量数据，输出为相机到 IMU 的变换矩阵，用于后续 VIO 系统初始化与状态估计。

环境与依赖

Ubuntu 20.04 + ROS Noetic（或 18.04 + Melodic）
Kalibr 源码（github.com/ethz-asl/kalibr），按官方文档编译安装
相机驱动（如 usb_cam）与 IMU 驱动（如 imu_filter_madgwick 的上游原始数据）
标定板：Aprilgrid 6x6，打印后贴于平面硬板

数据采集要求

固定标定板，缓慢移动相机-IMU 组合体，确保：

标定板始终在视野内，覆盖图像各区域
IMU 三轴均有充分激励（旋转 + 平移），避免匀速运动
采集时长 60~120 秒，频率：图像 ≥20Hz，IMU ≥100Hz

录制 ROS bag：rosbag record /camera/image_raw /imu/data

标定流程

1. 创建标定目标配置文件

新建 aprilgrid.yaml：

target_type: 'aprilgrid'
tag_cols: 6
tag_rows: 6
tag_size: 0.033   # 每个方格边长（米）
tag_spacing: 0.2  # 方格间距 / 边长 比值

2. 编写相机-IMU 标定配置

新建 cam_imu.yaml：

cam0:
  camera_model: pinhole
  distortion_model: radtan
  distortion_coeffs: [0.0, 0.0, 0.0, 0.0]
  intrinsics: [460.0, 460.0, 320.0, 240.0]  # fx, fy, cx, cy
  resolution: [640, 480]
  rostopic: /camera/image_raw
  timeshift_cam_imu: 0.0

imu0:
  rostopic: /imu/data
  update_rate: 200.0
  accelerometer_noise_density: 0.0002
  accelerometer_random_walk: 0.00002
  gyroscope_noise_density: 0.0002
  gyroscope_random_walk: 0.00002

注意：内参和畸变系数需先用 Kalibr 单独标定相机（kalibr_calibrate_cameras）获得，噪声密度从 IMU 数据手册获取。

3. 执行联合标定

# 终端1：播放数据
rosbag play --clock your_data.bag

# 终端2：运行标定（需等待bag播放完成）
kalibr_calibrate_imu_camera \
  --target /path/to/aprilgrid.yaml \
  --cam /path/to/cam_imu.yaml \
  --imu /path/to/cam_imu.yaml \
  --bag /path/to/your_data.bag \
  --bag-freq 100.0 \
  --timeoffset-padding 0.02

关键完整代码：结果解析与验证脚本

标定完成后生成 cam_imu.yaml 结果文件，以下 Python 脚本解析并验证外参：

#!/usr/bin/env python3
import yaml
import numpy as np

# 加载 Kalibr 标定结果
with open('cam_imu.yaml', 'r') as f:
    result = yaml.safe_load(f)

# 提取旋转（四元数 xyzw）和平移
q = result['cam0']['T_cam_imu']['q']  # [x, y, z, w]
t = result['cam0']['T_cam_imu']['p']  # [x, y, z]

# 四元数转旋转矩阵
def quat_to_rot(q):
    x, y, z, w = q
    return np.array([
        [1-2*(y*y+z*z), 2*(x*y-z*w), 2*(x*z+y*w)],
        [2*(x*y+z*w), 1-2*(x*x+z*z), 2*(y*z-x*w)],
        [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x*x+y*y)]
    ])

R_ci = quat_to_rot(q)
t_ci = np.array(t).reshape(3, 1)

# 构建齐次变换矩阵 T_ci（相机←IMU）
T_ci = np.eye(4)
T_ci[:3, :3] = R_ci
T_ci[:3, 3] = t_ci.flatten()

print("T_ci (相机←IMU):")
print(T_ci)

# 验证：旋转矩阵正交性
I = R_ci @ R_ci.T
print("\n正交性检查 R*R^T 应接近单位阵:")
print(I)

# 验证：平移量级（典型值 0.01~0.1 米）
print(f"\n平移范数: {np.linalg.norm(t_ci):.4f} m")

运行与验证

运行标定：执行上述 kalibr_calibrate_imu_camera 命令，等待程序完成（通常 5~15 分钟）。
检查输出：标定目录下生成 cam_imu.yaml、report-cam_imu.pdf 等文件。
验证通过条件：

PDF 报告中重投影误差 < 0.5 像素
IMU 陀螺仪与加速度计残差曲线无明显漂移趋势
外参平移量级与机械安装尺寸一致（误差 < 2cm）
运行上述 Python 脚本，正交性检查结果接近单位阵（误差 < 1e-6）

常见问题

标定发散：检查 IMU 噪声参数是否准确，采集时是否充分激励三轴。
时间戳不同步：增大 --timeoffset-padding 至 0.05，或检查驱动时间戳精度。
重投影误差大：确认相机内参标定准确，标定板平整无弯曲。