M-LOAM多激光雷达系统和代码解析(IEEE 2021)

点击上方“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性能，并与最先进的方法做了对比。我们认为所提出的方法是一个完备、鲁棒并且拓展性好的多激光雷达系统。

方法论

A.优化变量

设多激光雷达中的任意一激光雷达为主激光雷达（以下记为Pr LiDAR），其他剩下的激光雷达为辅激光雷达（以下记为Au Lidar）。

优化变量分为两部分：

1. Pr LiDAR的位姿（多帧）

    a. 将历史N帧的Pose存在滑窗中（代码中，N=4）

    b. 并且设置较老的P帧为已经优化好的位姿，剩下的N-P帧为未优化好的位姿。（代码中，P=2）。

    c. 设尚未优化好的最老的一帧位姿为Pivot 位姿。

综上，M-LOAM的一个滑窗示例可以表示为：

||||    Fixed_pose(t-3), Fixed_pose(t-2), pivot_pose(t-1), pivot_pose(t)    ||||

2. 其他Au LiDAR到Pr LiDAR的外参。

这样的建模方式便可以不用维护Au LiDAR的位姿，通过Pr LiDAR的位姿即可计算出其他雷达的位姿。

与传统的LOAM相似，M-LOAM也分为odometry 与 mapping 两个阶段。在odometry阶段，M-LOAM估计与优化激光雷达的外参，并且获得与激光雷达频率一致的定位输出，而在mapping阶段则会考虑三维点的位置不确定性，并且与局部地图点进行匹配来获得一个更加准确的map位姿。

B.Odometry 算法流程

Odometry 分为四个阶段：

1. 外参粗标定

   在没有外参初始值的时候 ，利用手眼标定的方式求激光雷达之间的外参。

2. 外参细标定

   当有了粗标定的结果后，构建优化问题，同时优化外参与Pr LiDAR的pose。

3. 外参标定收敛性判断。

   如果得到的收敛的外参结果足够多（这里会进行外参的收敛性判断），则进入纯里程计阶段。

4. 纯里程计

   在这个阶段，Pr LiDAR与Au LiDAR的特征点都会被用来构建优化问题。

B.1 外参粗标定

在尚未获得外参初始值的阶段，M-LOAM通过LOAM算法里的提取edge特征与planar特征并进行两两帧匹配的方式，算出每个激光雷达的两两帧之间的相对运动，累积得到每个激光雷达在各自的开机坐标系下的位姿。随后构建手眼标定问题AX=XB，其中A和B是要标定的两个激光雷达各自在各自的开机坐标系下的位姿，而X是这两个雷达的外参。M-LOAM这里通过堆叠外参中旋转的约束，构建AX=0的等式，通过对矩阵A SVD分解来获得外参中的旋转部分。当求得旋转之后，通过堆叠外参中的平移约束可以构建与求解最小二乘问题，即可求得位移。

B.2 外参细标定

通过粗标定获得激光雷达之间的外参初始值后，构建优化问题。

1. 首先构建Pr LiDAR的Local map，方法是将滑窗内点云都合并到Pivot帧（Pivot定义见优化变量节）的坐标系下。

2. 随后为优化问题添加特征关联因子。

   1. 对于滑窗内每个不是Fixed的帧的Pr LiDAR的点云的特征点（Edge point 或Planer point)，将其直接通过该帧的初始Pose与Pr LiDAR的Local Map关联（K-d Tree)。则优化时会更新该帧的pose。

   2. 对于滑窗内每个不是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. 执行优化。

B.3 外参收敛性判断

如何判断优化是否收敛？这个问题可以转化为如何判断优化结果是否出现退化（即优化的结果在某一个或多个维度约束不够）：如果最后优化结果没有出现退化，则可以认为优化收敛。

M-LOAM判断优化是否收敛，采用了与LOAM作者Ji Zhang在16年发表的论文《On degeneracy of optimization-based state estimation problems》一致的思想。在这篇文章中，作者提出了利用退化因子lambda来判断优化的结果是否发生退化。其中lambda是J^TJ的最小特征值，而J是优化问题的Jacobian。Ji Zhang的这篇文章很有意思，有兴趣的同学可以深入了解一下推到过程，这里就不赘述了。

当外参细标定的过程中，每一帧得到的外参优化结果都会进行退化因子的判断，若退化因子大于特定的阈值，则认为当前帧的标定结果收敛，将其保存到set中。若set中的外参结果数量超过阈值，则认为外参已经标定完成。将set中的所有外参取平均，作为传感器最后固定的外参。此时进入纯里程计阶段。

B.4 纯里程计

当标定结果收敛后，所构建的优化问题的优化变量与因子都发生了变化。具体为：

1. 优化变量只有Pr LiDAR的位姿，而没有Au LiDAR的外参。

2. 为每个Au LiDAR构建Local map，并且修改特征关联因子：

   1. 第一个因子仍与外参细标定的第一个因子相同，将Pr LiDAR的特征点与Pr LiDAR的Local map关联。

   2. 第二个因子为，Au LiDAR的特征点与Au LiDAR的local map关联。

这里可以比较纯里程计与外参细标定阶段的因子的不同之处。最大的不同在于，纯里程计的阶段多构建了Au LiDAR的local map。这里试着给出为什么这么做的理解：

1. 在外参细标定阶段，外参还没有固定，此时依赖外参+Pr LiDAR的Pose构建 Au LiDAR的local map，会因为外参持续变化导致local map的不一致性。

2. 而添加了Au LiDAR的特征点到Au LiDAR的local map的特征关联的约束后，一定程度上提升了系统的鲁棒性（因为用上了Au LiDAR的观测，增加了更多的约束）。

C. Mapping 算法流程

Mapping阶段相比于Odometry阶段而言，多了对三维点的位姿不确定性的估计。

1. 首先，参照论文《Associating uncertainty with three-dimensional poses for use in estimation problems》的方法，根据当前时刻Pr LiDAR的位姿的不确定性，以及Pr LiDAR到Au LiDAR的外参不确定性，计算出Au LiDAR的位姿不确定性。随后按照文中公式（25）计算出Au LiDAR（或Pr LiDAR）的特征点在Map坐标系下的不确定性。

2. 随后构建局部地图。从历史的关键帧中，找到距离当前帧最近的几帧，获取其点云，构建局部地图。

3. 将所有特征点（包括Au LiDAR与Pr LiDAR的）与局部地图的点云做关联。当特征点的位姿不确定性过大时，对其残差会进行相应的缩小，即认为该点提供的信息不可靠。

D.基于贪心算法的特征点选择

多激光雷达带来的一大问题就是数据量较大，为此，需要利用尽可能少的的特征点来获得当前的位姿。

M-LOAM中虽然没怎么提到，但是代码中的特征点提取是依赖于作者的另一篇文章《Greedy-Based Feature Selection for Efficient LiDAR SLAM》。其核心思想是，通过计算特征点在残差中的Jacobian J，并且计算信息矩阵A=J^TJ来判断一个特征点的信息量的多少。为此，通过随机选取特征点，并验算其信息量，加入到一个特征点的优先级队列中。这个优先级队列总是维持数量为N的信息量最多的特征点。当场景没有发生退化时（通过计算上文提到的退化因子），N为0.2倍所有特征点的数量，否则N为0.8倍特征点的数量。

最后将优先级队列里面的特征点用于构建优化问题，既减少了计算量，又能够保证足够的位姿精度。

E. 实验

E1. 标定误差的验证。

    为了验证标定误差，作者提出了两种指标：

1. 与Ground Truth的对比。因为真实世界没有Ground Truth，作者利用了一种PS-Calib的方法来获得。

2. Mean Map Entropy，平均地图信息熵。如果外参标定的很好，那么多个激光雷达点云拼接起来，每个点（尤其是平面点）的协方差应该比较小。对所有点云求得其协方差的行列式，再取对数平均，便获得了这个值。这个值是越小越好。

实验证明M-LOAM的外参标定优于其他方法。

E2. SLAM的精度

    在该实验中，M-LOAM验证了其在仿真与真实场景下的定位精度（与A-LOAM以及LEGO-LOAM对比）。由于后两种方法没有外参标定，因此给其提供精确的外参，并将多个激光雷达的输入根据外参合并到一起，作为单帧输入。实验表明，M-LOAM总体上有更好的translation 精度以及不错的rotation精度。并且通过消融实验证明了mapping过程中的不确定性传播对精度提升是有一定的正面作用的。

E3. 外参扰动实验

    在该实验中，M-LOAM关闭了外参标定模块，并且给外参人为添加不同程度的噪声。结果表明，M-LOAM相比于A-LOAM以及LEGO-LOAM，有更好的抗外参扰动的能力（在粗糙的外参下其定位精度仍然较高）。

E4 单个激光雷达与多激光雷达对比实验

通过抽帧来模拟运动更快（3m/s 与9m/s）的场景，并且比较1/2/3/4个激光雷达的SLAM的定位精度。从箱线图中可以看出，激光雷达使用的数量越多，其定位误差越小。

F 不足

论文中阐述该工作有以下几点不足：

1. 论文中获得的外参标定结果还没足够优秀以获得高精度地图。获得高精度地图的要求是外参标定的误差应小于0.01m与1°。

2. 由于激光雷达的特性，使得其不能够在高速场景下进行SLAM（因为线性运动的假设会导致高速场景下特征关联的失败）。

3. 基于点面的特征选择，使其容易发生场景退化，并且较难应用于场景识别与重定位。

G 代码相关

上述的算法流程是在通读论文以及代码后给出的，代码内容与论文流程基本一致，通过点击原文链接可以获得代码的脑图。

https://www.jianguoyun.com/p/DeK6AgMQzcSTBxif4LsEIAA

里程计相关的代码在estimator/src/estimator里，而mapping相关代码在estimator/src/lidarMapper里。

Abstract

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.

本文仅做学术分享，如有侵权，请联系删文。

3D视觉工坊精品课程官网：3dcver.com

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缺陷检测教程：理论、源码与实战

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的视频课程（三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等）、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近4000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~