多个LiDAR-Camera无目标跨视角联合标定方法

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

>>点击进入→自动驾驶之心【相机标定】技术交流群

0. 写在前面

最近很多小伙伴咨询激光雷达和相机标定相关的内容，今天Nicky为大家分享一个多个LiDAR-Camera无目标跨视角联合标定方式，建议收藏哦！

1. 个人的一些理解

由于半固态雷达低成本的优势，现在正在越来越多的得到应用。但是半固态雷达由于FOV比较小，同时现在传感器布置方案中的overlap很小，基于多激光匹配的方案容易出现标定结果的不稳定。因此跨视角联合标定在当前的智驾系统中拥有非常重要的意义。同样在不少L4方案中，同一辆车可能会同时出现机械式雷达和半固态雷达。由于不同雷达的成像原理的不同，它们往往也拥有不同的scan pattern，也因此需要一种通用的特征提取与关联方式。最后，当前智驾车辆普遍拥有多个相机，在没有产线的情况下相机和激光雷达的联合标定会非常费力，产线标定的精度也一般不能满足道路场景的需求。因此需要一款在自然场景下实现自动标定的方法。在这样的需求背景下，由MARS实验室于2021年发表并开源的MLCC是一款非常有竞争力的自然场景多激光，激光相机联合标定方式。这套标定方案具有非常理想的标定精度与鲁棒性。

假定一套多传感器系统中有个激光雷达和个相机，其中被定义为主雷达，其余的均为副雷达，在这套方案中我们将会把所有副雷达和相机标到主雷达。记副雷达到主雷达之间的外参为，以及相机到主雷达的外参，这是我们待优化的变量。标定之前需要对待优化的变量获取一个初值，比如从CAD图纸中获取，通过实际测量，或者手眼标定等等。为了使用这套标定方案，我们采集数据的过程中需要通过充分的评议和旋转的运动使得所有激光雷达和相机拥有共视区域（拍到了同一个地方），并且多传感器传感器之间时间同步完好。假设通过抽帧，去除运动畸变等前置的数据处理之后总共有了帧同时刻的雷达点云和相机图像数据，记这些数据时间戳为。在这套方案中假设相机模型对应的内参已经标好，首先标定雷达之间的外参，再在准确的雷达外参的基础上构建多激光局部地图，用于标定激光相机之间的外参。

2. 多激光雷达跨视角联合标定

整个激光雷达标定的核心是所有帧的雷达点云堆叠出的局部地图的一致性展开：假设我们可以获取一个精确的主雷达里程计，以及准确的多激光外参，那么就可以将任一时刻的一帧来自雷达点云投影回世界坐标系，构成的局部地图具有很高的一致性:

这样将所有雷达点云投影到世界坐标系下构成局部地图定义评估地图一致性的consistency indicator为，通过优化这个一致性评价指标来同时优化雷达的pose和外参。

，

在MLCC中使用自适应提速化的方式提取平面特征，以八叉树的数据结构存储特征地图，并通过八叉树递归的方式构建点面误差作为局部地图一致性评价指标。下面将分别介绍特征地图的提取，存储与遍历；损失函数的构建；以及损失函数建立，简化以及解析形式的二阶近似的计算。

由于优化的维度庞大，比如一趟具有n个激光雷达，采了m帧数据的标定，其维度就高达维度，如果数值优化中给的初值不够好的话，优化问题就不容易收敛，因此除了需要一个较为理想的初始化意外，作者将多激光标定分为三步：

• 第一步通过优化主雷达堆叠出的地图的一致性指标优化主雷达里程计，

• 第二步借助优化的主雷达里程计堆叠主雷达+副雷达地图，通过优化这个地图的一致性优化各个副雷达到主雷达的外参。

• 第三部借助前面两步的结果，将所有雷达的点云堆叠到一起构成地图，通过优化这个地图的一致性同时优化和。

2.1. 自适应体素化特征提取

评估地图一致性的一个直观方法就是看“静态的平面特征有多厚”。在MLCC中使用了自适应体素化方法进行平面特征提取，并通过提取的平面特征构建局部地图的一致性指标用于优化多激光雷达外参。

论文中展示的自适应平面体素特征，可以发现大块的平面被分割成大的体素，小块的平面也可以被分割出来（比如树叶），这极大提升了标定方案对场景的适应性

为了解决雷达点云稀疏，特征关联困难的问题，在 经典算法详解 | BALM激光点云的BA优化问题 中提出了自适应体素化的特征提取与关联方法：对于任一堆叠出的局部地图先分割为一定大小的体素，对于每一个体素都会计算体素内所有点坐标的covariance，通过判断体素内点的的最大与最小特征值之比()判断这个体素内的点是否为线面特征(如果这个比值很大那么就是面特征，如果比值很小就是线特征)。如果这个体素不满足线面特征的要求，那么就将这个体素分为八个小体素，对每一个体素再判断是不是特征体素。如此递归下去，直到分割的体素尺寸已经足够小，或者递归的层数达到设定的上限。最后的feature map会以八叉树(octo-tree)的数据结构进行存储和访问。

这种方法可以达到快速且精细的点云平面特征提取与关联，可以在很多路端场景下建立约束的同时，避免了频繁建立KD树搜索最近邻的造成的性能开销；此外这种方式还兼容了所有型号雷达的特征提取：只要是个测距准确的仪器，不管什么样的扫描形式都可以用这种方式实现特征的提取与关联，机械式-固态联合标定都可以搞定，保障了标定方案的通用性，真正做到“不挑场景，不挑雷达，不挑安装方式”。

2.2. 优化问题与损失函数的构建：

基于自适应体素提取的面特征地图，我们可以通过遍历八叉树的方式访问每一个体素。对于每一个面特征体素，都可以计算这个体素内所有点云的点面误差。也因此多激光雷达外参联合标定的优化问题的损失函数可以写作

其中代表特征体素中点云的数量，是第个面特征体素中的法向量，是位于这个面上的任意一点。但是这个优化问题自由度非常庞大：如果用四元数做参数化的话，对于一个个雷达的系统就有个变量需要去优化。直接做的话几乎没法收敛，因此会把优化问题拆成以下三个阶段一步步去做：

1. 一阶段：优化主雷达位姿：第一阶段中先优化主雷达里程计。优化之前的初值可以靠纯激光里程计，或者借助其他传感器（如IMU和轮速）的方式获取，使用这个里程计堆叠主雷达的局部地图，并通过自适应提速化的方式提取面特征，通过八叉树递归的方式计算每一个体素的点面误差，构建局部地图的一致性评价指标 :

1. 二阶段优化问题的构建：副雷达标到主雷达。这个阶段优化的是每一个副雷达的外参。将一阶段中优化出的主雷达里程计作为真值，对于每一个副雷达，使用主雷达里程计以及各副雷达到主雷达的外参将各个副雷达的点云投影回世界坐标系构成局部地图，并和一阶段中的地图叠在一起构成新的局部地图。和一阶段相同，对使用自适应体素化方法提取面特征，并对于每一个特征体素递归构建基于点面误差的一致性评价指标

   一般来讲完成了这一阶段的优化之后，local map和单帧其实就已经有了较好的拼接效果。

2. 三阶段优化问题的构建：所有雷达外参，以及主雷达里程计的联合优化。在这一步优化中，使用主雷达里程计以及各副雷达到主雷达的外参将各个副雷达的点云投影回世界坐标系构成局部地图，将所有雷达的局部地图堆叠在一起构成新的局部地图。同样的，对于这个局部地图构建基于点面误差的损失函数：

下面将看到在三个阶段中，虽然各自优化问题的构建和优化的目标不一样，但是其实损失函数的简化与二阶近似是一样的，也就是说这三个阶段的优化其实是可以放在一个框架下进行的。

2.3. 损失函数的简化与二阶近似

我们可以发现三个极端的优化问题中除了我们待优化的位姿以外，还有每个体素中的法向量和体素中的点是没有被确定的。[1]的附录中证明取平面上任何一个点都不会影响最终优化的结果，将选作这个体素内所有点云的质心：时，记矩阵，那么法向量取作最小特征值对应的特征向量，此时优化问题均可以被简化为

这样损失函数就可以被简化为一个跟点云和法向量无关，只和位姿有关的函数，大大缩小了优化的自由度，便于进行下一步的求导，二阶近似与优化。

2.3.1. 简化后损失函数的二阶近似与优化

从式中我们可以看到，位姿的变化引起点云坐标的变化，从而引起局部地图一致性发生变化。因此根据链式求导法则，需要先对点云坐标求扰动，损失函数的二阶近似如下：

其中雅克比矩阵中的每一个元素表示为

假设点坐标的covariance没有重数大于1的特征值，黑塞矩阵中的每一个元素表示为

其中

有了以上的推导之后，可以进一步对位姿求扰动来实现对点坐标的扰动：

其中可以分为和主雷达里程计相关的项，以及和外参相关的项：

通过对损失函数的截取，就可以实现一阶段，二阶段和三阶段的优化。论文中并没有使用ceres等开源优化库进行优化，而是使用了一套自己写的LM进行优化。很大程度上降低了代码的可读性，如果有希望使用这套方案进行多激光联合标定的读者可能需要多琢磨琢磨代码。

3. 激光相机联合标定

在做完了多激光联合标定之后，基于优化的主雷达里程计和优化的雷达外参，可以堆叠出一个由具有高一致性的点云地图，其坐标系在主雷达的第一帧雷达坐标系上。这样的话哪怕激光雷达和相机在安装上没有overlap，也可以通过传感器的运动和地图的构建创造出overlap。然后对点云和图像分别进行边缘特征的提取，将点云中的边缘特征通过相机外参投影到图像进行对应与优化，实现了激光相机外参的联合优化。

3.1. 基于自适应体素特征的点云边缘提取

图像的边缘提取非常成熟，通过canny等方式就可以完成，原论文中也是这么做的。但是点云的边缘提取容易产生很多的outlier，影响后续标定的精度和稳定性。文中作者基于自适应体素的平面特征，提出了一个鲁棒的边缘特征提取方法：通过对比相邻体素的法向量，如果两个平面体素法向量的夹角超过某一阈值，就将这两个平面相交的部分提出作为一个edge。下图就非常直观得展示了这个过程和效果。

3.2. 特征对应与优化问题构建

假设在点云地图中有一条边，其中第个点在全局坐标系下的坐标为，假设所选用的相机模型为，畸变模型为，那么将这个点投影到相机上可以表示为

假设这个数据集中有若干张图片拍到了这个点，记这些图像的集合为,其中表示这是第个相机拍到的，表示图像的编号，那么对于每一个点在图像上的投影都建立KD树，搜索对应图像个最近邻的图像边缘特征点。从这些图像特征点中可以拟合出一个二维边缘特征的法向量，也就是这几个点形成的covarience矩阵的最小特征值所对应的特征向量

因此，对于雷达点云中的一个特征点，它所对应的第帧图像中的最近邻特征点构建出的点线残差为

对所有雷达特征和所有图像，把这个残差进行相加，即完成了优化问题的构建

文中并没有给出，代码中直接通过构建ceres::AutoDiffCostFunction进行自动求导和计算，具体可以看代码。

4. 小结

这套方案给出的自适应体素的方法提取平面和边缘特征是非常实用，以及值得借鉴的。整套标定方案也具有很搞的精度和强鲁棒性。新上手也可以从中学习点云着色等操作，是一套非常值得深入研究的多传感器标定方案。这个但是这套方案不是7自由度，带temporal的标定，对多传感器时间同步要求比较高，而且对场景有点要求，比如那种都是地面点，两边没有约束的场景，就有可能跑飞，或者优化不出来，这个需要注意。

投稿作者为『自动驾驶之心知识星球』特邀嘉宾，如果您希望分享到自动驾驶之心平台，欢迎联系我们！

① 全网独家视频课程

BEV感知、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、协同感知、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码学习）

视频官网：www.zdjszx.com

② 国内首个自动驾驶学习社区

近2000人的交流社区，涉及30+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（2D检测、分割、2D/3D车道线、BEV感知、3D目标检测、Occupancy、多传感器融合、多传感器标定、目标跟踪、光流估计）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

③【自动驾驶之心】技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多模态感知、Occupancy、多传感器融合、transformer、大模型、点云处理、端到端自动驾驶、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向。扫码添加汽车人助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）

④【自动驾驶之心】平台矩阵，欢迎联系我们！