论文Review LIO Multi-session Voxel-SLAM | 港大MARS出品！体素+平面特征的激光SLAM！经典必读！-EW帮帮网

基本信息

题目：Voxel-SLAM: A Complete, Accurate, and Versatile LiDAR-Inertial SLAM System

来源：arXiv 2024

学校：HKU-MARS

是否开源：https://github.com/hku-mars/Voxel-SLAM

摘要：激光惯性SLAM

在这项工作中，我们提出了Voxel - SLAM：一个完整、准确和通用的LiDAR -惯性SLAM系统，该系统充分利用短期、中期、长期和多地图数据关联来实现实时估计和高精度制图。该系统包括5个模块：初始化、里程计、局部映射、循环闭合和全局映射，均采用相同的地图表示，即自适应体素地图。初始化为后续模块提供准确的初始状态估计和一致的局部地图，使系统能够以高度动态的初始状态启动。里程计利用短期数据关联，快速估计当前状态并检测潜在的系统发散。局部映射利用中期数据关联，采用局部LiDAR -惯性光束法平差( BA )在最近一次LiDAR扫描的滑动窗口内精化状态(和局部地图)。回环检测当前和之前所有会话中先前访问过的地方。全局映射使用高效的HBA对全局映射进行细化。环路闭合和全局映射都利用了长期和多地图数据关联。我们在3个代表性场景的30个序列上与其他先进方法进行了全面的基准比较，包括使用手持设备的狭窄室内环境、使用空中机器人的大规模荒野环境和车辆平台上的城市环境。其他实验证明了初始化的鲁棒性和效率，在多个会话中的工作能力，以及在退化环境中的重新定位。

Introduction

三维激光雷达( 3D Light Detection and Ranging，LiDAR )由于其直接、密集、主动和精确( DDAA )的深度测量，已成为自动驾驶车辆和无人机等自主移动机器人领域的热门传感技术。激光雷达同时定位与地图构建( SLAM )和激光雷达里程计( LO )利用激光雷达的测量数据提供必要的状态反馈和周围环境的三维稠密点云，以便在机器人上使用后续过程(例如,计划、控制)。此外，随着激光雷达技术的发展，固态激光雷达以其体积小、重量轻、价格低廉等优点得到了广泛的研究，并在一些LO 和LiDAR SLAM系统中得到了应用。

LiDAR SLAM与激光雷达里程计( odometry，LO )的区别在于，LiDAR SLAM的目标是在移动机器人上构建环境的一致地图并实时估计该地图中的当前位姿，而LO系统侧重于实时定位并累积地图，而不考虑对地图进行精化以缓解漂移。遵循ORB - SLAM3 的理念，LiDAR SLAM相对于LO的优势在于能够充分利用4种类型的数据关联来匹配和精化先前的测量。

短期数据关联：将当前扫描关联到一张地图，并尽可能高效地估计当前的自我运动，以供后续处理(例如,计划、控制)使用。大多数LO / LIO系统都具有短期数据关联，并将当前的LiDAR扫描累积到它们的地图中，而不考虑任何状态或地图的进一步细化，从而导致累积漂移。
中期数据关联：将最近的多次扫描关联到地图上，并对地图进行相应的精化。这一般通过光束法平差( BA )技术实现，在最近一段时间内同时精化状态和局部地图，缓解累积漂移，增强鲁棒性。
长期数据关联：关联历次LiDAR扫描数据，实现全局地图一致性。长期数据关联允许通过位姿图优化( PGO )来检测先前访问过的位置来重置漂移。全局BA也可用于进一步提高准确性。
多地图数据关联：关联多个地图会话。它在长期数据关联中使用类似的方法将不同的地图会话合并为一个，这可能是由于不同的采集时间或由于LiDAR退化而重新初始化新的会话引起的。

在这项工作中，我们充分利用这四种数据关联，提出了Voxel - SLAM，包括五个关键模块：初始化、里程计、局部建图、回环检测和全局建图。Voxel - SLAM建立在BALM的高效LiDAR光束法平差、HBA的全局建图方法、BTC [ 18 ]的位置识别工作以及VoxelMap [ 19 ]的地图表示方法之上。除了开源系统本身，Voxel - SLAM的主要贡献有：

在所有任务中使用相同的地图格式：初始化，里程计，局部映射，闭环，和全局映射。Voxel - SLAM采用了高效且通用的自适应体素地图，为不同任务提供了充足的特征，能够很好地适应各种场景。
健壮且快速的初始化。Voxel - SLAM只需要短时数据(在我们的实现中)来初始化(或者在系统发散的情况下重新初始化)，并且可以在静止和动态两种初始状态下进行初始化。初始化为后续模块提供准确的状态和一致的映射。
高效的局部映射。Voxel-SLAM使用一个高效的LiDAR -惯性BA来精化最近一次LiDAR扫描的滑动窗口内的局部体素地图和状态，增强了系统的准确性和鲁棒性。在计算资源有限的机器人机载计算机上，本地映射(窗口大小10)能够以与其里程计相同的速率( 10 Hz )实时运行。
多会话上的闭环。Voxel - SLAM具有在当前和以前的会话中检测回环的能力，并在全局范围内优化所有涉及的扫描姿态。
对单个或多个会话进行高效准确的全局映射。Voxel-SLAM使用层次BA来实现扫描姿态和地图一致性的有效全局优化。
充分挖掘四类数据的关联关系。Voxel SLAM利用其里程计和局部建图中的短期和中期数据关联，以及闭环和全局建图中的长期和多地图数据关联，以实现实时操作和全局地图一致性。

论文Review LSLAM BALM | 经典激光SLAM方案！港大MARS出品！RAL2021 | 激光BA优化-CSDN博客

论文Review LSLAM HBA | HKU-MARS出品！多层因子图全局优化必备模块！经典必读！RAL2023 -CSDN博客

[18] C. Yuan, J. Lin, Z. Liu, H. Wei, X. Hong, and F. Zhang, “Btc: A binary and triangle combined descriptor for 3d place recognition,” IEEE Transactions on Robotics, vol. 40, pp. 1580–1599, 2024.

[19] C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry,” IEEE Robotics and Automation Letters, vol. 7, no. 3, pp. 8518–8525, 2022.

Related Works

A. LiDAR(-Inertial) Odometry and SLAM

LOAM作为早期的LiDAR里程计和测图框架，对后续的工作产生了很大的影响。它包括三个模块：特征提取、扫描到扫描的里程计和扫描到地图的映射。通过每条扫描线的局部平滑性，提取特征点，包括平面特征点和边缘特征点，以减少位姿估计的计算负担。然后里程计模块利用特征点将当前扫描与前一次扫描进行匹配，实时获取粗略的相对位姿。为了提高精度，映射模块累积历史特征点构建kd树，并以点到平面(边缘)的距离为代价，从里程计中精化位姿。然而，由于需要重建kd树，与10Hz里程计相比，建图模块只能在1Hz下运行。legoLOAM在LOAM框架的基础上，在特征提取中采用了地面分割方法，并引入了闭环来缓解长期漂移。LOAM-Livox 将该框架改装成固态激光雷达。固态激光雷达的小视场( FoV )和非重复扫描模式使得连续两次扫描之间的对应关系非常少。因此，LOAM - Livox只使用扫描地图配准进行里程计和建图，提高了里程计的精度但是增加了每次扫描时构建kd树的计算负载。（激光SLAM耗时长的原因在于kd树）

融合惯性测量单元( Inertial Measurement Unit，IMU )的测量数据可以为LiDAR SLAM提供更好的初始位姿，补偿运动畸变，增强估计的鲁棒性。LIOM 受VINS 的启发，是首个开源的紧耦合LiDAR -惯性里程计( LIO )之一。然而，由于缺乏高效的BA方法，LIOM中的批量优化过于耗时，无法实时运行。LiLi-OM，类似于LIOM，使用局部地图和滑动窗口优化紧耦合的LIO，适应于固态激光雷达，并使用基于ICP的闭环。为保证实时性，LiLiOM采用较小的优化窗口。LIO-SAM 摒弃了LIOM中效率较差的局部地图，直接将LiDAR里程计结果输入因子图，利用IMU预积分进行优化。因此，LIO - SAM可以保持较大的滑动窗口，并且容易与其他传感器进行融合，如GNSS (全球导航卫星系统)。LINS将基于滤波器的方法引入到LIO系统中，以避免优化多个状态。为了降低计算卡尔曼增益的维度，FAST-LIO 采用了一种新的卡尔曼增益。FAST-LIO2 作为最先进的LIO系统，避免了特征提取，并设计了一种新颖的增量式kd树。LTA-OM在FAST - LIO2的基础上增加了回环闭合，增加了STD位置识别描述符[ 28 ]。Faster-LIO用并行稀疏增量体素替换FAST - LIO2中的增量kd树，以获得更好的效率。Point-LIO是一种逐点的LIO框架，具有较高的里程计输出率，对攻击性运动和IMU饱和具有良好的鲁棒性。

[28] C. Yuan, J. Lin, Z. Zou, X. Hong, and F. Zhang, “Std: Stable triangle descriptor for 3d place recognition,” in 2023 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2023, pp. 1897–1903.

与主流的开源LiDAR (惯性) SLAM系统相比，Voxel - SLAM具有初始化使系统在高度动态的初始状态下启动、使用LiDAR -惯性BA进行局部建图以增强精度和鲁棒性、回环闭合以检测多个会话中的回环约束、全局建图以进一步精化全局地图等模块。Voxel - SLAM充分利用短期、中期、长期和多地图数据关联，实现实时估计和高精度建图。

B. LiDAR Scan Registration Scheme

LiDAR点云的配准方法可以分为两类：pairwise配准和multiview配准。成对配准只考虑估计单个位姿，是LO和LIO中主要的配准方法。由于LiDAR扫描的稀疏性，LOAM及其变体采用点到平面(边缘)的配准代替点到点的配准，用于稠密点云。它们将历史扫描累积成点图，以匹配最新的扫描。点地图的缺点是需要额外的时间来形成新的扫描配准的平面或边缘。为了避免这种情况，Suma++、LIPS、和VoxelMap使用planes map而不是points map来为当前扫描中的每个点找到对应的平面，实现了更好的效率和精度。（对于激光来说，面特征非常重要，比点特征还重要）

使用pairwise配准会导致累积漂移，而多视图配准可以通过同时配准多个扫描来解决这一问题。EigenFactor( EF ) [ 33 ]将每个点到平面的距离作为代价，然后将代价转化为协方差矩阵的最小特征值。然后通过基于梯度的方法优化成本函数，由于特征值型cost的二阶性质，导致收敛缓慢。参考视觉BA，平面调整( Plane Adjustment，PA ) 同时考虑位姿和平面参数作为优化变量，利用Schur补来降低平面参数带来的维度。其缺点是当平面数量增加且平面参数具有奇异性时，需要更多的迭代次数才能收敛。为了更好地收敛，BAREG [ 35 ]修改了代价函数，增加了一个额外的启发式惩罚项，这可能会干扰地图一致性项，特别是在真实的噪声点测量中。BALM [ 36 ]引入了二阶导数求解器，并证明了几何特征可以解析求解，这比前面提到的方法需要更少的步数才能收敛。BALM的主要缺点是需要枚举所有的个体点来计算雅克比矩阵和海森矩阵，由于LiDAR点较密集，导致计算复杂度较高。为了解决这个问题，BALM2 [ 16 ]提出了点簇，将原始点编码成一个紧凑的参数集，并推导出相应的Hessian和Jacobian矩阵。BALM2收敛速度快，精度高。在BALM2的基础上，Liu等人提出了一个用于大规模全局映射的层次光束平差( Hierarchical bundle adjustment，HBA )框架。

[33] G. Ferrer, “Eigen-factors: Plane estimation for multi-frame and timecontinuous point cloud alignment.” in IROS, 2019, pp. 1278–1284.

[35] H. Huang, Y. Sun, J. Wu, J. Jiao, X. Hu, L. Zheng, L. Wang, and M. Liu, “On bundle adjustment for multiview point cloud registration,” IEEE Robotics and Automation Letters, vol. 6, no. 4, pp. 8269–8276, 2021.

[16] Z. Liu, X. Liu, and F. Zhang, “Efficient and consistent bundle adjustment on lidar point clouds,” IEEE Transactions on Robotics, vol. 39, no. 6, pp. 4366–4386, 2023.

Voxel - SLAM同时使用了成对配准和多视图配准。在里程计中，Voxel - SLAM利用一个高效的平面地图( Voxel Map )来实现高效的扫描地图配准。在初始化和局部建图中，Voxel - SLAM采用BALM2进行LiDAR (惯性) BA优化，兼顾了效率和精度。此外，为了提高全局建图精度，Voxel - SLAM在普通PGO之后加入了高效的HBA框架。

C. LiDAR-Based Place Recognition

受正态分布变换( NDT )的启发，有人将点云细分为规则的小网格，并将所有小网格编码为直方图矩阵，通过匹配这些直方图矩阵来检测回环。M2DP [ 38 ]将三维点云投影到多个二维平面，并为每个平面生成一个密度签名，利用这些签名的左右奇异向量作为描述符进行匹配。SegMatch 将点云分割成语义特征来检测回环检索。Giseop Kim等人受shape context的启发，提出了一种新的空间描述子Scan Context [ 41 ]，将三维点云编码为2.5D信息矩阵。他们的后续工作Scan Context++解决了横向不变性的问题并且通过提出一种新的通用描述符和使用子描述符来加速循环检索，并且解决Scan Context搜索的效率低下问题。与视觉SLAM中的BoW类似，BoW3D [ 44 ]在LiDAR点云中为Link3D创建的三维特征构建词袋。近年来，深度神经网络( deep neural network，DNN )被引入到LiDAR SLAM ( LCDNet 、LoGG3D-Net 、LocNet )中，用于提取局部特征和编码全局描述符。这些基于学习的方法的主要缺点是对训练数据敏感，泛化能力有限。当激光雷达类型或环境发生变化时，需要重新对其进行训练。最近，Yuan等人提出了一种二进制三角形组合( Binary Triangle Combined，BTC )描述符[ 18、28 ]，该描述符引入了一种鲁棒的二进制特征提取和具有良好视点不变性的三角形描述符匹配方法。此外，BTC可以很容易地适应任何类型的LiDAR，并提供了快速的回环检索策略和精确的6DoF相对位姿估计。

[38] L. He, X. Wang, and H. Zhang, “M2dp: A novel 3d point cloud descriptor and its application in loop closure detection,” in 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2016, pp. 231–237.

[41] G. Kim and A. Kim, “Scan context: Egocentric spatial descriptor for place recognition within 3d point cloud map,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 4802–4809.

[44] Y. Cui, X. Chen, Y. Zhang, J. Dong, Q. Wu, and F. Zhu, “Bow3d: Bag of words for real-time loop closing in 3d lidar slam,” IEEE Robotics and Automation Letters, vol. 8, no. 5, pp. 2828–2835, 2022.

[18] C. Yuan, J. Lin, Z. Liu, H. Wei, X. Hong, and F. Zhang, “Btc: A binary and triangle combined descriptor for 3d place recognition,” IEEE Transactions on Robotics, vol. 40, pp. 1580–1599, 2024.

[28] C. Yuan, J. Lin, Z. Zou, X. Hong, and F. Zhang, “Std: Stable triangle descriptor for 3d place recognition,” in 2023 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2023, pp. 1897–1903.

考虑到准确性和效率，Voxel - SLAM结合了BTC [ 18 ]进行回环检测。与先前所述的基于体素图的扫描地图配准和(混合) LiDAR光束平差类似，BTC也使用体素图来检测平面，以提取其二进制三角形组合描述符。这使得自适应体素映射成为VoxelSLAM所有模块的统一映射结构，包括初始化、里程计、局部建图和全局建图。

III. SYSTEM OVERVIEW

A. Workflow

Voxel-SLAM 是一个高效的 SLAM 系统，结合了 LiDAR 和 IMU 数据，通过多线程并行处理实现实时定位与建图。系统的核心思想是通过模块化设计和多层次数据结构（数据金字塔）来处理 LiDAR 点云和 IMU 数据，以实现高精度、鲁棒的定位和地图构建。系统的状态定义、模块划分、数据结构和优化方法是其核心内容。

系统状态定义

Voxel-SLAM 包含五个模块，运行在三个并行线程中（见图 1，绿色部分为模块，灰色虚线框表示同一线程，蓝色部分为数据金字塔，红色部分为自适应体视素地图）：

初始化模块：在系统启动或重启时，使用 1 秒的 LiDAR 和 IMU 数据进行初始化，通过专门的 LiDAR-惯性束调整（BA）优化来估计所有扫描的状态、初始局部地图和世界坐标系中的重力向量。
里程计模块：通过紧密融合 LiDAR 和 IMU 数据，实时估计当前状态，并检测由于 LiDAR 退化（degeneration）导致的系统发散。
局部建图模块：将当前扫描加入滑动窗口，通过 LiDAR-惯性 BA 优化滑动窗口内的状态和局部地图。最老的扫描会被边缘化（marginalized）为关键帧。
闭环检测模块：从关键帧中提取闭环描述子，检测当前会话或历史会话中的闭环。检测到闭环后，构建姿态图（pose graph）进行优化。
全局建图模块：执行关键帧 BA，将关键帧合并为子地图（submap）。在会话结束时，对所有子地图进行全局 BA 和自顶向下的优化，得到所有扫描的精确姿态。

B. Data Pyramid

数据金字塔是一个三层数据结构，用于处理输入的 LiDAR 点云序列：

底层：原始 LiDAR 扫描（10 Hz），直接用于里程计和局部建图，实时估计状态。
中层：关键帧（keyframe），由 10 个 LiDAR 扫描通过 LiDAR BA 优化合并而成，用于闭环检测提取描述子。
顶层：子地图（submap），由 10 个关键帧通过 LiDAR BA 优化合并而成，用于全局建图。关键帧合并采用滑动窗口（大小 10，步幅 5），确保连续子地图间有重叠区域（5 个关键帧），而扫描到关键帧的合并无重叠（步幅 10），因为 LiDAR 测量距离长，扫描间已有足够重叠。

C. Adaptive Voxel Map

自适应体视素地图（Adaptive Voxel Map）是系统的核心数据结构，用于提取平面特征并提供跨扫描、关键帧或子地图的特征关联。它的构建过程如下：

空间划分：将空间划分为均匀的体视素（根体视素，尺寸 Lr ），通过哈希表组织，每个 LiDAR 点根据位置映射到对应体视素。
体视素内部结构：每个体视素包含一个八叉树（octree），叶节点表示不同尺寸的平面特征。
平面检测：对于一个扫描、关键帧或子地图的点云，若某体视素内的点位于同一平面上（通过点协方差矩阵的最小特征值与次小特征值之比判断），则该体视素被标记为平面体视素并保存；否则，递归分割为更小的子体视素，直到满足平面条件或达到终止条件（如最小体视素尺寸或点数限制）。
用途：
- 局部体视素地图：覆盖 LiDAR 周围距离 Lm，用于初始化、里程计和局部建图，实时估计状态。
- 闭环检测体视素地图（BTC）：用于提取关键帧的闭环描述子。
- 全局建图体视素地图（HBA）：用于跨关键帧和子地图的平面特征提取与关联。

IV. LIDAR-INERTIAL BA OPTIMIZATION

LiDAR-惯性 BA 优化用于初始化和局部建图模块，旨在同时估计多个扫描的状态。与基于滤波的 LiDAR-惯性里程计（LIO）方法相比，BA 优化利用更长时间的数据关联（mid-term），因此更精确和鲁棒，但计算复杂度较高。为解决计算效率问题，系统采用了 BALM2 方法，并结合 IMU 预积分，优化公式如下

IMU 预积分残差

需要注意的是，在 $\mathbb{R}^3$ 中，重力矢量被当作一个矢量处理，其尺度是经过优化的，而不是像视觉SLAM中那样固定尺度的 $\mathbb{S}^2$ 。这是因为LiDAR的测量具有精确的尺度信息，使得从数据中确定尺度成为可能。相比之下，视觉测量缺乏尺度信息，使得在大多数视觉SLAM系统中需要固定尺度的重力。采用二阶求解器LM求解器对式( 2 )中的代价函数进行迭代优化。为了加快优化过程，我们推导了成本函数的Jacobian和Hessian的解析形式。

V. INITIALIZATION

初始化模块是 Voxel-SLAM 的起点，旨在为系统提供鲁棒的初始状态、重力向量和一致的初始局部地图，尤其是在非静态初始条件下（如序列中途重启）。传统滤波器基的 LIO 系统（如 [8, 13]–[15]）通常假设传感器初始静止，这种假设在非静态场景下会导致状态估计错误，进而影响 LiDAR 扫描的畸变校正和特征关联，最终导致不一致的初始地图和后续里程计的退化。为了解决这个限制，我们在之前的LiDAR -惯性BA优化的基础上，提出了一个鲁棒和高效的初始化阶段，即使在非静态的初始状态下，也能提供对初始状态、重力矢量和一致的初始局部地图的鲁棒估计。此外，该模块还可以确认此初始化是否成功，并将重力矢量重新对准世界坐标系的z轴。

初始化使用前 N 个 LiDAR 扫描及其对应的 IMU 测量数据，具体步骤如下：

初步状态估计：采用 [fastlio] 中的 LIO 方法实时粗略估计前 N 个扫描的状态和重力向量，作为后续优化的初始值。
粗到精体视素化和 BA 优化：基于初始状态，执行多轮粗到精的体视素化和 LiDAR-惯性 BA 优化，逐步精炼状态和地图（见 V-A 节）。
初始化成功判定：根据三个标准判断初始化是否成功（见 V-B 节）。若成功，完成初始化（见 V-C 节）；否则，收集下 N 个扫描，重复上述过程。

A. Coarse-to-Fine Voxelization and BA Optimization

由于初始状态可能不准确，直接进行体视素化可能导致错误的平面特征关联（如假阳性或假阴性），进而影响 BA 优化。为此，系统采用粗到精（coarse-to-fine）的策略：

初始阶段：使用较宽松的平面特征判定标准（见 III-C 节，即点协方差矩阵最小特征值与次小特征值之比的阈值较大），即使初始状态误差较大，也能召回平面特征。
BA 优化：基于召回的平面特征，执行 LiDAR-惯性 BA 优化（见 IV 节），精炼状态。
迭代精炼：使用精炼状态校正每个扫描的运动畸变，构建下一轮自适应体视素地图，并逐步收紧平面特征判定标准。重复体视素化和 BA 优化，直到代价函数的下降幅度小于某阈值，表示收敛。

B. Criteria for Initialization Success

初始化成功的三个标准：

收敛性：粗到精过程在规定轮次内收敛（即代价函数下降幅度足够小）。
重力向量模值：优化后的重力向量模值接近 $9.8 \, \text{m/s}^2$ 。
平面约束方向：初始体视素地图包含三个线性无关方向的平面约束，避免退化（degeneration）。退化评估见附录 B，若最终 BA 优化中出现退化，说明状态缺乏足够的平面特征约束，可能存在较大估计误差。

退化评估：通常通过分析平面法向量的分布确定是否存在足够约束。若平面法向量集中在某一方向（如仅在水平面），则系统在其他方向上可能缺乏约束，导致退化。

C. Completion of Initialization

成功初始化后：

获得精确的重力向量和前 N 个扫描的去畸变点云。
通过旋转矩阵 $R^z_{g}$ 将重力向量对齐到世界坐标系的 z 轴，状态也相应旋转。
重力向量在后续模块中固定。
基于优化状态构建的自适应体视素地图作为初始局部地图，供里程计模块使用。
初始化仅在每次启动或重启时执行一次，后续数据直接进入里程计模块。

VI. ODOMETRY

里程计模块旨在以最小时间延迟估计当前状态，为规划和控制等任务提供支持，同时为局部建图模块提供较准确的初始值。里程计利用短时数据关联，通过扫描到地图的配准（scan-to-map registration）实现高效状态估计。

A. State Prediction and Update

里程计采用 [19] 的scan-to-map配准方法和 [Fast-lio] 的扩展卡尔曼滤波（EKF）紧密耦合 LiDAR 和 IMU 数据，具体流程：

同步数据：同步 LiDAR 扫描和 IMU 测量。
状态预测：通过 IMU 传播（propagation）预测状态，同时校正 LiDAR 扫描的运动畸变。
点云处理：
- 去畸变后的扫描点云按空间分辨率 Ld 降采样。
- 使用预知的 LiDAR-IMU 外参将点云从 LiDAR 坐标系转换到 IMU 坐标系。
- 每个点根据计算协方差矩阵，投影到世界坐标系，与体视素地图中的平面特征匹配（选择概率最大的平面）。
状态更新：点到平面的距离作为系统测量值，通过误差状态迭代卡尔曼滤波（error-state iterative Kalman filter）更新 IMU 预测的状态。
数据发布：将估计的状态和 LiDAR 扫描发布给其他模块。

[19] C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry,” IEEE Robotics and Automation Letters, vol. 7, no. 3, pp. 8518–8525, 2022.

B. Detection of System Divergence

在缺乏足够几何特征的退化环境中（如长走廊），状态估计可能变得不适定（ill-posed），导致系统发散。Voxel-SLAM 在每次扫描配准后检测潜在发散：

退化评估：收集扫描到地图配准中使用的平面，采用附录 B 的方法评估退化（如平面法向量分布是否集中在单一方向）。
发散判定：
- 偶尔的单次退化不视为发散，因为 IMU 在短时间内预测较准确。
- 连续多次扫描的退化视为发散。
处理措施：
- 保存当前数据会话。
- 系统从远离当前会话的新起点重启，避免新会话点云与当前会话重叠。
- 重启触发新的初始化，获取新会话的初始状态和局部地图。

VII. LOCAL MAPPING

局部建图模块利用中时数据关联（mid-term data association），在滑动窗口内精炼局部体视素地图和近期状态。相比里程计，局部建图通过更长时间的 LiDAR 和 IMU 测量进行 LiDAR-惯性 BA 优化，因此更精确和鲁棒。

A. Keyframe Loading and Voxelization

局部建图接收里程计模块的 IMU 测量、当前 LiDAR 扫描及其估计状态，具体处理：

点云分布：将当前扫描的点按世界坐标系位置分配到对应体视素。
IMU 预积分：对 IMU 测量进行预积分，得到 IMU 预积分因子。
动态加载关键帧：
- 为支持长期数据关联，动态加载时间上较远（≥ 1 分钟，避免近期关键帧）但空间上较近（≤ 10 米）的关键帧（参考 [10]）。
- 关键帧的点分配到对应体视素，作为固定的历史地图约束。
自适应体视素化：
- 跟踪新增点的体视素，枚举这些体视素，将所有点（来自当前扫描、关键帧或已有点）进行自适应体视素化（见 III-C 节），直到每个叶节点包含有效平面。
- 叶节点中的点编码为紧凑的 点簇（point cluster） 结构（参考 [16]），滑动窗口内不同扫描有不同点簇。
- 叶节点中来自动态加载关键帧或先前边缘化扫描的点称为 “固定”点，编码为单一点簇，作为地图约束。

[10] Z. Zou, C. Yuan, W. Xu, H. Li, S. Zhou, K. Xue, and F. Zhang, “Ltaom: Long-term association lidar–imu odometry and mapping,” Journal of Field Robotics, 2024.

B. Sliding Window BA Optimization

滑动窗口 BA 优化采用 IV 节的 LiDAR-惯性 BA 方法，精炼滑动窗口内的所有状态。与初始化 BA 优化的区别：

地图约束：包含来自体视素中“固定”点的约束，确保状态估计在世界坐标系中的准确性。
固定重力向量：初始化已估计重力向量，因此在滑动窗口优化中固定。

C. Map Marginalization

BA 优化后：

地图更新：根据精炼状态更新局部体视素地图中的点簇和平面参数（平面中心和法向量），供下一轮里程计的扫描到地图配准使用。
边缘化：
- 移除滑动窗口中最老的扫描，其点合并到每个叶节点的“固定”点簇，作为地图约束。
- 最老扫描及其姿态发送给闭环检测模块进一步处理。
内存管理：
- 移除与滑动窗口状态无关或距离当前位置超过局部地图尺寸 Lm 的体视素，减少内存占用。

VIII. LOOP CLOSURE

闭环检测是 Voxel-SLAM 的核心模块，用于消除长期累积的漂移（drift）并实现多会话地图的对齐。它通过长期数据关联检测已访问位置，优化姿态图以纠正累积误差，并将当前会话与历史会话在统一世界坐标系中匹配，从而构建一致的环境地图。

A. Keyframe Generation and Loop Detection

闭环检测从局部建图模块接收边缘化的扫描，具体流程如下：

关键帧生成：
- 边缘化的扫描被推入姿态图（pose graph）。
- 连续 N N N 个边缘化扫描累积为一个帧（frame）。
- 若当前帧与最新关键帧的移动距离和旋转角度超过阈值（如 0.5 米和 5 度），则该帧被选为关键帧。
- 关键帧用于提取 BTC 描述子（参考 [18]），并匹配当前会话或历史会话中的候选闭环关键帧，同时发送到全局建图线程处理。
闭环检测与验证：
- 使用 BTC 描述子（参考 [18]）进行候选闭环检测。
- 为避免误检测（false-positive），采用三个验证标准：
  1. BTC 内置几何验证：确保候选闭环在几何上合理。
  2. 平面约束方向：当前关键帧与候选关键帧的点到平面配准需包含三个线性无关方向的平面约束（参考附录 B 评估退化）。
  3. 漂移距离比例：漂移距离（当前关键帧与候选关键帧的位姿差）与移动距离之比需足够小（如 ≤ 1%），适用于：
    - 当前会话内的闭环（见图 4(a)）。
    - 历史会话中第二次及以上的闭环（见图 4(b)）。
- 特殊情况：若候选关键帧来自历史会话且为首次匹配，第三标准不适用，第一和第二标准需更严格，因为首次跨会话闭环错误会对后续操作产生严重影响。

B. Pose Graph Optimization and Map Rebuilding

成功检测到闭环后，系统执行姿态图优化（PGO）和地图重建：

会话连接：
- 若闭环关键帧来自历史会话且为首次匹配：
  - 将当前会话所有位姿对齐到历史会话的世界坐标系。
  - 连接两会话的姿态图。
- 若闭环关键帧来自当前会话或历史会话的第二次及以上匹配：
  - 姿态图内形成闭环，触发 PGO 优化（使用 GTSAM ）。
位姿更新：
- PGO 更新姿态图中扫描的位姿。
- 根据更新的扫描位姿，同步更新关键帧和子地图的位姿（见图 1 数据金字塔，图 5 红色节点）。
- 更新姿态图外的扫描位姿，包括：
  - 闭环执行期间边缘化的扫描（图 5 绿色节点）。
  - 当前滑动窗口内的所有扫描（因滑动窗口已向前滑动，图 5 蓝色节点）。
- 这些扫描的位姿通过最后扫描的位姿校正 $\Delta T_p$ （见图 5）更新： $T_{\text{new}} = T_{\text{old}} \cdot \Delta T_p$ 其中T 表示位姿（包括旋转R和平移p）。
地图重建：

使用更新位姿的扫描（包括边缘化扫描、滑动窗口扫描和姿态图中最新五个关键帧的扫描）构建新的自适应体视素地图。
新体视素地图的叶节点中：
- 来自关键帧和边缘化扫描的点为“固定”点，编码为单一的点簇，作为地图约束。
- 来自滑动窗口扫描的点按扫描分别编码为点簇（见 VII-A 节）。
新体视素地图替换原有地图，供后续里程计和局部建图使用。
为节省计算资源，仅当漂移距离（VIII-A 中的漂移距离）超过阈值（如 0.1 米）时执行 PGO 和地图重建。

IX. GLOBAL MAPPING

全局建图模块进一步精炼闭环检测生成的点云地图，解决 PGO 仅考虑位姿约束而忽略直接地图一致性约束导致的不一致问题。全局建图基于数据金字塔（见图 1），采用层次化全局 BA 方法（参考HBA）降低优化维度，包含三个步骤。

实验

硬件平台

数据集

Hilti [49, 50]：室内外建筑环境，手持设备，Hesai XT-32 LiDAR（10 Hz），Bosch BMI085 IMU（400 Hz）。提供绝对轨迹误差（ATE）评估。
MARS-LVIG [51]：无人机向下俯视（100 米高度），Livox Avia LiDAR（10 Hz，内置 BMI088 IMU，200 Hz），覆盖自然场景（树林、丘陵、河流）。
UrbanNav [52]：城市机器人车，Xsens-MTI30 IMU（400 Hz），HDL-32E LiDAR（10 Hz），包含动态对象和长时采集（最长 56 分钟）。
私有数据集：轻量级手持设备，Livox Avia LiDAR（10 Hz，内置 IMU），2 个序列，覆盖森林和校园环境。

[49] L. Zhang, M. Helmberger, L. F. T. Fu, D. Wisth, M. Camurri, D. Scaramuzza, and M. Fallon, “Hilti-oxford dataset: A millimeter-accurate benchmark for simultaneous localization and mapping,” IEEE Robotics and Automation Letters, vol. 8, no. 1, pp. 408–415, 2022.

[50] A. D. Nair, J. Kindle, P. Levchev, and D. Scaramuzza, “Hilti slam challenge 2023: Benchmarking single+ multi-session slam across sensor constellations in construction,” arXiv preprint arXiv:2404.09765, 2024.

[51] H. Li, Y. Zou, N. Chen, J. Lin, X. Liu, W. Xu, C. Zheng, R. Li, D. He, F. Kong et al., “Mars-lvig dataset: A multi-sensor aerial robots slam dataset for lidar-visual-inertial-gnss fusion,” The International Journal of Robotics Research, 2024.

[52] L.-T. Hsu, F. Huang, H.-F. Ng, G. Zhang, Y. Zhong, X. Bai, and W. Wen, “Hong kong urbannav: An open-source multisensory dataset for benchmarking urban navigation algorithms,” NAVIGATION: Journal of the Institute of Navigation, vol. 70, no. 4, 2023.

Benchmark