SLAMesh: Real-time LiDAR Simultaneous Localization and Meshing 论文阅读

Abstract：

1. 第一个仅cpu的实时LiDAR SLAM系统，该系统可以同时构建网格图并根据网格图进行定位。
2. 在几个公共数据集上进行实验。结果表明，我们的SLAM系统可以运行在40Hz左右。
3. 定位和网格精度也优于最先进的方法，包括TSDF地图和泊松重建

1. INTRODUCTION

网格是由面、边和顶点组成的三维实体表面。在三维建模领域，三角网格以其简单且能逼近大多数复杂的三维结构而成为主要的表示形式。与网格地图不同，网格地图减轻了离散化问题，可以为机器人应用模拟光滑表面。

与冲浪图和无损检测图相比，网格图还可以描绘流形结构。此外，mesh map具有内存效率高、可扩展、保留拓扑信息等优点[12][13]。

然而，实时三维激光雷达网格映射目前仍难以实现，特别是在大尺度环境下。主要的困难是网格的建立和更新通常是耗时的。一个被广泛采用的解决方案是首先关注点云映射，然后使用点云图单独执行网格划分。这种两步解决方案不能使定位和网格划分相互受益[14]。

为了解决这个问题，本文设计了一个同时定位和网格划分的系统SLAMesh。该系统可以在只有一个CPU的情况下实时运行。在SLAMesh中，我们使顶点均匀分布，使网格构建，注册和更新更有效。如图1所示为高斯过程(GP)[15]重构后的网格图。我们之前的研究[16][17]已经证明了在SLAM中使用GP的好处，特别是在距离数据稀疏的情况下。然而，他们产生点云地图没有导出曲面模型。此外，当使用64波束激光雷达等具有更密集点云的传感器时，它们无法实现实时性能。本工作利用GP进行网格地图构建，是GP- slam +的升级版[17]。本文的主要贡献如下:

• 提出了一种基于GP重构和顶点连接的网格划分策略，实现了网格地图的快速构建、查询和更新。
• 我们设计了一种点到网格的配准方法。结合约束组合和多线程实现，保证了网格图定位和映射的效率和准确性。
• 在网格地图上开发了一个密集、实时、可扩展的开源LiDAR SLAM系统，并通过实验证明了该系统的优势。

II. RELATED WORK

一种被广泛采用的网格构建思路是将深度数据融合到TSDF地图中，然后通过行军立方体算法18按需提取隐式网格。相反，有些方法显式地生成网格图。Ryde等[20]对点云进行体素化，并在每个体素内回归一个平面形成多边形。Piazza等[21]利用Delaunay三角剖分[22]对稀疏特征点进行网格构建和更新。Schreiberhuber等[23]将相机图像中的深度点连接起来，然后将局部网格块拼接在一起。我们的工作明确地基于GP生成网格地图。与[23]不同的是，我们的SLAMesh处理无序的点云，并在世界帧中构建网格地图，而不是合并不同图像帧中的网格补丁。

上述基于tsdf的方法中的光线跟踪模型忽略了局部表面连续的事实。

相反，GP建立的是一个带有不确定性的连续模型。这一特性吸引人们利用GP来恢复占用场[28]和TSDF场[29]，这两个领域自然是连续的。与[28][29]不同的是，我们使用GP来预测曲面的欧氏坐标。此外，我们的系统也可以适用于大规模的环境。

III. THE PROPOSED APPROACH

A. Approach Overview

这项工作的动机是在激光雷达SLAM系统中建立、利用和维护网格图。图2说明了总体概况。该系统主要由网格划分、配准和网格管理三个部分组成。首先，使用恒速模型的初始猜测将每个新的LiDAR扫描转换为世界框架fWg as S raw。以下操作在fWg中也可执行。然后，将点分配到体素单元中。GP对每个单元内部的局部曲面进行重构，得到顶点vi，这些顶点连接起来形成一个网格。

在配准组件中，设计了点对网格的配准，将重建的当前扫描图像与构建的网格图m对齐，最后迭代更新网格图。

B. Meshing Strategy

如上所述，构建和更新网格非常耗时。为了解决这个问题，我们采用了重构和连接策略，以方便后续的管道，使整个系统能够实时运行。如图2所示。GP从体素内的噪声和稀疏点云中恢复局部表面。然后，这些顶点是曲面的插值结果。三维顶点的两个坐标是均匀分布的(称为locations)，另一个坐标(称为prediction)具有连续的值域。这些位置用作索引，以便在恒定时间内实现快速查询。预测的值域是连续的，以避免离散化引起的精度损失。

这里对GP过程进行了描述(关于GP的更详细的描述可以在[16][17]中找到)。粗体小写字母表示向量，大写字母表示矩阵。GP的输入是原始点云的子集
包含ni个点，其中k表示当前扫描中的第k个单元格Ck，
为输入噪声的各向同性方差。输出是包含$n_j$个顶点的带有不确定性${\sigma }_j^2$层$L_k^z=$
上标z表示GP预测了坐标z作为
,当坐标可以是x, y或z中的任意一个时，省略它。我们将输入点和输出点集的位置分别表示为i和j。给定输入观测值f，预测值$\tilde{f}$也遵循高斯分布。$\tilde{f}$的期望为:

预测的不确定性就是它的方差:

其中kjj、kij和Kii表示位置核函数的不同组合。例如,

其中k (i，j)是一个尺度值，κ是一个常数(在我们的算法中κ = 1)。该指数核函数可以表示二维流形中的局部光滑曲面。因此，当建模一个复杂的结构时，一个单元可以包含更多具有不同GP函数的网格层(如果所有坐标都被插值，则最多可包含3个网格层)[17]。

在一定阈值下，具有不确定性$\sigma {{}^2}_j$的顶点匹配是足够准确或有效的。三角网格面是通过连接位置的二维空间中的相邻或对角顶点来构建的。如果所有顶点都有效，则网格面是有效的。类似于在二维空间中建立边缘的Delaunay三角剖分，我们的方法可以防止不良形状的网格面。

C. Point-to-Mesh Registration

如上所述，我们同时进行定位和网格划分，以便网格划分有益于定位。为此，一个直观的想法是将顶点视为点或从表面中提取点，然后使用传统的点云配准进行姿态估计。然而，这种想法忽略了网格面的法线信息。Puma[3]表明，点对网格误差可以提高精度。与Puma中的光线投射数据关联不同，我们的SLAMesh基于位置建立对应关系。

对于在重建的当前帧$S^{gp}$的顶点$v_p$，我们首先查询位于相同或相邻单元中的子网格层(见图3)，然后找到共享相同位置的顶点$v_q$(见图2(d))。在$v_p$和包含$v_q$的有效面之间建立数据关联。在网格中沿边缘找到$n_q$个相邻的顶点是很快的。包含$v_q$的面的法线被平均以得到光滑的法线$\overline{n}$，以防表面凹凸不平:

$\parallel .\parallel$是2范数。我们将一个单元格内对应的个数记为$n_q$，重叠单元格的个数记为K。点$v_p^{{}^{\prime }}$和对$v_p$进行变换$T\in SE3$(包括旋转R和移动t)后的点到网格的残差为:

上面的T表示矩阵的转置。在所有K个重叠层的点网格残差$L_k$连接的优化问题中，计算最优相对变换T:

我们使用Ceres solver1中的Levenberg-Marquardt算法来解决这个非线性最小二乘问题。推导解析雅可比矩阵，加快求解过程:

其中符号$(\cdot {)}_{\times }$表示向量对应的偏对称矩阵。网格可能包含许多面，因此优化问题可能很大。我们在优化前将每一层内部的残差均值合并为一，加快了优化过程。

在体素单元中存储数据会引起边界上数据关联的不连续。如果只对重叠的单元格进行配准，当车辆快速移动时，配准找到全局最优解的可能性变小。因此，如图3所示，我们通过在每一层的预测轴上查询单元格，允许跨单元格重叠。当配准过程趋于收敛时，查询的长度会减少。

D. Mesh Management and Multithreading

网格地图比点云地图和基于网格的地图更难维护。点和网格可以独立于它们的邻域。相反，网格保持了顶点相互连接的拓扑结构。地图更新过程应该保持现有的连接，并避免不良网格形状。典型的解决方案包括使用Delaunay三角剖分法重新网格划分[21]，保持类似于Voxblox[4]的隐式域，或者像Puma[3]一样重复网格划分过程。SLAM要求网格的快速更新。在我们的SLAMesh中，顶点的位置是固定的，所以只需要更新1-D预测。对于小于${\sigma }_{update}^2$阈值的前t个数据，新的预测可以用迭代最小二乘法求解:

单元存储在散列映射中，这确保了插入、删除和查询的线性复杂性。此外，这种结构是灵活的，因为它可以增量增长。我们与VGICP[30]共享的基于细胞的地图的一个优势是并行处理的兼容性。由于每个单元在这些步骤中是独立的，我们可以使用多线程加速GP重建和网格发布过程。

IV. EXPERIMENTAL RESULTS AND DISCUSSIONS

第一个实验从精度和完整性两个方面对网格质量进行了评价。我们使用Mai City数据集[3]，该数据集建立在CARLA模拟环境上，具有地图的地面真值。在这个数据集中，传感器是一个模拟的64波束Velodyne HDL-64E激光雷达。车辆行驶99米，产生100帧激光雷达扫描。环境的真实情况是由高分辨率传感器扫描的密集点云。我们评估了Voxblox与A-LOAM2、Puma和SLAMesh的组合。由于这项工作的目的是建立一个SLAM系统，而不是一个离线测绘工具，并且地面真实姿态通常在真实环境中不可用，因此我们使用每个管道中估计的姿态或额外的里程计系统(即使用Voxblox的ALOAM)。

图1给出了每种方法生成的网格图。所有的地图都很好地重建了主要结构。在放大的视图中，我们可以观察到每种策略的特点。SLAMesh和Voxblox中的顶点由于重建或体素化而均匀组织。用泊松法重建Puma网是水密的（网格的所有表面都是封闭的），但由于存在多层现象，网格结构相对复杂。我们认为这是因为SLAMesh和Voxblox都是迭代地将观察结果融合到表面上，而Puma则累积了几次扫描，然后对它们进行网格划分。

A. Meshing Evaluation

定量评估使用标准的基于点云的指标:Precision、Recall和F1-score[31]。我们对网格图进行密集均匀采样，形成点云，然后与地面真实点云进行比较。定量结果如表I所示。其中距离阈值d设置为0.3m。我们的SLAMesh表现最好，其次是Voxblox+A-LOAM和Puma。图4显示了精度和召回结果。数值由浅到深递减。Voxblox用它的光线追踪方法擦除微小或薄的物体，如树木(Recall子图中的黑色区域)。PUMA会沿着轨迹积累误差。水密的假设使墙壁的顶部边缘略微弯曲。我们的SLAMesh可以高精度地恢复大型和小型结构。

B. Odometry Evaluation

我们还在广泛使用的KITTI[32]测程基准中定量评估了我们的姿态估计性能。KITTI数据集提供了来自Velodyne HDL-64E激光雷达的点云和地面真实姿态。序列00 ~ 10(包括城市、乡村和高速公路环境)的结果如图5所示。我们估计的轨迹与地面事实相比具有很高的一致性。

几种最先进的方法与不同类型的地图进行了测试。Suma[9]是一种基于surfel的方法，Suma++[10]通过动态去除目标来提高测程精度。LiTAMIN2[8]维护了一个体素化无损检测地图。A-LOAM是LOAM的一种实现[1]。Puma[3]也是一种基于网格的方法。表.II为标准相对平移误差和旋转误差的定量评价结果[32]。这些方法的结果直接从他们发表的论文中导入。我们的SLAMesh实现了卓越的性能，平均平移误差为0:676%，旋转误差为0:291度/100米，优于点云、无损检测、冲浪和基于网格的方法。注意，我们可以在没有任何循环闭包的情况下实现这一点。另外，据文献[9][10]报道，Suma需要GPU，冲浪地图相对比较杂乱。

图1还显示了SLAMesh在序列00、07和09中构建的在线网格图。当车辆行驶回起点时，地图显示出精确的对齐。这些序列的最大传播长度为5.07km，证明了SLAMesh的可扩展性。一个有趣的观察是，当我们允许当前扫描与重新访问的区域配准时，地图和整个轨迹显示出更好的一致性，但当评估KITTI中里程计的相对误差时，禁用这种隐式回环可以提高精度。我们认为这种现象在使用细胞组织图谱的方法中很常见[30]。

C. Memory and Computational Efficiency

KITTI序列07中每种方法的地图大小如图6所示。原始点云图扩展到几gb，这对于在线任务来说太重了。三种基于网格的方法显示了它们在内存效率方面的优势。Voxblox和SLAMesh比Puma消耗更少的内存资源。我们推测其原因是Puma不迭代融合点云。

图7显示了KITTI序列07上每帧的时间成本。由于是细胞组织的地图，SLAMesh的时间成本不会随着场景的规模而增加。请注意，激光雷达在该数据集中以10Hz的频率运行。然而，Puma平均每帧花费4.7s。对于前30帧，Puma使用迭代最近点(ICP)而不是网格。A-LOAM+Voxblox管道每帧花费129ms (7.8Hz)，这是近似实时的。我们的SLAMesh可以以40Hz的频率运行，超过其他方法。

图8进一步分析了各模块的时间成本。他们通常分为三组。第一部分为预处理，包括SLAMesh中的降采样和GP重构，A-LOAM中的特征提取，Puma中的正常计算。第二部分是配准，包括数据关联和优化。最后是地图维护，包括网格图的更新和发布。在Puma，泊松重建是主要的负担。在SLAMesh和A-LOAM中，注册都会迭代两次。SLAMesh的主要计算成本是预处理(占63%)，而A-LOAM的注册和地图更新成本为87%。这个观察结果显示了我们使用重构来提前构建数据的不同策略。Voxblox由于其快速集成策略而成为一种高效的网格划分工具。传感器的最大探测范围从100米减少到50米，这也有利于其效率。然而，管道需要维护两个映射，一个带KD-tree的点云映射和一个TSDF映射，这需要更多的计算和内存资源。

D. Ablation Study

我们进行了消融研究，以调查我们系统中每种技术的贡献。SLAMesh中的参数对于所有变体都是相同的。表II显示了结果。如果SLAMesh禁用点到网格的误差度量，平移和旋转误差就会增加。这表明，给定更多的几何信息，如曲面的法线，姿态估计可以更准确。约束组合也减少了误差。原因是在非结构化区域，单个点对网格误差度量可能不够鲁棒，并且约束组合可以抑制异常值的影响。

约束组合和多线程在提高效率方面也起着重要作用。在图8中，多线程将每次扫描的SLAMesh处理时间从57.1ms减少到23.8ms。其中，多线程使GP重建速度提高了3倍，网格发布速度提高了2:5。我们注意到Puma和Voxblox在实践中也利用了CPU的多核，这保证了公平的比较。约束组合由于平均约束数从9000个减少到540个而节省了约95%的优化时间成本。

我们对一组参数进行了广泛的测试，发现我们的方法对参数有很大的容忍度。当单元格大小增加到3m时，SLAMesh的帧率可以增加到80Hz左右，相对平移误差为0.88%。原因是我们在每个体素中使用了一个非参数化模型，而不是一个简单的元素。