4、空间同步（标定）：相机内参标定、激光雷达与相机外参标定、多雷达标定

空间同步，说白了就是让不同传感器「说同一种语言」。

你想想看，相机看到的是像素，激光雷达看到的是点云，毫米波雷达看到的是目标列表。如果它们各自用各自的坐标系说话，那融合就是个笑话。我刚开始做多传感器融合时，就吃过这个亏——相机和雷达各自跑得挺好，一融合就乱套，后来才发现是外参偏了0.5度。嗯，0.5度在10米外就是将近10厘米的偏差，对于目标跟踪来说，这误差已经大到没法忍了。

4.1 相机内参标定

相机内参标定，是所有标定的第一步。为什么？因为相机的外参是相对于相机坐标系来说的，而相机坐标系又依赖于内参。内参不准，外参就是空中楼阁。

内参标定说白了就是求四个东西：焦距（fx, fy）、光心（cx, cy）和畸变系数（k1, k2, p1, p2, k3）。我个人习惯用棋盘格标定板，OpenCV的cv2.calibrateCamera()函数就能搞定。

核心步骤：

打印一张棋盘格标定板（建议用A3纸，格子大小5cm以上）
从不同角度拍摄15-20张照片（注意：覆盖边缘和角落）
用OpenCV提取角点，进行标定
检查重投影误差，一般要小于0.5像素

import cv2
import numpy as np

# 棋盘格参数
CHECKERBOARD = (9, 6)  # 内角点数量
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)

# 准备世界坐标系中的点
objp = np.zeros((CHECKERBOARD[0] * CHECKERBOARD[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:CHECKERBOARD[0], 0:CHECKERBOARD[1]].T.reshape(-1, 2)

objpoints = []  # 3D点
imgpoints = []  # 2D点

images = glob.glob('calib_images/*.jpg')

for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, CHECKERBOARD, None)
    
    if ret:
        objpoints.append(objp)
        corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
        imgpoints.append(corners2)

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
    objpoints, imgpoints, gray.shape[::-1], None, None
)

print("内参矩阵:\n", mtx)
print("畸变系数:\n", dist)

避坑指南：我曾经用手机拍标定板，结果发现重投影误差一直降不下去。后来才发现是照片分辨率太高，角点提取不稳定。建议把图片缩放到1000x800左右再标定，效果会好很多。

4.2 激光雷达与相机外参标定

激光雷达和相机的外参标定，说白了就是求一个旋转矩阵R和一个平移向量t，把激光雷达坐标系下的点云投影到相机图像上。

我常用的方法有两种：

基于标定板的方法：在标定板上提取激光雷达的线特征和相机的角点特征，然后求解PnP问题
基于自然特征的方法：利用场景中的边缘、角点等自然特征进行匹配

我个人更推荐第一种，因为稳定、可重复。具体做法是这样的：

放置一个棋盘格标定板（最好用亚克力板，表面平整）
激光雷达扫描标定板，提取板上的点云边缘
相机拍摄标定板，提取角点
用最小二乘法求解R和t

# 伪代码：激光雷达点云投影到图像
def project_lidar_to_image(points_3d, R, t, camera_matrix, dist_coeffs):
    # 将激光雷达点转换到相机坐标系
    points_cam = R @ points_3d.T + t.reshape(3, 1)
    
    # 投影到图像平面
    points_2d, _ = cv2.projectPoints(
        points_cam.T, 
        np.zeros((3,1)), 
        np.zeros((3,1)), 
        camera_matrix, 
        dist_coeffs
    )
    return points_2d

注意：激光雷达的点云是稀疏的，标定板上的点可能只有几十个。我曾经遇到过标定板太小，激光雷达扫不到足够点的情况。建议标定板尺寸至少30cm x 30cm，并且放在距离传感器3-5米的位置。

4.3 多雷达标定

多雷达标定，说白了就是让多个激光雷达「对齐」到同一个坐标系下。这在自动驾驶中很常见——车顶一个128线主雷达，车头两个补盲雷达，车尾再来一个。

多雷达标定有两种思路：

方法	原理	适用场景	精度
直接法	利用公共视场中的标定物，直接求解相对位姿	雷达之间有重叠视场	高（< 2cm）
间接法	先各自标定到车体坐标系，再推导相对关系	雷达之间无重叠视场	中等（< 5cm）

我个人习惯用直接法，因为精度更高。具体做法是：

在公共视场中放置多个标定物（比如三个不同高度的圆柱体）
每个雷达分别提取标定物的中心点
用ICP或SVD分解求解相对位姿

# 伪代码：多雷达标定中的SVD求解
def solve_relative_pose(points_src, points_tgt):
    # 去中心化
    centroid_src = np.mean(points_src, axis=0)
    centroid_tgt = np.mean(points_tgt, axis=0)
    
    points_src_centered = points_src - centroid_src
    points_tgt_centered = points_tgt - centroid_tgt
    
    # SVD分解
    H = points_src_centered.T @ points_tgt_centered
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T
    
    # 确保旋转矩阵行列式为+1
    if np.linalg.det(R) < 0:
        Vt[-1, :] *= -1
        R = Vt.T @ U.T
    
    t = centroid_tgt - R @ centroid_src
    return R, t

经验之谈：多雷达标定最容易出问题的地方是「时间同步」。我曾经遇到过两个雷达的扫描频率不一样（一个10Hz，一个20Hz），导致标定结果时好时坏。后来加了时间戳对齐，问题就解决了。建议在标定前先确认所有雷达的时钟是否同步。

4.4 标定结果验证

标定完了，怎么知道准不准？我一般做三件事：

可视化验证：把激光雷达点云投影到图像上，看边缘是否对齐。如果车身边缘和图像中的边缘偏差超过3个像素，就要重新标定。
距离验证：在已知距离的位置放一个目标，看融合后的距离误差。一般要求误差小于10cm。
重复性验证：拆装传感器后重新标定，看结果是否一致。如果两次标定结果偏差超过1度，说明安装结构有问题。

核心要点总结：

相机内参标定是基础，重投影误差要小于0.5像素
激光雷达与相机外参标定，建议用标定板法，稳定可靠
多雷达标定，优先用直接法，精度更高
标定完成后一定要做验证，别偷懒

好了，空间同步这块就聊到这儿。标定这东西，说白了就是「差之毫厘，谬以千里」。我见过太多项目因为标定没做好，后面融合算法写得再好也白搭。所以，别嫌麻烦，每一步都做扎实了，后面才能省心。

📚 课程目录

4、空间同步（标定）：相机内参标定、激光雷达与相机外参标定、多雷达标定

4.1 相机内参标定

4.2 激光雷达与相机外参标定

4.3 多雷达标定

4.4 标定结果验证