loam_livox_loop 论文翻译

A fast, complete, point cloud based loop closure for LiDAR odometry and mapping

摘要:本文提出了一种闭环校正激光雷达里程表测量(LOAM)中长期漂移的方法。

我们提出的方法计算关键帧的二维直方图，一个局部的地图补丁，并使用二维直方图的归一化互相关作为当前关键帧与地图中的关键帧之间的相似性度量。我们证明了这种方法是快速的，不旋转，并产生可靠和准确的环路检测。该方法经过精心的工程实现，并集成到LOAM算法中，形成了一个完整而实用的系统。为了通过为闭环提供基准来造福社区，整个系统在Github上是开源的。

一、介绍

利用激光雷达传感器的SLAM方法具有估计6自由度(DOF)状态的能力，同时能够建立周围环境的高精度地图，被认为是一种精确可靠的机器人感知方法。近年来，激光雷达里程测量与测绘(LOAM)已成功应用于机器人领域，如自动驾驶汽车[1]、自动驾驶无人机[2,3]、野外机器人测绘[4,5]等。本文主要研究基于激光的SLAM系统中快速完整闭环系统的开发问题。

闭环是SLAM系统的重要组成部分，用来估计局部特征匹配引起的长期累积漂移。在一个常见的循环闭合范式中，成功检测循环起着至关重要的作用。循环检测是一种通过测量任意两个地方之间的相似性来识别之前访问过的地方的能力。对于视觉-slam方法，闭环从各种广泛可用的计算机视觉算法中获益良多。例如，利用词袋模型[6,7]，将特征描述符聚类为词，可以在词空间中计算观察值之间的相似性。这种方法已被用于大多数先进的可视化SLAM系统中(如[8,9])，并在过去几年取得了巨大的成功。

与visual-SLAM不同的是，基于激光的闭环的相关研究很少，而且我们很难找到任何可用的开放式解决方案来解决这一问题。我们将这些现象归结为两个主要原因:首先，与相机传感器相比，激光雷达传感器的成本非常昂贵，阻碍了它们的广泛使用。在大多数机器人导航感知中，激光雷达并不总是首选。其次，点云上的位置识别问题非常具有挑战性。与包含丰富的纹理、颜色等信息的二维图像不同，点云中的可用信息只是三维空间中的几何形状。

在本文中，我们开发了一个快速、完整的激光雷达里程表测量与测绘(LOAM)闭环系统，包括快速环路检测、地图对齐和位姿图优化。我们将所提出的环路闭合方法集成到LOAM算法中，并使用Livox MID402传感器，这是一种易于获得的高性能低成本激光雷达传感器。我们得到的一些结果如图1和图2所示。为了促进基于激光的slam方法的发展，我们将开放Github上所有的数据集和代码

二、相关工作

在visual-SLAM中，环路闭合被广泛用于纠正其长期漂移。常用的管道主要包括三个步骤:首先，二维图像的局部特征提取。通过使用手工描述符，如缩放不变特征变换(SIFT)[10]、二进制鲁棒独立基本特征(BRIEF)[11]、定向快速旋转BRIEF(ORB)[12]等。然后使用bag-of - world模型[6,7]对这些特征进行聚类，建立字典，实现环路检测。最后，利用检测到的环路，建立了位姿图优化模型，并根据历史位姿图进行优化求解。

与视觉SLAM不同，点云数据的环路检测仍然是基于激光SLAM的一个开放的、具有挑战性的问题。Bosse等[13]通过直接从3D点云中提取关键点并使用手工制作的3D格式塔描述符描述它们来实现位置识别。然后，关键点投票选出离它们最近的点，分数用于确定是否检测到循环。[14]中也使用了类似的方法。Magnusson等人[15]利用正态分布变换(NDT)描述了3D点云的外观，他们从NDT描述符的直方图中计算两个扫描的相似度。

除了手工制作的描述符，基于学习的方法近年来也被用于循环检测(或位置识别)。例如，Dube等人[16]提出的SegMatch通过匹配建筑、树、车辆等语义特征来实现地点识别。Angelina等[17]通过从端到端网络中提取全局描述符来实现位置识别，该描述符通过结合PointNet[18]和NetVLAD[19]来训练。基于学习的方法计算量大，训练过程中对数据集的依赖程度高。

尽管有了这些评审工作，但据我们所知，还没有开源代码或数据集来测试LOAM的循环关闭问题，这给读者在复制他们的工作时留下了一些困难。为此，我们提出了一个完整的闭环系统。我们工作中的环路检测主要受到[15]方法的启发，并在我们的场景中做了一些调整。

由于我们考虑的激光雷达传感器的小视场和特殊的扫描模式，我们对扫描的累积时间(即关键帧)执行循环检测。综上所述，我们的贡献主要体现在三个方面:(1)我们开发了一种快速的循环检测方法来快速测量两个关键帧的相似度;(2)我们将环路闭合系统(包括环路检测、地图对齐和位姿优化)集成到激光雷达里程计和映射算法(LOAM)[20]中，形成了一个完整和实用的系统;(3)通过在Github上开放我们的系统和数据集，我们为基于点云的闭环提供了一个可用的解决方案和范式。

三、系统

我们的系统工作流程如图3所示，每一个从LiDAR输入的新帧(即扫描)通过LOAM算法[20]注册到全局地图。如果接收到指定数量的帧(例如，100帧)，则创建一个关键帧，它形成一个小的本地映射补丁。由LOAM注册到全局映射(第IV节)的单元格的原始点对应于新的关键帧被检索来计算其2D直方图(见第V节)。将计算出的二维直方图与数据库进行比较，数据库包含由所有过去关键帧组成的全局地图的二维直方图，以检测可能的循环(见第六节)。同时，将新的关键帧二维直方图添加到数据库中，以使用下一个关键帧。一旦检测到循环，关键帧与全局映射对齐，并执行位姿图优化以纠正全局映射中的漂移。

四、map和cell

在本节中，我们将介绍算法的两个关键元素，地图和单元格。

为了方便起见，我们使用M和C分别表示地图和单元格。

A.cell

单元格是通过对三维空间的划分而形成的固定大小(即x、y、z方向上的Sx、Sy、Sz)的小立方体。它由中心位置Cc表示，由其中的第一个点Pi = [Pix, Piy, Piz]T创建。

设N为位于细胞Cc中的点数，该细胞的平均Cµ和协方差CΣ为:

注意，单元格是三维空间的一个固定分区，它不断被新的点填充。为了加快均值2和协方差3的计算速度，我们推导出其递归形式如下。PN+1为新点，N为细胞内现有点个数，平均C0µ，协方差为C0Σ。细胞内所有N + 1点的平均Cµ和协方差CΣ分别为:

因此，细胞C由其静态中心Cc、动态更新的平均Cµ和协方差CΣ、原始点集合{Pi}: C = (Cc, Cµ，CΣ， {Pi})组成。

B、地图

地图M是保存在单元格中的所有原始点的集合。更具体地说，M由哈希表H和全局八叉树O组成。哈希表H使我们能够根据其中心Cc快速找到特定的单元格。八叉树O使我们能够找到位于给定范围内特定区域内的所有单元格。这两点对于加快地图对齐是非常重要的。

对于任何新添加的单元格C，我们使用单个组件哈希索引的异或操作(Ccx, Ccy, Ccz)计算其哈希索引H(Cc)。然后将计算得到的哈希索引添加到地图h的哈希表中。因为单元格是3D空间的固定分区，它的中心位置Cc是静态的，不需要对哈希表中的现有条目进行更新(哈希表是动态增长的)。

新添加的单元格C也根据其中心位置添加到八叉树O中，类似于[21]中的八叉树地图。算法1演示了从新帧增量创建单元格和映射的过程。

五、旋转不变量的二维直方图

快速循环检测的主要思想是，我们使用二维类似图像的直方图来粗略描述关键帧。二维直方图描述了关键帧内特征方向的欧拉角分布。

A. 单元格中的特征类型和方向。

如前所述，每个关键帧由若干帧(如100)组成，每个帧(即扫描)被划分为单元格。对于每个单元格，我们确定其点形成的形状和相关的特征方向(表示为Cd)。与[15]类似，我们对(3)中的协方差矩阵CΣ进行特征值分解:。

其中Λ为特征值由高到低的对角矩阵(即Λ 1≥Λ 2≥Λ 3)。在实际操作中，我们只考虑有5个或更多点的单元，以增加鲁棒性。

•平面形状单元格:如果λ2显著大于λ3，则认为该单元格为平面形状，并将平面法线作为特征方向，即Cd = V3，其中V3为矩阵V的第三列。

•线形单元格:如果单元格不是平面，且λ1明显大于λ2，则将该单元格视为线形，并将线形方向作为特征方向，即V的第一列Cd = V1。

•无特征的单元格:既不是直线形状也不是平面形状的单元格不被考虑。

B.旋转不变性

为了使我们的特征描述符不受关键帧任意旋转的影响，我们将每个特征方向Cd乘以一个额外的旋转矩阵R来进行旋转，并期望大部分特征方向位于x轴上，其次大部分特征方向位于Y轴上。由于平面特征比线特征更可靠(例如，将平面特征的边缘视为线特征)，我们使用该特征平面单元的方向确定旋转矩阵R。与前几节类似，我们计算关键帧中所有平面特征方向的协方差Σd:

其中N为平面单元数，Cid为第i个平面单元的特征方向(即平面法线)。同理，Σd的特征值分解为:。

其中Λd是特征值按降序排列的对角矩阵(λd1≥λd2≥λd3)， Vd = ?Vd1 Vd2 Vd3 ?是特征向量矩阵。则旋转矩阵R确定为:

在计算旋转矩阵R后，我们对所有的特征方向(平面和线)进行旋转变换。

C.关键帧二维直方图。

利用关键帧内所有单元格的旋转不变的特征方向，我们计算二维直方图如下:

首先，对于给定的特征方向Cd = [Cdx, Cdy, Cdz]，选择X分量为正的方向，即Cd =符号(Cdx)·Cd，然后计算特征方向的欧拉角(如图4所示):

图四：一个特征方向的欧拉角及其对二维直方图的贡献，二维直方图的每个元素都是位于对应bin中的具有俯仰θ和偏航φ的特征方向的个数。

我们使用的2d直方图是一个60 × 60的矩阵(在俯仰和偏航角上都有3◦分辨率)，这个矩阵的元素表示线/平面单元的数量，其俯仰θ和偏航φ位于相应的bin中。例如，第i行第j列元素eij为其特征方向角满足的单元数:

为了增加二维直方图对可能的噪声的鲁棒性，我们对每个计算的二维直方图应用高斯模糊。

计算具有旋转不变性的二维直方图的完整算法见算法2。

六、快速环路检测

A.环路检测的过程。

如前所述，我们将n帧(例如n = 100)分组到一个关键帧f中。它可以被视为全局地图M的局部补丁，包含了最后n帧中出现的所有单元格，如图5所示。我们计算一个新的关键帧F的二维直方图，以及它与过去所有关键帧的相似性(第VI. B节)，以检测一个循环。然后将具有检测到的循环的关键帧与映射匹配(第VI. C节)，并使用位姿图优化更新映射(第VI. D节)。

B.两个关键帧的相似性。

对于每一个新添加的关键帧，我们测量它与所有历史关键帧的相似度。在本研究中，我们使用二维直方图的归一化互相关来计算它们的相似度，该方法已广泛应用于计算机视觉领域(如模板匹配、图像跟踪、。两个二维直方图H1, H2的相似度S(H1, H2)计算为:

其中¯Hk = 1 N p1 Hk(I)是Hk的均值，I = (I, j)是Hk中元素的索引。如果两个关键帧之间的相似度S(H1, H2)高于阈值(例如，平面为0.90，直线为0.65)，则认为检测到环路。

c .地图定位。

在成功检测到一个循环之后，我们执行映射对齐来计算两个关键帧之间的相对位姿。地图对齐问题可以看作是目标点云和源点云之间的配准问题，是他们[22]的工作。

由于在LOAM算法[20]中对线性形状和平面形状的单元格进行了分类，我们利用边对边和平面对平面的特征来迭代求解相对位姿。

对齐后，如果上的边/面特征点的平均距离足够接近(距离小于0.1m)，则认为这两幅图对齐了。

D.位姿图优化

如图3所示，一旦两个关键帧对齐，我们按照[23,24]中的方法进行位姿图优化。我们使用谷歌ceres-solver3实现了图优化。在优化姿态图后，我们通过重新计算包含的点、点的均值和协方差来更新整个地图中的所有单元格。

7、结果

a .关键帧、单元格和二维直方图的可视化

我们可视化了两个关键帧，它们相关的二维直方图局部映射，以及图6(a)中包含的单元格。这张图显示了两个不同场景的2D直方图非常有特色。

B.旋转不变性

我们通过计算图6(a)中两个场景随机旋转的相似度来评估我们环路检测方法的旋转不变性。对于每个旋转级别，我们生成50个方向随机但大小相同的随机旋转矩阵，用生成的旋转矩阵旋转两个场景中的一个，并计算所有50个大小相同的旋转之间的平均相似度。关键帧A与自身、关键帧B与自身、关键帧A与关键帧B的相似度如图6(B)所示。可以看出，平面特征的相似度在不同或旋转幅度下基本保持一致，同一关键帧(任意旋转)的相似度不断高于不同关键帧的相似度。对于直线特征，虽然在发生旋转时，同一关键帧的相似度略有下降，但仍然显著高于不同关键帧的相似度。

C.计算时间

我们在两个平台上评估系统的每个步骤的时间消耗:桌面PC(配备i7-9700K)和板载计算机(配备i7-8550U的DJI流管24)。我们算法在科大大规模数据集(图7第一列)上的平均运行时间见表。I，我们可以看到我们提出的方法是快速的，适用于两个平台上的实时场景。

D.大规模闭环结果

我们在图7中的四个数据集上测试我们的算法，其中第一行是闭环之前(绿色实线)和闭环之后(蓝色出售线)的轨迹比较，红色虚线表示检测到的环路。第二行图是闭环前(红色)和闭环后(白色)的点云图对比，可以看出闭环可以有效减少LiDAR里程表和测绘的漂移(特别是在黄圈内的区域)。我们将闭环后的点云与第三行的google maps进行对齐，可以看到对齐非常准确，说明我们系统的精度是很高的。

图5:一个关键帧由n(例如n = 100)帧(没有显示)组成，然后包含许多单元格。每个关键帧有两个二维直方图，一个用于线单元格，另一个用于平面单元格。