多传感器融合中的外参标定：相机与激光雷达位姿对齐原理与SVD求解实践

LVIO-SLAM这个系列写到这里，终于轮到外参标定了。我一直觉得，多传感器融合系统里最磨人的不是后端优化，不是回环检测，而是标定这一步。相机告诉你颜色和纹理，雷达告诉你距离和几何，惯导告诉你加速度和角速度，但问题是这三者各说各话，坐标系都不一样。你让它们协同作战，总得先把各自的位置和朝向对齐。这个对齐的动作，就是外参标定。这篇文章是系列的第二十一篇，先聚焦相机与激光雷达的外参标定，把原理讲透，把初值求解的流程走一遍。后续再延伸到惯导相关的外参，以及在线标定的实现。

先说结论：外参标定说白了就是求解两台传感器坐标系之间的刚体变换，一个旋转加一个平移，共六个自由度。听起来简单，实操起来坑不少。点云稀疏、图像稠密，一个是几何测量，一个是光度测量，很难自动找对应关系。选点选不好、初值给不准、时间对不齐，都会让结果一言难尽。这篇我会从方案选型、数据采集、对应点求解、残差优化到结果验证，把整套流程捋一遍，适合刚接触LVIO或者想自己动手标定传感器对的外参的开发者参考。

1. 先弄明白外参标定在LVIO里解决什么问题

1.1 相机、雷达和惯导各说各话，融合前必须对表

LVIO框架里，激光雷达负责提供精确的几何结构，相机负责提供丰富的视觉特征和纹理，惯导负责在雷达和相机的更新间隙里做高频运动预测。这三类传感器各有各的优势，但融合的前提是：它们描述的是同一个物理世界，只是从不同位置、不同角度去观测。雷达看到一个墙角，坐标是相对于雷达系原点的；相机拍到同一个墙角，坐标是像素坐标，隐含在图像平面上；惯导测量的加速度和角速度，则是相对于IMU系的三轴分量。如果不把这些观测统一到同一个坐标系下，特征匹配、深度关联、残差构建全都会乱套。

外参标定解决的就是这个问题。它给出传感器之间的相对位姿关系，通常用一个四乘四的齐次变换矩阵表示。这个矩阵一旦确定，雷达点就能投影到图像平面，视觉特征也能反投影到三维空间，惯导数据也能和视觉、激光数据建立起空间上的对应。在LVIO的优化状态向量里，外参通常被当成待优化变量，和非线性优化里的位姿、速度、零偏一起估计。只是很多框架默认外参是已知的、固定的，所以前期标定的质量直接决定系统能不能跑出好效果。

1.2 外参的数学定义：一个四乘四矩阵背后是六自由度

外参矩阵本质上是一个刚体变换。刚体变换的意思是，不会改变物体形状，只改变位置和朝向，数学上属于特殊欧氏群 SE(3)。一个刚体变换包含两部分：旋转矩阵 R 属于 SO(3)，表示坐标系之间的朝向关系；平移向量 t 属于 R3，表示原点之间的偏移。组合起来是：

T = [ R t; 0 1 ]

这个矩阵的作用很简单：如果雷达系 L 下有一个三维点 p_L，要转换到相机系 C 下，就做一次矩阵乘法 p_C = T_CL * p_L。注意这里的下标约定，T_CL 表示从雷达系到相机系的变换。方向搞反是初学者最容易犯的错误，我见过不少人在矩阵乘法里把变换方向写反，结果点云投影全乱了。建议统一约定：代码里一律用“源坐标系到目标坐标系”的命名方式，比如T_cam_lidar，后面跟乘号时把源点放右边，这样不容易出错。

六个自由度里，旋转三个（绕X、Y、Z轴），平移三个。旋转矩阵有几个约束，必须是正交阵且行列式为1。所以不能随便填九个数字进去，而是要通过欧拉角、四元数或者李代数来参数化。在李代数里，SE(3) 对应的李代数维度恰好是六，这在后面的非线性优化里特别实用，因为可以在流形上做扰动，保证优化过程始终停留在合法的刚体变换集合上。

2. 标定方案怎么选：先别急着采数据

2.1 离线标定与在线标定的取舍

LVIO的整个生命周期里，外参标定可以发生在两个阶段。一个是在系统运行之前，专门摆场景、采数据、做标定，这叫离线标定。另一个是系统运行过程中，利用SLAM的位姿估计结果反推外参，并且不断修正，这叫在线标定。两种方式各有适用场景。

离线标定的优势是可控。你可以在一个静止或慢速的场景里让传感器充分观测，可以人为布置标定板、特征明显的杆子和墙角，可以仔细检查每一帧数据。只要操作得当，精度能做到很高，而且标定结果可以复用于之后的所有运行。缺点就是麻烦，每次传感器安装位置动了，或者设备拆装过，就得重新标一遍。

在线标定则是在SLAM或里程计估计过程中，把外参作为状态量的一部分放进去，利用视觉重投影残差和雷达配准残差共同约束外参。这种方法不需要专门的标定流程，传感器装好直接跑，系统自己会去估计。但问题是可观测性受限，有些运动模式下外参根本估不准，比如一直往前走、不转弯，旋转外参就容易退化。所以我的建议是：离线标定做初值，在线标定做细化和补偿。先离线拿到一个可靠的外参，再让系统在运行中微调，这是最稳的路线。

2.2 靶标法、手工对应点法与免靶标法，分别适合什么场景

离线标定内部又分几派。靶标法是最经典的，利用棋盘格或专用标定板，让相机和激光雷达同时观测同一块靶标，然后通过靶标在不同传感器中的观测来建立约束。这种方法精度高，因为靶标的角点特征非常准确。但靶标法对雷达点云密度有要求，多线雷达在近处分辨率还可以，如果靶标放得远了，点云打在靶标上的点数太少，角点就拟合不准。单线雷达甚至只能扫到一条线，如果没有跟棋盘格的面相交出几个清晰的点，基本没法做。

手工对应点法是我最常用的一种，简单直接：在两帧同步数据里，肉眼识别几个特征点，比如墙角、杆尖、标定板角点，然后在雷达点云里读取三维坐标，在图像上读取对应的像素坐标或者直接反投影到三维，从而得到若干组3D-3D对应点，然后用最小二乘求解。这个方法的好处是完全不需要昂贵的标定板，坏处是人工选点精度有限，所以通常只用来求初值，后面再用非线性优化精化。

免靶标法属于进阶玩法。这类方法从场景的自然特征出发，利用边缘、平面或反射强度等信息自动建立约束，或者利用运动中传感器形成轨迹的一致性来标定。好处是自动化程度高，不需要人工干预，适合批量标定和大规模部署。缺点是实现复杂度高，而且在某些退化场景下会失效。

选型时还要考虑雷达的类型。多线机械雷达（比如常见的16线、32线、64线）点云密度相对均匀，适合靶标法和手工对应点法。固态雷达的视场角有限，点云分布会有明显的视场边界，标定时要充分考虑共视区域。如果是单线雷达和相机标定，老实说非常难，只有手工对应点法加上非常严格的场景布置才可能拿到可用的结果。

3. 实操一：采数据集时的关键细节

3.1 场景布置与采集要点

标定数据的质量，一半取决于场景布置。我的经验是分成三个层次：近处、中处、远处各布置一些特征。近处放一个棋盘格或者黑白靶标，用作高精度的参考；中处在视野里找几个角点特征，比如墙角、窗角、柱子棱线；远处找一些明显的边缘，比如楼顶的轮廓线、电线杆。这样做的原因很简单，外参标定非常忌讳“矮胖”的数据分布。如果所有对应点都堆在正前方一两米的地方，旋转外参的估计会非常脆弱，因为角分辨率太大，微小的旋转误差在近距离内几乎看不出差异；而远处的点则对旋转误差非常敏感，能起到很好的牵制作用。

采集时让设备静止或者缓慢平移，扫描一个开阔角度。尽量让传感器水平旋转、俯仰变化，把周围一圈的特征都覆盖到。如果条件允许，可以采集多组不同距离和角度的数据，最后一起参与优化。每采一组，记录时间和传感器时间戳，方便后面做时间同步。

一个最常见的错误是只为了省事，直接站在走廊中间，对着走廊尽头采了一堆点，结果所有特征点几乎都在同一个平面上。这种近似共面的配置会让SVD求解退化，甚至解出的旋转矩阵都不满足正交性。

3.2 时间同步和点云畸变，两个容易翻车的坑

时间不同步这个问题，是所有多传感器标定里最容易翻车的坑之一。相机一般每秒出30帧图像，雷达从10Hz到20Hz不等，IMU更高频。如果你只是简单地采集数据，不做时间对齐，那么相机拍到的场景和雷达扫描到的场景可能已经发生了变化。设备一移动，这种错位直接转化为标定误差。处理方式大致分两种：硬同步和软同步。硬同步是指硬件上给各个传感器打同一个触发脉冲，让它们在同一时刻采样。软同步则是靠软件去对齐时间戳，一般是取最近邻，或者做线性插值。对于标定场景，我建议尽量让设备待在一个静止或极缓慢运动的状态，这样时间不同步的影响就被大幅削弱了。

点云畸变也是常被忽视的问题。机械雷达是旋转扫描的，每一个点其实是在不同时刻采到的。如果设备本身在运动，这一帧点云叠加在一起就产生了畸变。静止标定可以避免这个问题，但如果你的标定流程需要在运动中采集，就必须做运动补偿。最简单的方法是借助IMU做时间戳级插值，或者在离线处理时使用已知的标定平台运动轨迹，将点云补偿到帧首时刻的坐标系下。

相机图像的畸变同样不能忘。别以为拿了相机内参就完事了，如果图像没有去畸变，标定出来的外参一定会带进内参畸变残留的误差。所以我的操作顺序是：先把相机内参用棋盘格标定到重投影误差小于半个像素，再去畸变图像，最后才做外参标定。内参不准，外参白干，这是铁律。

4. 实操二：手工对应点加SVD求初值

4.1 对应点怎么选：三类可靠特征

求初值的核心是建立3D-3D对应点。雷达系下的三维点可以直接读取，相机系下的三维点则需要一点技巧。如果你相机视野里有标定板，可以直接用PnP解出标定板在相机系下的位姿，从而得到板角的三维坐标。如果没放标定板，那就需要用双目视觉或者从单目运动恢复结构，这对初学者来说门槛偏高。所以我建议入门阶段就用标定板，简单可靠。

选点的时候，优先选三类特征。第一类是多面体角点，也就是三个平面交汇出来的角，比如房间的墙角、窗台角，这种特征在雷达点云里极其明确，三个面的点各占一侧，角点位置在直观上就能确定。第二类是杆状物体的顶点，比如路灯顶端、电线杆的尖端，前提是杆的轮廓在点云里足够清晰，而且杆尖跟图像里的特征能一一对应。第三类就是标定板上的黑白格子角点，这个最准，代价是需要标定板。

选完点之后要检查一下点的分布。新的约束条件有三个：旋转三个自由度、平移三个自由度，所以理论上三对不共线的对应点就能求解。但实际上三对点太勉强了，噪声稍微大一点，结果就偏了。我建议最少选十对以上，分布到画面的不同区域，远近结合。如果雷达点云里有些点不太确定是哪根杆，宁可不选，也不要硬凑。

4.2 闭式解推导：SVD为什么能解刚体变换

三对以上的3D-3D对应点，可以构成一个最小二乘问题。目标函数是找一个旋转 R 和平移 t，让所有点的残差平方和最小：

min Σ || q_i - (R p_i + t) ||^2

这里 p_i 是雷达系下的点，q_i 是相机系下的对应点。这个问题有闭式解，经典做法是去质心加SVD。

先把两组点的质心算出来，p_bar 和 q_bar，然后把所有点减去质心，得到去质心坐标 P_i = p_i - p_bar，Q_i = q_i - q_bar。构造一个三乘三的协方差矩阵 H = Σ P_i Q_i^T。对 H 做SVD分解，H = U Σ V^T，那么旋转矩阵的最优解就是 R = V U^T。这里有一个细节需要注意，如果 det(R) 为负，说明解出来的是一个反射变换而不是刚体变换，这种情况通常发生在噪声特别大或者点的分布退化的时候，需要把 V 的最后一列取负再计算。平移项则直接用质心关系重建，t = q_bar - R p_bar。

这个结果的原理可以这样理解：刚体变换不会改变点之间的相对距离，所以最优的旋转只取决于点云的整体形态匹配，用协方差矩阵的特征结构来估计旋转正好利用了这种形态信息。去掉质心这一步，本质上就是把平移项分离出来，让旋转的求解不再受绝对位置影响。

实操中，我一般用C++配上常用的矩阵库直接实现，核心代码很短：

// p_cam, p_lidar 是 n×3 的点集，已按对应关系排好 Eigen::Vector3d p_bar = p_lidar.colwise().mean(); Eigen::Vector3d q_bar = p_cam.colwise().mean(); Eigen::MatrixXd P = p_lidar.rowwise() - p_bar.transpose(); Eigen::MatrixXd Q = p_cam.rowwise() - q_bar.transpose(); Eigen::Matrix3d H = P.transpose() * Q; Eigen::JacobiSVD<Eigen::Matrix3d> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::Matrix3d R = svd.matrixV() * svd.matrixU().transpose(); // 反射修正 if (R.determinant() < 0) { Eigen::Matrix3d V = svd.matrixV(); V.col(2) *= -1; R = V * svd.matrixU().transpose(); } Eigen::Vector3d t = q_bar - R * p_bar;

这段代码跑完，外参的初值就出来了。我实测下来，在正常情况下，这个初值的精度已经足够用来做后续的非线性精化。

4.3 RANSAC剔除误匹配

手工选点的时候，很难避免有几个点选偏了。哪怕只是偏了几厘米，在最小二乘里也会拉偏整个结果，因为平方误差让离群点的影响被放大了。所以单纯跑一遍SVD不够，还需要用RANSAC把误匹配点筛掉。

RANSAC的思路不复杂：每次随机抽三对点，用SVD求一个候选外参，然后拿这个外参去试所有点，算每对的残差距离，小于阈值的当作内点。反复迭代，最后保留内点最多的那组模型，再用全部内点重新做一次SVD，得到最终的外参。迭代次数不是随意定的，可以根据公式算：假设离群点比例是 e，要保证至少一次采样全部抽到内点，迭代次数 N = log(1-p) / log(1-(1-e)^s)，其中 p 是置信概率，一般取0.99，s 是每次采样的点数。如果离群点比例是30%，三对点采样时算出大概需要100次左右迭代；如果离群点比例升到50%，就要接近150次。这个公式能让你心里有数，不用盲目加大迭代次数。

阈值设置也要动脑筋。雷达点云本身有几厘米测量噪声，手工选点再引入几厘米的误差，所以初始阈值可以放宽松一点，比如0.1米到0.2米。先跑一轮RANSAC，把明显离群的选点错误去掉，剩下的数据再用更严格的阈值，比如0.05米，做一次精化。这种由粗到精的迭代，效果远好于一次性定死阈值。

5. 实操三：非线性优化精化和结果验证

5.1 从初值到精化：用非线性优化再压一步残差

手工对应点的精度是有上限的，即使经过RANSAC，残差也可能还在几个厘米级别。这时候该上非线性优化了。把外参参数化到SE(3)流形上，用李代数扰动模型构建雅可比，然后用高斯牛顿或LM算法迭代，把残差压下去。

优化目标依然是点-点距离残差，但这次不再做闭式解，而是迭代优化。残差定义为：

r_i = q_i - (R p_i + t)

这里位姿增量用左扰动模型表示。对旋转和平移求导，得到雅可比矩阵。优化过程用LM算法，它会自动调整阻尼系数，在靠近最优解时退化为高斯牛顿，在远离时近似梯度下降，对初值不太好的情况也有一定容忍度。我在工程里喜欢加一层鲁棒核函数，比如Huber核。原因是手工选点偶尔还有漏网之鱼，Huber核能把大残差的影响力限制住，不会让一个坏点把整体结果带偏。

注意，非线性优化不是万能的。如果初值错得离谱，比如旋转差了二三十度，那优化很容易落入局部极小值。所以初值求解这一步永远不能省。我自己实测，初值已经把旋转误差控制到几度以内的时候，优化收敛得又快又稳。

5.2 验证标定结果：把点云投到图像里眼见为实

标定完，一定要做可视化验证。最直观的方式是把雷达点云按解出的外参投影到相机图像上，然后叠加显示。我的做法是：对一帧点云里的每个三维点，先变换到相机系，再通过相机内参投影到像素坐标，把点的反射强度或者深度值映射成图像上的一个彩色点。如果外参是对的，你会看到点云贴合在图像里的物体边缘上：墙面的点在墙面上，杆子的点在杆子上，地面的点铺在地面上。如果外参有偏差，点位会整体往一个方向偏。

投影的代码也很直接：

cv::Mat img = cv::imread(...); for (const auto& p_lidar : cloud) { Eigen::Vector3d p_cam = T_cam_lidar * p_lidar; double u = fx * p_cam(0) / p_cam(2) + cx; double v = fy * p_cam(1) / p_cam(2) + cy; cv::circle(img, cv::Point(u, v), 1, color, -1); }

验证的时候不要只看一帧，要挑不同距离的几帧检查。近处的对齐情况好，不代表外参就是准的。如果旋转外参存在一个微小偏差，在近处可能只偏几个像素，到了远处会被放大成很大的偏移。所以一定要看远景，比如几十米外的建筑边缘、电线杆，这才是检验外参精度的试金石。还有一个技巧：你把点云投影到图像上之后，分别检查左边缘、右边缘、上边缘、下边缘四个区域的对齐情况。如果四个区域同时对齐，说明外参在整体上可信；如果某个区域系统性偏了，多半是旋转外参在某个轴上有残留误差。

6. 实战中踩过的坑与问题速查

最后一个心得：外参标定不是一锤子买卖。设备在运行过程中可能因为热胀冷缩、螺丝松动产生微小变化，所以LVIO系统里最好留一个在线微调外参的接口，让系统在运行中不断修正。但微调的前提是你提供了一个足够好的离线初值，有了这个初值，后续的在线优化就是在它的附近做小范围搜索，不容易跑飞。

我个人的习惯是每次标定完，除了保存外参矩阵，还会把当时用的原始数据、选点文件、RANSAC迭代参数、优化残差的变化曲线一起存档。这样下次如果发现系统效果不好，可以回溯是哪一步出的问题，而不是重新做一遍完整的标定流程。这套习惯帮我省了不少排查的时间，也推荐给刚上手LVIO的人。