港科大最新即将开源SLIM：构建轻量化可扩展的点云地图

文章：SLIM: Scalable and Lightweight LiDAR Mapping in Urban Environments

作者：Zehuan Yu, Zhijian Qiao, Wenyi Liu, Huan Yin, and Shaojie Shen

编辑：点云PCL

代码：https://github.com/HKUST-AerialRobotics/SLIM.

欢迎各位加入知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。

摘要

激光雷达点云地图因其高一致性被广泛应用于道路上的机器人导航。然而，稠密点云在长期维护中面临高内存消耗和维护性降低的问题。在本研究提出了SLIM，一种用于城市环境中长期激光雷达建图的可扩展轻量化建图系统。该系统通过将结构点云参数化为线和面，提供轻量化和结构化的表示，满足地图合并、位姿图优化和捆绑调整（Bundle Adjustment, BA）的需求，确保增量管理和局部一致性。针对长期使用，我们设计了一种基于地图的非线性因子恢复方法，用于稀疏化位姿，同时保持建图精度。使用KITTI、NCLT和HeLiPR等经典激光雷达建图数据集的多会话真实数据验证了SLIM系统的精度、轻量化特性和可扩展性。还通过基于地图的机器人定位验证了地图的可重用性。最终使用多会话激光雷达数据，SLIM系统提供了一种全局一致且内存消耗较低的地图（130 KB/km）。

图 1. HeLiPR 数据集中两个区域的 SLIM 演示。放大并显示颜色效果最佳。SLIM 提供带有线条（青色）和平面（洋红色）的参数化地图。显示下采样的点云地图以供比较（浅蓝色）。SLIM 地图自然比传统的 LiDAR 点云地图更轻量。地图消耗显著减少，从 (a) 中的 239.50 MB 减少到 1.20 MB，从 (b) 中的 82.38 MB 减少到 0.72 MB。

主要贡献

城市环境是我们日常生活的主要场景。城市环境中的建图历史悠久，可以追溯到DARPA城市挑战赛期间自主车辆的开发。随着激光雷达SLAM的进步，移动机器人现已能够处理结构良好的城市区域中的建图任务。然而将现有系统部署到城市环境中供移动机器人长期使用时，仍然存在以下几个挑战：

1. 稠密点云的高内存消耗：激光雷达传感器直接生成3D点云。传统建图系统通过点、体素和网格等稠密表示实现了精确的机器人位姿估计和激光雷达建图。这些方法尽管能够精确建模环境，但在大规模城市环境的资源受限平台上，内存效率不高。
2. 内存效率与定位精度的平衡：高层次或隐式表示（如对象或神经函数）提供了内存效率，但可能无法确保城市环境中精确定位所需的足够精度。由于机器人应用需要重复使用生成的地图，在激光雷达建图系统中集成低级几何信息至关重要。
3. 长期扩展性：许多现有激光雷达建图系统主要针对单会话测试设计，缺乏对长期部署的考虑。对于多会话激光雷达建图系统，扩展性在两个方面至关重要：一是全局合并多个会话地图，二是随着会话数量的增加，保持局部地图的一致性和尺寸。

总体而言，城市环境的特征是结构化且人工部署的，例如路灯和交通标志，这为长期建图提供了强有力的先验知识。类似的结构也可见于室内环境。Rosinol等人提出的Kimera在此类环境中建模了语义场景图。受到这些研究的启发，我们设计了一种可扩展轻量化激光雷达建图（SLIM）系统，利用城市环境的结构特性。图1展示了SLIM生成的地图与广泛使用的点云地图的比较。具体来说，SLIM的关键贡献涵盖了从前端表示到后端优化的多个方面，包括：

• 提出将线和面参数化为内存高效的表示形式，这种形式不仅能编码几何信息，还适合地图合并。
• 设计了基于位姿图优化（PGO）和捆绑调整（BA）的粗到精的激光雷达建图优化方法。
• 引入了一种基于地图的非线性图稀疏化方法，以管理随会话增加而扩大的地图尺寸，确保长期维护的扩展性。
• 在三个不同的多会话数据集上验证了SLIM系统的准确性、轻量化特性和扩展性。

内容概述

总体概述

系统流程如图2所示。对于输入的连续激光雷达扫描数据，SLIM首先根据几何特性将原始激光雷达点云转换为线和面的表示形式。这些参数化的表示形式紧凑且易于管理，适用于后续任务。系统利用激光雷达里程计或附加的车载里程计累积线和面信息，生成轻量化的地图特征。

图 2. 系统概述。前端地图矢量化模块提取特征并将平面参数化为车道。然后SLIM 系统通过地图合并生成一个全局轻量级地图。地图细化模块（包括 PGO 和 BA）仅使用线和平面来平滑建图结果。以地图为中心的边缘化可以随着地图会话的增加而限制计算和存储需求。总体而言，SLIM 是一个集中式服务器，可以实现地图矢量化、地图合并和细化，用于长期 LiDAR 建图。

为了合并多会话地图，SLIM利用地点识别并随后进行局部位姿估计。尽管地图被统一到同一个坐标系中，但仍然存在来自里程计的漂移，SLIM通过基于参数化线与面的粗到精优化方法（PGO和BA）实现地图的平滑优化。随着合并会话地图的增加，系统实施了一种基于地图的非线性因子恢复（NFR）方法，用于控制位姿数量，确保可扩展性。上述所有操作使得该建图流程能够随着多会话激光雷达地图的增加而保持扩展性。在算法1中总结了这一流程。

值得注意的是，在地图矢量化之后，地图特征仅由参数化的线和面构成，没有存储任何稠密点云。因此，后端的地图优化和维护都基于相同的表示形式。在实验部分，我们验证了这些轻量化表示形式能够有效支持搭载不同3D激光雷达传感器的多种移动机器人进行长期城市建图部署。

地图表示、合并和精细化

地图表示（Map Representation）

在许多基于传感器的数据处理或SLAM（同步定位与地图构建）技术中，地图不仅仅是一个简单的空间布局，而是通过一组特征、位置和估计值来构建的。这些特征可能包括点、线段、面或更复杂的几何形状。

地图表示的选择依赖于应用的具体需求。例如，对于室内导航系统，可能会使用基于网格的地图表示（如栅格地图），而对于大范围的户外导航系统，可能会采用基于特征的地图表示（如地标地图或点云地图）。地图可以分为局部地图和全局地图，局部地图通常只表示一个小范围的区域，而全局地图则表示整个环境。

在这一部分中还会讨论地图如何与传感器数据进行结合。例如，当使用激光雷达、摄像头或其他传感器时，如何从这些数据中提取出有意义的特征，并将它们组织成一个一致的地图结构。

图 3. 线和面地标的公式

地图合并（Map Merging）

地图合并是指将多个局部地图合并成一个全局地图的过程。这一过程通常发生在多次测量或多台设备数据的整合阶段，目的是获得一个完整且一致的环境表示。地图合并的挑战在于如何处理不同局部地图之间的重叠区域。由于局部地图可能是从不同角度或不同时间点采集的，因此它们可能存在偏差或误差。为了有效地合并这些地图，必须进行坐标对齐或配准（registration），这通常是通过一些算法来实现的，如ICP（Iterative Closest Point，迭代最近点）算法，或者通过特征匹配技术来优化合并过程。此外，地图合并还涉及到如何有效地去除冗余信息，并确保合并后的地图能够准确反映环境中的变化。合并过程中还需要考虑到地图的更新，特别是在动态环境中。

图 4. 两种块配准情况。我们通过聚类减少平面地标的数量，从而加快地图合并的配准过程。聚类地标之间的线表示不同坐标中的块之间的数据关联（对应关系）。

地图优化与细化（Map Optimization and Refinement）

地图优化与细化的目标是进一步提升地图的精度和一致性。在地图合并之后，可能会出现一些小的误差或不一致的地方，这时就需要进行优化。常见的优化方法包括基于图优化的技术，例如最小二乘法（Least Squares）优化，或者使用非线性优化算法（如Levenberg-Marquardt算法）来提高地图的精度。图优化方法将地图的每个元素（点、线、面等）视为一个节点，通过优化节点之间的约束关系来最小化误差，最终得到一个更加精确的地图。

此外，地图优化也可能涉及到一些后处理技术，例如通过去噪或平滑算法来减少地图中的细微误差，或者通过某些算法对地图的细节进行增强，以便在实际应用中更好地利用。总结来说，地图优化与精细化是一个不断迭代的过程，目的是确保地图在动态和复杂环境中的适应性和精确度。

图 5. 所提出的 LiDAR BA 的因子图。残差由里程计姿态、点到线和点到平面的测量值构成。利用先验姿态因子来固定全局姿态以进行优化。

基于地图的非线性因子恢复（NFR）方法

优化的维度会随着时间的推移而增加，这是长期地图构建中典型的问题。这种增长会导致优化的时间成本显著增加。本文介绍了一种基于地图的非线性因子恢复（NFR）方法，旨在随着会话的增加，保持一个稀疏化的姿态因子图和未改变的地标。具体来说，目标是将冗余的关键帧姿态边缘化，同时保持原始问题结构用于束调整（BA）。所提出的基于地图的NFR在保持全局一致性的同时，也限定了长期地图构建的计算需求。

A. 动机与问题表述

在经典的SLAM系统中，边缘化是一个关键技术，旨在修剪图并保留先前的信息，同时不破坏系统的可观测性。该技术的理论基础是Schur补充。然而，Schur补充通常会在Hessian矩阵中引入“填充”非零块，显著减缓非线性优化的速度。非线性因子恢复（NFR）通过减少恢复因子的数量，近似原始问题的分布。目标是最小化原始分布与恢复分布之间的Kullback-Leibler散度，从而确保优化的高效性。

NFR的目标是恢复一个新的分布，以稀疏拓扑的方式近似原始分布。这个过程被表述为一个Kullback-Leibler散度最小化问题。

图 6. 因子图拓扑及其对应的Hessian矩阵。左侧显示了原始因子图及其Hessian矩阵，其中绿色对角块表示地标的Hessian矩阵。浅蓝色框包含了所有需要保留的变量，而红色框包含了需要边缘化的变量。中间部分显示了边缘化后的因子图和密集的Hessian矩阵，其中地标部分不再是块对角形式。基于地图的非线性因子恢复（NFR）旨在找到右侧的Hessian矩阵。我们的解决方案紧密保持了块对角结构，同时使用左侧的稀疏矩阵块进行计算，近似表示由中间问题所代表的状态变量的概率分布。

两步边缘化

拓扑重建：第一步是选择关键帧进行边缘化，以获得新的因子图拓扑。应用基于密度的下采样方法，去除因子图中的一些关键帧姿态。通过特定的连接性准则确保Jacobian矩阵的可逆性，从而保持地标与关键帧之间的连接。

高效的NFR：第二步是计算边缘化后的残差信息矩阵。高效的NFR避免直接计算协方差矩阵，而是利用稀疏矩阵运算来提高计算速度并减少内存消耗。通过基于Hessian矩阵的块结构的稀疏矩阵操作来实现这一点。在基于地图的方法中，关键帧移除会导致信息丢失，因此需要进行边缘化以保持必要的地图数据。采用基于Schur补充的边缘化来计算边缘化后的Hessian矩阵，然后用于高效的因子恢复。高效的NFR通过利用稀疏性提高边缘化的效率，从而实现长期地图构建。该方法只计算协方差矩阵的必要块，从而减少计算时间和内存消耗。

图 7. 边缘化前Hessian矩阵的可视化。

实验

实验设置

本实验使用了三个现实世界的数据集进行评估：KITTI、NCLT和HeLiPR。KITTI数据集包含配备Velodyne HDL-64E的车辆收集的序列，NCLT数据集使用了多个会话进行评估，HeLiPR数据集则包括32个区域的LiDAR序列。每个数据集都采用了不同的LiDAR里程计方法，并通过将多个会话划分为区域进行测试，以验证SLIM系统在大规模地图构建中的泛化能力。

图 8. LiDAR传感器及实验部分使用的LiDAR扫描图。由于LiDAR传感器的不同规格，LiDAR点云存在差异。前端地图矢量化模块仍然能够将点云参数化为线和面，从而保证后续的地图优化和维护过程。

地图准确性

SLIM系统在准确性方面的表现首先在KITTI数据集上进行了评估，结果表明SLIM在地图构建中与其他先进方法相比，保持了较高的准确性，尤其在长时间序列的地图合并中表现良好。

图10. NCLT和HeLiPR地图的可视化

尽管KITTI数据集包含城市环境之外的场景，SLIM仍能提供一致的线性和平面地图。此外，在HeLiPR数据集上，SLIM在长期地图构建中也表现出色，通过增量地图构建和精确的地图优化提高了地图的准确性。图11和图12展示了实验结果，验证了SLIM的有效性。

图11. HeLiPR上的地图精度定量结果。评估指标是绝对轨迹误差的均方根误差（RMSE）。总体结果表明：（1）使用PGO和LiDAR BA之后，精度有所提高，分别用紫色和青色表示；（2）经过BA后的最终优化误差在增加的地图会话下保持有界。

图 12. HeLiPR区域点云地图的可视化。点云地图通过在机器人姿态上积累LiDAR点云构建。左侧的姿态是在应用LiDAR束调整之前获得的，即使用LiDAR里程计和姿态图优化。经过LiDAR束调整后，右侧的姿态漂移较小，从而在放大视图中生成高精度的地图。此图展示了所提出的LiDAR束调整在直线和面表示上的有效性。

内存效率

SLIM系统在内存效率方面表现优异，显著低于其他传统的地图构建系统。通过对KITTI、NCLT和HeLiPR数据集的评估，SLIM生成的地图需要的存储空间非常小，分别为0.5MB/km和不到15MB的存储需求，这使得其在多机器人系统中非常适用。图13和图14展示了NCLT和HeLiPR上的内存消耗，进一步证明了SLIM在内存管理上的优势。

图13. SLIM地图消耗的定量结果以及地图优化的时间成本。NCLT上的结果表明：（1）提供的地图轻量级，仅使用较低的存储消耗；（2）随着会话数量的增加，消耗和时间成本保持稳定。

图14. 在HeLiPR数据集上使用与不使用NFR的定量结果。如果对多会话数据应用NFR，时间成本和地图消耗保持在一定范围内。具体而言，BA和NFR的耗时不到10秒。轻量级地图对一个区域的消耗不到1MB。

可扩展性

SLIM的可扩展性得到了充分验证。无论是在NCLT还是HeLiPR数据集上，SLIM都表现出良好的扩展能力，随着会话的增加，系统的地图合并和优化时间保持在可控范围内。通过与禁用NFR的对比，实验结果表明，NFR模块在长期地图构建中提供了重要的支持，能够有效减少地图优化的时间成本和存储消耗。图15和图16展示了关键帧稀疏化和拓扑重建的有效性，证明了SLIM在多个会话合并中的准确性和可扩展性。

图15. 使用与不使用NFR对地图精度的定量结果，即绝对轨迹误差。结果表明，NFR模块在地图精度方面改善了长期地图构建。

图16. 在HeLiPR上应用NFR后，稀疏的姿态图。多会话数据导致同一区域内多个姿态图。NFR可以稀疏化关键帧，并生成一个更紧凑的拓扑地图，以便长期使用。

在线定位中的地图重用

SLIM生成的地图可用于在线定位，实验中使用基于LiDAR的定位算法评估其定位能力。通过最小化点对地标的残差，SLIM能够实现厘米级的定位精度，并验证了地图的有效性。表III展示了在HeLiPR数据集上的定量定位结果，证明了SLIM在重新定位中的有效性。

总结

尽管设计的SLIM系统在长期LiDAR地图构建中具有广泛的适用性，但在实际应用中仍然存在一些潜在的限制，主要有以下两点：

1. NFR的封闭解要求恢复的残差维度与状态变量的维度相匹配。这意味着雅可比矩阵必须是方阵。在一般的SLAM后端中，这一条件并不总是满足。例如，融入GPS约束、俯仰-滚转约束或其他约束可能导致残差与状态变量之间的维度不匹配。我们系统的关键因素是线和面维度精确匹配它们的残差维度，容易满足封闭解的条件。然而，如果在SLIM中融合更多的信息，这一条件可能不再成立。
2. 虽然NFR也有迭代解法，但对于极大规模的地图，计算成本会迅速增加。幸运的是，对于包含1.0 × 10⁴到1.0 × 10⁵个地标的地图，所提出的SLIM能够在几秒钟内实现单步解，这对于迭代方法是可以接受的。如果涉及更多的地图地标，SLIM系统则需要更多的时间来提供解。

尽管本研究主要关注城市环境，但SLIM系统也可以应用于其他结构化环境，如森林和室内空间。在森林中，地面和树木可以参数化为平面和线条。室内环境具有高度结构化的特征，如墙壁和柱子，这些都是典型的平面和线条，可用于机器人定位。后端地图合并和优化可以使用相同的前端表示来处理这些环境。