1. 导言：动机与这项成果的优势
对于拥有惯导的自动驾驶车辆而言，激光雷达到惯导/车体（以下简称激光雷达外参自标定）的方法对后续的感知、定位等下游应用具有非常重要的意义。对于这一问题，最容易想到的办法是同时跑激光里程计与惯导的里程计，并使用手眼标定的方法寻找这外参。这个方法最主要的问题是标定精度受制于激光雷达里程计的精度，对于当前量产的固态激光雷达而言，受制于有限的FOV，测距精度较差，抖动，以及多变的标定场景等问题，难以适配鲁棒且高精度的激光里程计用于外参标定。
此外还有另一类激光雷达外参自标定的工具，它们将所有单帧点云通过外参和惯导里程计投影回世界坐标系。假设惯导里程计和激光雷达外参都非常精准，那么投影回世界坐标系得到的局部地图应该拥有相当高的质量，并且最大限度得反映真实世界的场景。当然前提是只能在惯导状态好的场景下使用才能完成对外参的优化。这个想法的代表作的有2015年发布的开源项目lidar align。然而这项成果对激励轨迹有非常高的要求，仅适合无人机、手持式设备的激光雷达自标定场景，而并不适合在自动驾驶车辆上使用。本文介绍2019年发布在《Remote Sensing》上的一项激光雷达外参自标定成果：FeatCalibr，也是属于这一类。在作者的实践中也是一款比较实用的激光雷达外参自标定基线方案。此外它提供了MATLAB代码，到手改改数据接口即可使用，非常值得学习和应用。这项工作的主要优点包括
- 对标定场景没有设定，可以在自然场景下执行激光雷达标到车体（惯导）等位姿传感器
- 通过三维几何特征的使用，可以充分激励各个标定参数，解决自动驾驶车辆运动受限，传感器自标定激励困难的问题
- 通过多尺度方法的引入，提升了标定算法对初始参数的鲁棒性的同时不损失标定精度。在仿真数据以及较大的初始化误差的条件下仍然可以达到平移量毫米级，旋转量0.1deg级别的精度。同时实验表明这款标定算法的结果也具有较好的一致性。是一款值得研究的激光雷达到位姿传感器自标定的算法基线。
2. 方法与流程：
程序输入：
- 激光雷达单帧点云
，其中
代表第个index的点云在雷达坐标系
下的位置，m代表一帧中的点云个数，代表点云帧数。 - 惯导/车体姿态数据
，其中
代表世界坐标系，
代表惯导或者车体坐标系。这个输入需要尽可能保证精度（比如GNSS无遮挡，RTK状态佳），否则优化过程中误差会被吃进外参 - 初始激光雷达到车体（或惯导）的外参
（需要一定程度得接近，比如从数模中获取。因为优化局部地图的这类自标定方法需要借助邻近点云的信息，有点像ICP，对初始化比较敏感） - 其他配置参数：降采样尺度，最大循环次数，tolerance等优化器设置
计算流程：如论文中的图片所示
程序输出：优化的外参（可以支持spatial-temporal标定）
2.1. 局部地图构建：
局部地图的构建非常简单，直接把每一帧雷达坐标系下所有点
都投影到世界坐标系下获得
就可以了。一般以起始时间的车体坐标系作为世界坐标系。首先对于每一帧点云中的每一个点，获取它的时间戳，然后使用这个时间戳对惯导/车体姿态数据进行插值，获取当前时刻惯导在世界坐标系下的位姿
，根据当前外参
可以得到
这样不仅可以将temporal作为变量引入标定流程，同时排除了运动畸变对激光雷达外参标定的影响。通过单纯堆叠点云，就可以获得局部地图：
这样局部地图也就成为了受外参控制的变量。外参的变化会对局部地图的质量造成影响。那么接下来的问题就变成了如何量化得评估地图的质量并构建损失函数
2.2. 特征构建与局部地图质量的评估：
对地图质量评估的基本思路是基于采样点的邻近点计算出的局部特征构建损失函数，通过非线性优化器优化外参，从而最小化损失函数，使地图质量达到最优。
2.2.1. 降采样
本文使用体素滤波的方式对局部地图进行降采样。对局部地图进行降采样的目的主要有以下几点
- 使得采样更加均匀，避免因为单帧点云远近分布不均以及不规则的运动使得地图某些地方稠密，其他地方稀疏，对局部地图质量的量化评估造成不良影响
- 降采样去除微小的目标，避免其对标定过程造成影响
- 可以为标定的流程提速，减少需要处理的点数
记原始的局部地图进行体素滤波降采样之后的地图为
。整个标定过程都是对降采样后的地图进行评估以及优化外参。
2.2.2. 基于局部特征的损失函数构建
为了描述被采样的点
的局部特征，基于该点领域
计算三维协方差矩阵（文中也称三维结构张量），并计算它们的特征值
，
。特征值可以辨别出以下的特征：
- 一维（直线）特征：
，这表明该点邻域中的点云朝一个主轴分布 - 二维（平面）特征：
， 这表明该点邻域中的点云朝两个主轴分布 - 三维特征：
，这表明该点领域中的点云在三维空间中随机，均匀分布
基于三维结构张量的特征值，本文提出了以下一些损失函数构建方法：
- 线性：
- 平面度：
- 圆柱度：
- 各向同性：
- 各向异性：
- 特征熵：
- 曲率变化：
根据标定场景的不同，选择不同的损失函数，比如对于平面特征较多的场景选用平面度作为损失函数，对于拥有较多连续平面的场景选用曲率变化与圆柱度。文中认为通用的选择为圆柱度，各向同性与特征熵，不过一般标定都会选择比较有利的场景进行，还是要根据实际情况进行调整和测试来达到最好的标定效果。假设可供选择的特征集合记作
，对降采样后的局部地图构建损失函数
通过最小化损失函数我们可以最大化构建的局部地图质量，获得对应的优化的外参。
2.3. 影响鲁棒性的问题与对应的tricks：
原文认为两大因素会影响对算法鲁棒性的影响
- 当外参得到优化的过程中，局部地图的质量也在逐渐提高，地图变得越来越清晰和“紧凑”，导致降采样后的点数量会随着优化的循环数量变得越来越少，尤其在最初的几轮优化
- 标定场景和选择的特征不完全符合。比如选择用平面度来构建局部地图质量的损失函数，而场景中出现了很多圆柱形的杆子。此外，点云中的一些噪点也有可能会对标定造成影响。
- 由于初始化和体素滤波降采样的引入带来的问题：如果采样率很高但是标定参数的初始估计很差，那么程序容易像ICP一样容易陷入局部最优，对不同的初始化不鲁棒；但是当采样率很低，地图比较稀疏时，标定结果可能会更加鲁棒，但是损失一定的精度
针对这两个问题，文中提出以下三个tricks：
- 要在每一个循环中都确保采样点的数量都相同：通过对每个点的特征值进行排序并选取排序靠前的若干个点来计算损失函数。以此来保证每次优化时采样点的个数都是相同的，并且去除场景中不那么符合预设的特征物
- 为了增强鲁棒性，降低噪点和不符合场景预设的点带来的影响，我们可以使用鲁棒核函数构建M-estimator。当标定场景不是那么符合我们的预设的损失函数时，M-estimator可以对离群特征点进行过滤并压制离群点带来的影响。比如原文中使用的Huber-loss
其中是一个优化参数。以此构建鲁棒的损失函数
这在各大优化库中也是非常容易实现的，也可以作为程序的一项可调参数进行调节和测试。
- 采用多尺度方法应对降采样带来的问题。文中对尺度的定义为降采样过程中单个体素的大小。顾名思义，也就是换个采样率多标几次：每次优化器收敛之后，就降低采样时的体素大小（提升采样率）再开启新一轮优化。这样就实现了初期不那么容易陷入局部最优，后期可以提升精度的效果。文中实验表明一般固定优化了几次降采样率之后标定结果将不再提升，整个标定过程可以收敛并结束。
3. 实验设计与结果
本文的实验同时具有仿真和实际的户外场景测试。设计仿真实验来分析不同损失函数对于自标定任务的适应程度，标定算法的边界（初始值应该给的多近），以及测试多尺度方法应对初始化与降采样带来问题的可行性。其次在KITTI数据集的自然场景下尝试进行了自标定实验，并与数据集中自带的Reference Calibration进行对比。在仿真和自然场景下作者都设计了对局部地图质量的评判标准作为标定结果好坏的评判标准。
3.1 仿真实验
仿真实验的数据通过Gazebo生成，由于仿真数据拥有ground truth，可以初步验证算法的精度，算法容忍误差的边界以及文中提出的多尺度方法的有效性。
3.1.1. 同一场景下不同三维几何特征标定结果评估
如图中所示，作者使用了不同三维几何特征构建损失函数进行了多次标定测试，并与基于RQE的自标定方法进行比较。结果可以发现使用采样点三维特征评估局部地图质量构建损失函数的方法拥有远好于基于RQE的自标定方法的性能，无论是精度方面还是一致性方面。同时各向异性&特征熵这俩个大特征表现出了非常好的标定性能。这也是为什么作者在前文推荐使用这两个作为通用配置的原因。
3.1.2. 算法边界
对于一个标定算法而言，“初始估计该给的多好”是一个非常有价值的话题。在量产车项目中一般机械加工精度比较有保障，算法能容忍的初始值误差边界可以小一些；而在商用车项目中有时候安装方式比较随意，如果算法不能容忍那么大的初始值误差就会没有什么意义，需要寻找其他的初始化方法带到真值附近然后再进行refine。
从图中可以看出，当初始值平移量差2m，旋转量差30度以下时，这个标定算法依然具有一定的精度。这个对大多数的激光雷达外参自标定需求来说相信应该都是足够的了。当然现实有传感器误差的时候还是要比仿真数据差不少的，这个看看就好。
3.1.3. 降采样与多尺度方法带来的提升
为了更加直观的展示降采样尺度对标定过程带来的影响，以及多尺度方法带来的裨益：
从图中可以看出：当降采样幅度比较剧烈的时候，算法可以容忍很大的初始化误差，但是会损失标定精度；当降采样率比较低的时候，算法的精度会更高，但是就不能容忍很大的初始误差了。而文中提出的多尺度方法却完美解决了这个问题：它同时保证了算法能容忍较大的初始化误差的同时，保证了标定结果的准确性。
3.2. 真实场景测试
本文在室内场景和KITTI数据集的户外大场景进行了测试，将标定结果和基于标志物的激光相机联合标定方法（TC）[1]/基于RQE的自标定方法（RQE）[2]进行对比。作者设计了一个评价标准：当外参完成优化之后生成最终的局部地图，并选取不同方向的六个平面（地上，天花板以及四周），来评价哪个方法产生的外参拥有最薄的平面（也即最佳的局部地图质量）。为了量化得进行评估，作者选取这些平面附近的点计算平均点面误差作为标定精度的评估方式。
3.2.1. 室内，小规模数据
在室内场景中使用手持式设备采集数据并进行标定。可以发现线性度与平面度两个特征基本无法用于标定；在所有特征中使用各向异性和特征熵的表现较好；同时在室内场景标定成功的情况下，Feat Calibr生产出来的外参构建的局部地图质量要远远超过TC与RQE的方法。
除此之外作者挑选了表现最优秀的各向异性特征，与基于RQE的自标定方法一起评估了标定结果的精度和一致性，如下图所示：
结果可以发现，FeatCalibr对比RQE的方法具有很高的一致性。
3.2.2. 户外，大场景自标定测试
文章还在KITTI数据集上进行了测试，在数据集中的Reference Calibration (RC)的基础上在三个旋转方向注入了5°的误差，以及在平移量上手动添加了5cm的误差。最后的标定结果对比RC以及同场景下使用基于RQE的自标定方法跑出来的结果进行对比的结果如下图所示:
可以发现基于各向异性特征获得的标定结果产生的局部地图质量评估获得了和Reference Calibration接近的局部地图质量，并远超基于RQE的自标定方法。但是局部地图质量并不是一个值得被认可的评估方式。作者还是单独展示了标定结果：如下图所示，可视化多次的标定结果可以发现，基于RQE的方法在Y方向（高度方向）误差很大。可能是因为缺乏激励的原因，而本文的方法可以获得这个方向的标定结果。roll和pitch的标定结果和KITTI数据集有较大的不同，但是多组标定值的中位数和KITTI的reference差不超过1°。由此说明了这款自标定算法的可用性。而且从论文中看的局部地图质量也是足够感知，建图与激光定位等下游在多数场景下使用了。
参考
[1] Hillemann, M.; Jutzi, B. UCalMiCeL–Unified intrinsic and extrinsic calibration of a Multi-Camera-System and a Laserscanner. ISPRS Ann. Photogramm. Remote Sens. Spat. Inf. Sci. 2017, 4, 17–24.
[2] Sheehan, M.; Harrison, A.; Newman, P. Self-calibration for a 3D laser. Int. J. Robot. Res. 2012, 31, 675–687.
原文首发微信公众号【自动驾驶之心】：一个专注自动驾驶与AI的社区（https://mp.weixin.qq.com/s/NK-0tfm_5KxmOfFHpK5mBA）
