点击上方“3D视觉车间，选择星标
第一时间送达干货
作者丨paopaoslam
来源泡泡机器人SLAM
Robust Odometry and Mapping for Multi-LiDAR Systems With Online Extrinsic Calibration
Jianhao Jiao, Haoyang Ye, Yilong Zhu and Ming Liu, HKUST RAM-LAB
IEEE Transactions on Robotics, 2021
编译：
审核：阮建源 王志勇
在SLAM在场景中，一个重要的应用方向是结合多个激光雷达的输入并结合多个激光雷达的输入获得更多的测量。本文提出了一个系统，可以利用多激光雷达获得鲁棒和实时外参校准、里程计和建图效果。我们的系统首先提取点云中的边缘和平面点。外参初始化后，我们使用基于滑窗的多激光雷达里程计来估计位置和在线校准，包括校准结果的收敛性判断。通过构建全局地图，结合足够的观测，构建并尝试减少测量和估计的不确定性，我们还提出了一种构图算法来优化位置。我们在足够的实验中验证了我们的校准和SLAM与最先进的方法相比，性能。我们认为提出的方法是一个完整的多激光雷达系统，鲁棒和良好的扩展。
激光雷达设置在激光雷达中的任何激光雷达（以下记录为），其他激光雷达为辅激光雷达(以下记录为）。
优化变量分为两部分：
1. Pr LiDAR的位姿（多帧）
a. N帧历史Pose滑窗中存在(代码中，N=4）
b. 并将旧的P帧设置为优化位置，其余的N-P帧是未优化的位置。(代码中，P=2）。
c. 设置尚未优化的最位置是Pivot 位姿。
综上，M-LOAM滑窗示例可以表示为：
这种建模方法不需要维护Au LiDAR位置，通过Pr LiDAR可以计算出其他雷达的位置。
与传统的LOAM相似，M-LOAM也分为odometry 与 mapping 两个阶段odometry阶段，M-LOAM对激光雷达的外参进行估计和优化，并获得与激光雷达频率一致的定位输出mapping阶段则会考虑三维点的位置不确定性，并且与局部地图点进行匹配来获得一个更加准确的map位姿。
Odometry 分为四个阶段：
没有外参初始值时 ，激光雷达之间的外参用手眼标定。
粗标定结果后，建立优化问题，优化外部参与Pr LiDAR的pose。
若收敛的外参结果足够多(此处将对外参进行收敛判断)，则进入纯里程计阶段。
现阶段，Pr LiDAR与Au LiDAR构建优化问题的特征点。
在尚未获得外参初始值的阶段，M-LOAM通过LOAM算法中的提取edge特征与planar特征和两帧匹配，计算每个激光雷达两帧之间的相对运动，在各自的启动坐标系下积累每个激光雷达的位置。然后建立手眼标定问题AX=XB，其中A和B两个激光雷达在各自的开机坐标系下，X是两个雷达的外参。M-LOAM这里通过堆叠外参中旋转的约束来构建AX=0的等式，通过对矩阵A SVD分解以获得外参中的旋转部分。通过堆叠外参中的平移约束，可以构建和解决最小二乘问题，即可获得位移。
激光雷达之间的外参初始值通过粗标定获得后，构建优化问题。
首先构建Pr LiDAR的Local map，方法是将滑窗内点云合并Pivot帧（Pivot坐标系下定义见优化变量节)。
然后添加特征关联因素来优化问题。
滑窗里的每一个都不是Fixed的帧的Pr LiDAR点云的特点（Edge point 或Planer point)，它直接通过帧的初始开始Pose与Pr LiDAR的Local Map关联（K-d Tree)。优化时会更新帧pose。
滑窗里的每一个都不是Fixed的帧的Au LiDAR点云的特点（Edge point 或Planer point)，将其通过Pr LiDAR的初始Pose以及该Au LiDAR到Pr Lidar的外参，与Pr LiDAR的Local Map关联。优化时，帧不仅会更新Pr LiDAR的pose，还会更新该Au LiDAR与Pr LiDAR的外参。
3. 执行优化。
如何判断优化是否收敛？这个问题可以转化为如何判断优化结果是否退化（即优化结果在一个或多个维度上不够）：如果最终优化结果没有退化，则可以认为是优化收敛。
M-LOAM判断优化是否收敛LOAM作者Ji Zhang16年发表的论文《On degeneracy of optimization-based state estimation problems》一致的思想。在本文中，作者提出使用退化因子lambda判断优化结果是否退化。其中lambda是J^TJJ是优化问题的最小特征值Jacobian。Ji Zhang这篇文章很有意思，感兴趣的同学可以深入了解推进过程，这里就不赘述了。
在外参细化校准过程中，每帧获得的外参优化结果将判断退化因子。如果退化因子大于特定阈值，则认为当前帧的校准结果收敛并保存set中。若set如果外参结果数量超过阈值，则认为外参已标定。set所有外参均为传感器最终固定的外参。此时进入纯里程计阶段。
当校准结果收敛时，优化问题的优化变量和因素发生了变化。具体情况如下：
只有优化变量Pr LiDAR没有Au LiDAR的外参。
为每个Au LiDAR构建Local map，并修改特征关联因素：
第一个因素仍然与外参细标定的第一个因素相同Pr LiDAR的特征点与Pr LiDAR的Local map关联。
第二个因素是，Au LiDAR的特征点与Au LiDAR的local map关联。
在这里，我们可以比较纯里程计和外参细校准阶段的因素之间的区别。最大的区别在于，纯里程计的阶段大多是建立起来的Au LiDAR的local map。试着给出为什么要这样做的理解：
在外参细化阶段，外参尚未固定，此时依赖外参 Pr LiDAR的Pose构建 Au LiDAR的local map，会因的持续变化会导致外参的持续变化local map不一致性。
而添加了Au LiDAR的特征点到Au LiDAR的local map经过特征关联的约束，系统的鲁棒性在一定程度上得到了提高(因为使用了)Au LiDAR观察，增加了更多的约束)。
Mapping阶段相比于Odometry在这个阶段，对三维点位置不确定性的估计更多。
首先，参考论文《Associating uncertinty with three-dimensional poses for use in estimation problems》的方法，根据当前时刻Pr LiDAR的位姿的不确定性，以及Pr LiDAR到Au LiDAR的外参不确定性，计算出Au LiDAR的位姿不确定性。随后按照文中公式（25）计算出Au LiDAR（或Pr LiDAR）的特征点在Map坐标系下的不确定性。
随后构建局部地图。从历史的关键帧中，找到距离当前帧最近的几帧，获取其点云，构建局部地图。
将所有特征点（包括Au LiDAR与Pr LiDAR的）与局部地图的点云做关联。当特征点的位姿不确定性过大时，对其残差会进行相应的缩小，即认为该点提供的信息不可靠。
多激光雷达带来的一大问题就是数据量较大，为此，需要利用尽可能少的的特征点来获得当前的位姿。
M-LOAM中虽然没怎么提到，但是代码中的特征点提取是依赖于作者的另一篇文章《Greedy-Based Feature Selection for Efficient LiDAR SLAM》。其核心思想是，通过计算特征点在残差中的Jacobian J，并且计算信息矩阵A=J^TJ来判断一个特征点的信息量的多少。为此，通过随机选取特征点，并验算其信息量，加入到一个特征点的优先级队列中。这个优先级队列总是维持数量为N的信息量最多的特征点。当场景没有发生退化时（通过计算上文提到的退化因子），N为0.2倍所有特征点的数量，否则N为0.8倍特征点的数量。
最后将优先级队列里面的特征点用于构建优化问题，既减少了计算量，又能够保证足够的位姿精度。
为了验证标定误差，作者提出了两种指标：
与Ground Truth的对比。因为真实世界没有Ground Truth，作者利用了一种PS-Calib的方法来获得。
Mean Map Entropy，平均地图信息熵。如果外参标定的很好，那么多个激光雷达点云拼接起来，每个点（尤其是平面点）的协方差应该比较小。对所有点云求得其协方差的行列式，再取对数平均，便获得了这个值。这个值是越小越好。
实验证明M-LOAM的外参标定优于其他方法。
在该实验中，M-LOAM验证了其在仿真与真实场景下的定位精度（与A-LOAM以及LEGO-LOAM对比）。由于后两种方法没有外参标定，因此给其提供精确的外参，并将多个激光雷达的输入根据外参合并到一起，作为单帧输入。实验表明，M-LOAM总体上有更好的translation 精度以及不错的rotation精度。并且通过消融实验证明了mapping过程中的不确定性传播对精度提升是有一定的正面作用的。
在该实验中，M-LOAM关闭了外参标定模块，并且给外参人为添加不同程度的噪声。结果表明，M-LOAM相比于A-LOAM以及LEGO-LOAM，有更好的抗外参扰动的能力（在粗糙的外参下其定位精度仍然较高）。
通过抽帧来模拟运动更快（3m/s 与9m/s）的场景，并且比较1/2/3/4个激光雷达的SLAM的定位精度。从箱线图中可以看出，激光雷达使用的数量越多，其定位误差越小。
论文中阐述该工作有以下几点不足：
论文中获得的外参标定结果还没足够优秀以获得高精度地图。获得高精度地图的要求是外参标定的误差应小于0.01m与1°。
由于激光雷达的特性，使得其不能够在高速场景下进行SLAM（因为线性运动的假设会导致高速场景下特征关联的失败）。
基于点面的特征选择，使其容易发生场景退化，并且较难应用于场景识别与重定位。
上述的算法流程是在通读论文以及代码后给出的，代码内容与论文流程基本一致，通过可以获得代码的脑图。
https://www.jianguoyun.com/p/DeK6AgMQzcSTBxif4LsEIAA
里程计相关的代码在estimator/src/estimator里，而mapping相关代码在estimator/src/lidarMapper里。
Combining multiple LiDARs enables a robot to maxi-
mize its perceptual awareness ofenvironments and obtain sufficient measurements, which is promising for simultaneous localization and mapping (SLAM).
This article proposes a system to achieve robust and simultaneous extrinsic calibration, odometry, and map- ping for multiple LiDARs. Our approach starts with measurement preprocessing to extract edge and planar features from raw mea- surements. After a motion and extrinsic initialization procedure, a sliding window-based multi-LiDAR odometry runs onboard to estimate poses with an online calibration refinement and conver- gence identification. We further develop a mapping algorithm to construct a global map and optimize poses with sufficient features together with a method to capture and reduce data uncertainty.We validate our approach’s performance with extensive experiments on 10 sequences (4.60-km total length) for the calibration and SLAM and compare it against the state of the art. We demonstrate that the proposed work is a complete, robust, and extensible system for various multi-LiDAR setups. The source code, datasets, and demonstrations are available at: https://ram-lab.com/file/site/m- loam.
本文仅做学术分享，如有侵权，请联系删文。
1.面向自动驾驶领域的多传感器数据融合技术
2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进4.国内首个面向工业级实战的点云处理课程5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)
9.从零搭建一套结构光3D重建系统[理论+源码+实践]
10.单目深度估计方法：算法梳理与代码实现
11.自动驾驶中的深度学习模型部署实战
12.相机模型与标定(单目+双目+鱼眼）
13.重磅！四旋翼飞行器：算法与实战
14.ROS2从入门到精通：理论与实战
15.国内首个3D缺陷检测教程：理论、源码与实战
扫码添加小助手微信，可
也可申请加入我们的细分方向交流群，目前主要有、、、、、等微信群。
一定要备注：，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。也请联系。
▲长按加微信群或投稿
▲长按关注公众号
：针对3D视觉领域的五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、答疑解惑、助你高效解决问题