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_barQ_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::JacobiSVDEigen::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.99s 是每次采样的点数。如果离群点比例是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. 实战中踩过的坑与问题速查现象原因解决办法SVD求出的矩阵行列式为负对应点近似共面或退化做反射修正同时检查选点分布点云投影到图像上整体偏斜、远点严重偏离旋转外参初值误差太大手选点覆盖远处先跑RANSAC再精化近处对齐、远处整体向外偏外参平移错误或旋转微小残差非线性精化重点加远程点的约束选点明明很准优化后反而变差有离群点混入在代价函数里加Huber核图像没去畸变投影在图像边缘明显弯内参畸变残留先标定相机内参重投影误差压到半像素内数据采集时设备在动点云出现拉伸点云运动畸变静止采集或做去畸变补偿相机和雷达时间不对齐投影错位无规律时间同步问题保证两路时间戳对齐或采集时保持静止最后一个心得外参标定不是一锤子买卖。设备在运行过程中可能因为热胀冷缩、螺丝松动产生微小变化所以LVIO系统里最好留一个在线微调外参的接口让系统在运行中不断修正。但微调的前提是你提供了一个足够好的离线初值有了这个初值后续的在线优化就是在它的附近做小范围搜索不容易跑飞。我个人的习惯是每次标定完除了保存外参矩阵还会把当时用的原始数据、选点文件、RANSAC迭代参数、优化残差的变化曲线一起存档。这样下次如果发现系统效果不好可以回溯是哪一步出的问题而不是重新做一遍完整的标定流程。这套习惯帮我省了不少排查的时间也推荐给刚上手LVIO的人。