4、多传感器联合标定：Camera-LiDAR联合标定原理、Camera-IMU标定（Kalibr工具）、LiDAR-IMU标定、实战：标定结果验证与可视化
各位同学，欢迎来到多传感器联合标定这一章。
说实话，前面几章我们分别聊了相机、激光雷达、IMU各自的标定。但真实场景下，这些传感器是装在一起的。你想想看，如果它们之间的相对位置都不知道，那数据融合就是一句空话。这一章，我们就来解决这个核心问题——让它们「对齐」。
4.1 为什么需要联合标定？
我刚开始做多传感器融合时，犯过一个低级错误。我把相机和激光雷达的数据直接叠加，结果发现图像里的电线杆，在点云里却偏移了两米。嗯，这就是没做联合标定的后果。
联合标定要解决的核心问题只有一个：求取不同传感器坐标系之间的刚体变换矩阵（旋转R + 平移t）。说白了，就是知道相机看到的那个点，在激光雷达坐标系下到底在哪儿。
核心公式（以Camera-LiDAR为例）：
PLiDAR = R * PCamera + t
其中R是3x3旋转矩阵，t是3x1平移向量。我们就是要标定出这12个参数。
4.2 Camera-LiDAR联合标定原理
这是最经典的一对组合。相机给纹理，激光雷达给深度，天生一对。但怎么让它们「对上眼」呢？
常用方法：基于标定板的方案
我个人习惯用棋盘格标定板。为什么？因为棋盘格角点检测成熟，而且激光雷达打在黑白交界处，反射强度会有明显跳变。
- 相机侧：检测棋盘格角点，得到2D像素坐标。
- 激光雷达侧：提取标定板平面上的点云，拟合平面，再找到角点在3D空间中的位置。
- 匹配：建立2D-3D对应点对，用PnP算法求解R和t。
我的经验：采集数据时，标定板要放在不同距离和角度。至少采集15-20组数据。我曾经只拍了5组，结果标定出来的外参在远距离误差很大，后来补拍才解决。
4.3 Camera-IMU标定（Kalibr工具）
Kalibr是ETH Zurich开源的工具，做Camera-IMU标定，它几乎是行业标准。我用了好几年，确实稳定。
标定原理：
Kalibr的核心思想是「运动激励」。你拿着设备做各种运动，相机看到的是图像特征点的运动，IMU看到的是加速度和角速度。通过联合优化，同时估计出：
- 相机内参
- 相机与IMU之间的外参（R, t）
- IMU的零偏、噪声参数
- 时间延迟（Time offset）
实际操作步骤：
- 准备标定板：Aprilgrid棋盘格，Kalibr对Aprilgrid支持最好。
- 录制数据：用rosbag录制，同时包含图像话题和IMU话题。运动要丰富：平移、旋转、急停、晃动。
- 运行Kalibr：
# 标定命令示例
kalibr_calibrate_cameras \
--bag /path/to/your.bag \
--topics /camera/image_raw \
--models pinhole-radtan \
--target /path/to/aprilgrid.yaml
# 然后标定Camera-IMU
kalibr_calibrate_imu_camera \
--bag /path/to/your.bag \
--cam /path/to/cam_chain.yaml \
--imu /path/to/imu.yaml \
--target /path/to/aprilgrid.yaml
避坑指南：我曾经在录制数据时，运动太慢，导致IMU的角速度激励不足。Kalibr报错说「无法初始化」。后来我改成快速晃动，问题就解决了。记住：IMU需要足够的角速度和加速度激励，才能把参数估计准。
4.4 LiDAR-IMU标定
LiDAR和IMU的标定，相对Camera-IMU要「粗糙」一些。为什么？因为LiDAR点云没有纹理，无法像图像那样提取特征点。
常用方法：
- 基于连续时间优化：利用LiDAR的扫描匹配（如ICP）构建约束，同时优化IMU的零偏和外参。
- 基于运动约束：让设备做纯旋转运动，LiDAR和IMU的角速度应该一致，从而标定旋转外参。
我个人推荐用LIO-SAM或FAST-LIO2这类框架自带的标定功能。它们内置了LiDAR-IMU联合优化，你只需要提供一段数据，跑一遍就能得到外参。
# 以FAST-LIO2为例，配置文件中设置：
# 初始外参可以给一个粗略值，算法会自动优化
extrinsic_rot: [1, 0, 0, 0, 1, 0, 0, 0, 1] # 初始旋转矩阵
extrinsic_trans: [0.0, 0.0, 0.0] # 初始平移向量
小技巧：如果你没有现成的框架，可以用一个简单方法：把LiDAR和IMU固定在一个平台上，做几次纯旋转运动。然后对比LiDAR点云配准得到的旋转和IMU积分得到的旋转，用最小二乘法解出外参旋转矩阵。平移部分，可以通过测量物理安装位置来近似。
4.5 实战：标定结果验证与可视化
标定完，怎么知道结果对不对？我见过太多人跑完标定就完事了，结果实际使用时发现数据对不上。验证这一步，绝对不能省。
验证方法一：投影可视化
这是最直观的方法。把激光雷达点云投影到图像上，看边缘是否对齐。
# Python伪代码示例
import numpy as np
import cv2
# 假设我们已经有了外参 R, t 和相机内参 K
def project_lidar_to_image(points_3d, R, t, K):
# 将LiDAR点云转换到相机坐标系
points_cam = (R @ points_3d.T + t.reshape(3,1)).T
# 只保留相机前方的点
mask = points_cam[:, 2] > 0
points_cam = points_cam[mask]
# 投影到像素平面
points_2d = (K @ points_cam.T).T
points_2d = points_2d[:, :2] / points_2d[:, 2:3]
return points_2d, mask
# 可视化
img = cv2.imread('image.jpg')
points_2d, _ = project_lidar_to_image(lidar_points, R, t, K)
for pt in points_2d:
cv2.circle(img, (int(pt[0]), int(pt[1])), 2, (0, 255, 0), -1)
cv2.imshow('Projection', img)
验证方法二：残差统计
对于标定板数据，可以计算重投影误差。如果误差在1-2个像素以内，说明标定质量不错。如果超过5个像素，建议重新标定。
| 验证指标 | 优秀 | 可接受 | 需重新标定 |
|---|---|---|---|
| 重投影误差（像素） | < 1.0 | 1.0 - 3.0 | > 3.0 |
| 点云与图像边缘对齐 | 完全对齐 | 轻微偏移（< 5cm） | 明显错位 |
| IMU与LiDAR旋转一致性 | 误差 < 0.5° | 0.5° - 2° | > 2° |
我的习惯：标定完成后，我会录制一段新的数据（不是标定用的那一段），然后做投影验证。如果新数据也能对齐，那标定结果才是真的可靠。这叫「交叉验证」，做工程的一定要养成这个习惯。
好了，这一章的内容就到这里。多传感器标定是数据融合的基石，花时间把这一步做扎实，后面的场景重建才能事半功倍。记住我那句老话：标定做不好，重建全白搞。