GS-LIVO：基于高斯泼溅的实时LiDAR、惯性和视觉多传感器融合里程计

文章：GS-LIVO: Real-Time LiDAR, Inertial, and Visual Multi-sensor Fused Odometry with Gaussian Mapping

作者:Sheng Hong , Chunran Zheng,Yishu Shen , Changze Li , Fu Zhang , Tong Qin , Shaojie Shen

编辑：点云PCL

摘要

近年来，三维高斯泼溅重建（3D-GS）已成为一种新兴的场景表示方法。然而，现有的仅基于视觉的3D-GS方法通常依赖于手工设计的启发式规则进行点云密化，并在处理遮挡、高GPU内存和计算消耗方面面临挑战。LiDAR-惯性-视觉（LIV）传感器配置凭借其互补的感知特性，在精准定位和高密度建图方面表现优异：相机提供丰富的纹理信息，LiDAR提供精确的几何测量，IMU则提供高频运动数据。受此启发，本文提出了一种基于高斯的实时同时定位与建图（SLAM）系统。该地图系统包括一个全局高斯地图和一个滑动窗口高斯集合，并采用基于IESKF（迭代误差状态卡尔曼滤波器）的实时里程计进行高斯地图优化。全局高斯地图采用哈希索引体素，并按照递归八叉树结构组织。这种分层结构能够有效覆盖稀疏的空间区域，同时适应不同尺度和细节级别的环境。高斯地图通过多传感器融合高效初始化，并利用光度梯度优化。系统增量式维护滑动窗口中的高斯点，极大减少了GPU计算和内存消耗，仅优化滑动窗口内的地图，从而实现实时优化。此外实现了一种紧耦合的多传感器融合里程计，结合IESKF迭代更新，通过实时更新和渲染高斯地图，实现高精度定位。该系统是首个可部署在资源受限的嵌入式系统上的实时高斯SLAM框架（全部采用C++/CUDA高效实现），并在NVIDIA Jetson Orin NX1平台上成功运行。该框架在保持强大多传感器融合能力的同时，实现了实时性能。所有实现的算法、硬件设计、CAD模型及GPU加速系统的演示视频将在以下地址公开：https://github.com/HKUST-Aerial-Robotics/GS-LIVO。

主要贡献

本研究旨在开发一种 激光雷达-惯性-视觉里程计（LIVO）系统，该系统利用 高斯泼溅建图（Gaussian Map） 作为新型地图表示方式，将激光雷达和摄像头测量数据进行紧密融合。本研究的目标包括：

• 通过高斯地图的多传感器紧耦合融合，实现高精度定位。
• 大幅提升高斯地图更新的效率，即使在大规模和复杂环境下仍能高效运行。
• 本方法针对当前 高斯 SLAM 系统在实际部署中的关键瓶颈 进行优化。

图 1：GS-LIVO 在大规模场景中的组成部分：基于航空数据集的实时里程计和高斯泼溅建图

本研究的主要贡献：

• 提出了一种基于空间哈希索引八叉树（spatial hash-indexed octree）的全局高斯地图结构。
• 该层次结构支持 高效的全局索引、共视性检查（co-visibility check），并能自适应不同的 细节层次（LoD），以适应大规模场景。
• 提出了一种快速初始化策略，将激光雷达和视觉数据融合，以快速生成结构良好且具备高保真渲染能力的全局高斯地图。
• 提出了一种增量式的滑动窗口高斯调度方法，该方法能够最小化地图维护成本和计算负担，优化高斯更新过程，并减少 GPU 内存占用。
• 提出了一种新型的视觉测量模型，利用高斯地图的逼真渲染能力，结合 IESKF（迭代误差状态卡尔曼滤波）进行激光雷达-惯性测量的紧密融合。
• 在大规模基准测试和真实世界实验中，我们的方法表现出显著减少内存使用和加速高斯地图优化的能力，同时在 室内和室外数据集上保持高质量渲染并实现与主流方法竞争的里程计精度。

主要内容

内容概述

GS-LIVO 的系统概述如图 2 所示。其硬件配置集成了同步的 LiDAR、IMU 和相机，并通过模拟每秒脉冲（PPS）信号实现精确的时间对齐。软件框架包括四个关键模块：

（1）基于空间哈希索引八叉树组织的全局高斯地图，该结构能够有效覆盖稀疏空间区域，并适应不同环境细节和尺度；

（2）基于 LiDAR 和视觉信息的高斯点快速初始化与光度梯度优化；

（3）局部高斯点滑动窗口的增量维护，实现低显存消耗的实时优化；

（4）使用 IESKF 进行位姿优化，并实现顺序更新。本系统是一种实时 SLAM 框架，能够无缝融合 LiDAR、惯性和视觉传感器，实现高精度定位。

图 2：GS-LIVO 系统概述——种基于高斯泼溅建图的实时 LiDAR-惯性-视觉里程计系统。该流程利用多传感器数据对高斯点进行联合初始化和优化，并通过哈希索引八叉树结构和滑动窗口机制进行管理。

全局高斯地图：哈希索引八叉树

GS-LIVO 系统采用了一种高效的全局高斯地图结构，以优化大规模环境下的三维映射。该系统主要由全局高斯地图和滑动窗口高斯组成。其中，全局高斯地图利用哈希索引八叉树进行存储管理，通过空间哈希索引（Hash Key）来高效覆盖场景中的稀疏区域，并根据环境复杂度递归细分，使地图能够适应不同的细节层次。

高效的空间索引与存储

• 全局地图索引机制：系统利用当前帧的 LiDAR 数据计算空间哈希索引，以快速定位视野范围（FoV）内的根体素（root voxels）。这种方法有助于确定可视区域，提供精准的环境信息。
• 存储架构优化：由于哈希索引存储的 非连续性 限制了 GPU 的并行优化能力，系统采用滑动窗口机制 来优化。
• GPU 计算：全局高斯地图采用非连续的哈希八叉树 存储结构，主要存放在 RAM 中，以支持大规模场景的数据管理。视野范围内的高斯点采用连续内存布局，存放在RAM 和VRAM（显存）中，以提升GPU 计算效率。
• 优化流程：当需要优化高斯参数时，FoV内的数据从 RAM 传输到 VRAM 进行GPU并行计算，优化后再同步回 RAM，确保全局地图的更新一致性。

优势

• 提高计算效率：减少 GPU 访问非连续存储带来的额外开销，提高并行计算效率。
• 支持大规模环境：相比受限的显存（VRAM），RAM 容量更大且可扩展，通过 交换空间（swap space） 进一步增强处理能力。
• 适应复杂场景：递归八叉树结构可动态调整地图细节层级，提高建图精度。

该设计有效提升了 三维建图的可扩展性与计算效率，为大规模、多传感器 SLAM 提供了稳定可靠的解决方案。如图 3 所示。

图 3：高斯滑动窗口增量更新流程概览

高斯的初始化和优化

当接收到新的 LiDAR 和相机帧时，为了减少 GPU 内存消耗，系统首先对密集的 LiDAR 点进行体素下采样。与其他方法不同，本文采用八叉树的叶子节点体素来采样物体表面，代表场景。每个体素内的 LiDAR 点被选取并用来高效地表示场景。如果叶子体素未填满，将会使用 LiDAR 和相机数据来初始化新的高斯并插入到叶子体素中。

1) LiDAR-相机联合初始化高斯

在这一过程中，首先使用 LiDAR 初始化高斯的结构参数。具体来说，基于哈希体素的层级，初始化高斯的缩放矩阵，构造高斯的旋转矩阵和协方差矩阵。

• 缩放矩阵：根据哈希体素的级别初始化，并使用一个超参数来设定切片特征。
• 旋转矩阵：通过 LiDAR-惯性 SLAM 系统获取表面法向量来初始化。
• 协方差矩阵：基于旋转矩阵和缩放矩阵计算得到，随后，系统对 2D 高斯进行栅格化，结合 3D 高斯的影响。

2) 滑动窗口中的高斯实时优化

为了优化高斯参数，系统首先通过双线性插值初始化球面谐波系数。初始化后，使用光度梯度对高斯进行优化。

• 双线性插值：计算图像中非整数像素的颜色。
• 高斯图像渲染：通过阿尔法混合将高斯渲染到图像中。
• 优化问题：通过最小化光度损失，不断调整高斯结构参数和球面谐波系数，更新高斯参数，从而使渲染图像与观测图像匹配。

优化过程中，采用 Adam 优化器高效解决问题，不断提高高斯地图的准确性和视觉一致性。

高斯滑动窗口的维护

为了提高内存效率和优化计算速度，系统将优化范围限制在高斯滑动窗口内的高斯。通过这种有针对性的优化方法，系统显著提升了优化速度并减少了内存消耗。这不仅简化了计算过程，还最小化了显存的使用，从而提升了性能。此外，限制优化范围可以避免 3D-GS 实现中的图块深度排序问题。在原始的 3D-GS 中，图块深度排序可能会导致前景点污染背景，从而引发视觉伪影和深度表示的不准确。通过将优化限制在高斯滑动窗口内，避免了这种混叠问题，确保了前景和背景的清晰分离，从而提升了深度排序的整体质量。在每帧中重新构建高斯滑动窗口通常需要大量内存复制，造成显著的计算开销。然而，连续帧通常共享大部分场景，这使得大部分工作变得冗余。为此，系统引入了高斯滑动窗口的增量更新策略。该方法显著减少了不必要的内存传输，提升了实时性能，并能更有效地扩展到大型复杂环境。

高斯滑动窗口的维护包括以下关键组件：

• 空间哈希表 (SHT)：基于哈希的索引结构，将空间坐标映射到 CPU 内存中的指针，确保快速查找和高效组织高斯参数。
• CPU 高斯缓冲区 (CGB)：一个连续的内存区域，用于存储当前活动体素的高斯参数，简化数据传输到 GPU 的过程。
• GPU 高斯缓冲区 (GGB)：在 GPU 上分配的内存块，提供对高斯数据的直接访问，支持并行处理和快速渲染。

增量维护高斯滑动窗口的五个步骤：

• 更新全局地图：识别前一帧高斯滑动窗口中仍在当前视场内的体素，优化后的参数将被复制回全局高斯地图，其他体素将被标记为删除。
• 删除和压缩：将标记为删除的体素与滑动窗口序列后部的体素交换，删除处理后的体素，保持内存连续性。
• 重叠和新增：通过当前 LiDAR 帧计算空间哈希键，识别与前一帧滑动窗口重叠的体素，并确定需要新增的区域。
• 添加新叶子体素：将新增的体素添加到 CPU 高斯缓冲区，并更新空间哈希表。然后，将高斯数据从 CPU 内存直接传输到 GPU 内存，以便进行进一步优化和渲染。
• 增量更新：通过只更新可见性发生变化的体素，而不是重新加载整个滑动窗口，显著减少了冗余的内存操作，从而提升了实时性能。

这种方法不仅显著减少了内存操作，还通过仅优化当前视场内的高斯，降低了计算消耗，提高了实时性能。此外，利用 CPU 内存的扩展性和大容量，系统能够更好地处理大型和复杂的环境，提升了系统的稳健性和效率。

状态估计

在高斯渲染的基础上，我们重新设计了 FAST-LIVO2的视觉更新流程。与其通过将当前帧的图像块与参考帧进行变换来计算光度误差，采用了一种新的方法，通过比较从高斯地图渲染的图像与实际图像，均匀地计算当前帧的光度损失。由于高斯渲染具有平滑且可微分的特性，优化的收敛性得到了保证，这一点在 MonoGS 中已有证明。如图 2 所示，我们的里程计系统通过使用改进的 IESKF（增量扩展卡尔曼滤波器）与序列更新，紧密集成了 LiDAR 和图像测量数据。首先使用当前视场内优化后的高斯地图，通过 LiDAR 惯性更新后的姿态渲染新视图。高斯渲染的特性使得我们能够避免传统方法中的补丁拼接问题，提供无缝的渲染效果，并且能够渲染非兰伯特表面，呈现出更高质量的照片级效果。我们通过计算光度损失的雅可比矩阵，更新 IESKF 中的 IMU 姿态。

需要特别指出的是，大多数基于高斯点绘的 SLAM 方法主要依赖优化器来计算相机姿态更新，通常不会评估更新后的姿态的协方差。然而通过我们的方法，姿态及其协方差可以进一步传递到下一个传感器更新中，如 IMU 和 LiDAR，这有助于形成紧密耦合的 IESKF 系统。

实验

为了全面评估所提出的系统，我们在不同的计算平台上进行了实验，包括高性能台式机和嵌入式设备。首先在一台台式机（Intel i9-13900KF CPU，128 GB RAM 和 NVIDIA RTX-4090 GPU）上进行了与多个先进的 SLAM 算法的对比实验。结果表明，我们的方法在准确性和效率上与现有方法相当。随后将系统部署在嵌入式计算平台—NVIDIA Jetson Orin NX 上。尽管该平台的计算资源有限，我们的算法始终保持实时性能，突显了其在机器人平台上的适用性。

数据集准备

在我们的研究中，使用了多个数据集，包括公共数据集和自采集数据集。在公共数据集中，我们选择了 FAST-LIVO2 数据集，具体包括“CBD03”和“HKU01”序列，这些序列展示了大规模的大学场景。此外，我们还使用了 MARS-LVIG 数据集中的“HKairport01”和“HKisland03”序列，这些数据是在山脉和海洋等广阔自然环境中通过无人机收集的。MARS-LVIG 数据集的特点是包含了 D-RTK 系统，提供了精确的地面真实值来进行里程计。

为补充这些数据集，我们还收集了三个自有序列（“Playground01”，“Playground02”和“landmark01”），这些数据是在小规模室内环境中使用运动捕捉系统（MoCap）作为地面真值进行收集的。该设置使我们能够在受控条件下评估算法的准确性。为了确保数据质量，我们仔细校准了相机的内参，并校准了 LiDAR 和相机之间的外参。这些努力确保了公共和自有数据集的高质量，并为稳健评估提供了精确的校准和同步。

对比实验

在这一部分通过深入分析两个关键方面：高斯渲染性能下的建图质量和里程计精度，全面评估了我们的系统。

建图质量评估：我们将我们的方法与几种基于 LiDAR 的高斯重建方法进行了比较，包括 S3Gaussian和 LetsGo。对于里程计精度，我们的实验涵盖了传统的最新多传感器融合SLAM 系统，以及使用高斯地图的先进 SLAM 框架。采用不同的参数设置来比较室内和室外场景。在室内环境中，我们使用 0.03 米的细根体素大小，最大细分层数为 2，以捕捉细节特征。在大规模室外环境中，我们采用较粗的根体素大小（1.0 米），保持相同的细分层数。为了公平比较，我们每种方法运行 15,000 次迭代（相当于 1,500 帧的 10 次迭代），确保优化充分收敛。

如表 II 所示，首先将我们的方法与 LiDAR 集成的高斯重建方法（如 S3Gaussian 和 LetsGo）进行比较。尽管 LetsGo 通过基于距离的体素大小实现了自适应层次细节（LoD），但我们固定层次的八叉树结构在渲染质量上与其相当，并且减少了计算开销。效率提升的关键在于我们采用的滑动窗口策略，这使得我们能够实现实时的滑动窗口更新，而 LetsGo 则采用离线处理方法。

此外还将我们的系统与利用高斯表示的 RGB-D SLAM 方法进行了比较，特别是 SplaTAM和 MonoGS。为了比较将 LiDAR 测量转换为深度图。这些方法采用不同的地图管理策略—SplaTAM 使用基于轮廓的选择，而 MonoGS 则依赖于基于共视性的关键帧选择。我们的方法直接在 3D 体素空间中对高斯点进行下采样，从而更高效地提取结构特征。此外IMU 测量的集成提供了运动先验，比纯 RGB-D 方法更能增强快速运动和振动下的鲁棒性。图 5 展示了三个不同的现实场景（HKU 校园、UAV 游乐场和一个著名的地标）。对于每个场景，我们展示了：（a）从相机视角的渲染，（b）从漫游视角的高斯可视化，以及（c）底层的高斯结构。第一个场景展示了高渲染保真度，建筑标牌上的 HKU 字样清晰可见。第二个场景展示了精确的几何重建，游乐场上的棋盘图案清晰可见。第三个场景中，铭文被渲染得非常细致。（c）中的子图显示，高斯点自然沿着表面方向延伸，展示了我们的方法如何通过联合 LiDAR 可视化优化有效捕获场景几何。

图 5：三个不同现实场景的建图结果（a）-（c）。第一排：来自相机姿态的渲染结果。第二排：来自漫游视角的渲染结果。第三排：场景高斯形状。

定位评估：如图 6 所示，对不同 SLAM 系统的性能进行了综合分析。在室内环境中（图 6(a)），我们的方法在精度上与传统的基于 LIV 的 SLAM 方法相当，同时比其他基于高斯的方法高效得多。在室外场景中（图 6(b)），我们的方法表现出更高的精度，RMSE 为 0.042 米，而 R3LIVE 为 1.465 米，LVI-SAM 为 4.665 米。尽管 GS-LIVO 的处理时间稍高于某些传统方法，但它在提供增强的映射能力的同时，始终保持实时性能。在不同数据集上的一致性评估中，设置了如下实验参数：图像分辨率为 640×480，室内环境的八叉树配置为（0.06 米，2 层），室外环境为（0.5 米，2 层），滑动窗口大小为 100,000 个高斯点进行增量地图更新。

图 6：不同 SLAM 系统在准确度（RMSE）和计算效率（处理时间）方面的性能比较。

如表 III 所示，GS-LIVO 展现了竞争力的定位精度，明显优于传统方法如 R3LIVE，同时在性能上与 FAST-LIVO 相当，精度略低。就计算效率而言，我们的系统在室内和室外环境中的处理时间都保持在 90 毫秒以下。处理时间的差异主要归因于我们的映射方法：尽管 FAST-LIVO 通过稀疏视觉子地图变换实现了高效的姿态优化，但我们的系统通过实时的稠密高斯地图更新，在优化相机姿态的同时保持了光度准确性。

综上所述，我们的系统在大规模室外环境中的表现卓越，维持了实时性能并在轨迹的 RMSE 为 0.58 米时显著优于传统方法如 R3LIVE 和 LVI-SAM。在计算效率上，尽管 R3LIVE 和 LVI-SAM 在处理大地图时面临更高的计算开销，我们的系统通过高效的高斯点滑动窗口管理，保持了接近实时的性能，同时维持了逼真的地图渲染效果。

滑动窗口消融研究

在消融研究中比较了有无滑动窗口的高斯处理下的显存使用情况、优化时长，以及在室内和室外序列中地图维护过程所需的处理时间。

1. 内存消耗：在室内（如图10所示）和室外（如图11所示）环境中，使用滑动窗口处理高斯数据显示出显著的优势。我们的策略是将当前视场内的活动高斯存储在GPU内存中，同时通过CPU内存中的八叉树结构维护全局地图，这在映射质量和计算效率之间达到了最佳平衡。如图10(a)和图11(a)所示，这种方法在显著降低内存消耗的同时，保持了与完全GPU实现相当的高峰信噪比（PSNR）值（图10(e)和图11(e)）。这种高效的地图管理使我们的系统能够处理大规模环境和复杂场景，而传统的3D-GS方法会遇到GPU内存限制，展示了我们方法的实际扩展性。
2. 时间消耗：如图10(b)和图11(b)所示，采用滑动窗口策略后，总处理时间——包括窗口维护和高斯优化——在室内和室外环境中始终保持在100毫秒以下，实现了10Hz的实时更新。相比之下，不使用滑动窗口优化的方法随着地图的增大，计算时间不断增加，导致在大规模场景中的实时性能受到阻碍。

图11：滑动窗口方法在室外环境中的性能分析（HKisland03.bag序列）

处理时间的细分，如图12所示，表明我们的滑动窗口策略在不同组件中实现了高效的响应时间。室内场景的平均处理开销为23毫秒，室外环境为71毫秒，这可以通过根据可用计算资源调整细节层次（LoD）参数进一步优化。这种适应性使得我们的方法适用于各种平台和场景。重要的是，如图10和图11所示，我们的滑动窗口方法在显著减少计算开销的同时，保持了高质量的地图。系统在视角变化时仅出现暂时性的PSNR下降，但通过在滑动窗口内的迭代优化，PSNR很快恢复到25dB到30dB之间，证明了我们的效率提升并未以牺牲映射质量为代价。

图12：建图过程的时间消耗分析

嵌入式系统实验

为了验证我们算法的效率，我们将GS-LIVO部署在一台配备NVIDIA Jetson Orin NX的移动平台上（如图8(e)所示），配置了0.5m的根体素大小、2层子分割、256×216的图像分辨率，以及20,000个高斯的滑动窗口大小。

该系统在嵌入式平台（ORIN NX 16G）上保持实时性能，优化耗时15.3毫秒（图8(b)），地图维护耗时18.9毫秒（图8(c)），整个管道耗时48.3毫秒，同时达到23.52dB的PSNR（图8(a)）。

图 8：GS-LIVO 在嵌入式平台上的性能评估：(a)-(d) 系统指标，包括 PSNR 和处理时间分析；(e) 我们的传感器套件与安装在移动底盘上的 Jetson Orin NX 集成。

为了进一步展示我们提出的里程计的实时性和高精度性能，我们将GS-LIVO集成到一个完整的自主导航系统中。该高斯地图被处理生成2D占用网格用于路径规划，而里程计则提供实时定位以进行轨迹跟踪。集成系统成功演示了使用标准规划和控制算法（A*用于全局规划，LQR用于轨迹跟踪）进行自主导航。据我们所知，这是第一个成功部署在基于ARM的嵌入式平台上的实时高斯基础SLAM系统，且能够实现在线地图更新。

总结

本文提出了GS-LIVO，一种新型的实时SLAM系统，集成了传统的激光雷达-惯性-视觉里程计和新型的3D高斯泼溅点云表示地图。通过用基于高斯的场景表示替代传统的彩色点云和稀疏补丁地图，我们的系统实现了精确的定位和高保真度的建图。主要贡献包括：(1) 一种基于空间哈希索引的八叉树结构，用于高效管理全局高斯地图；(2) 激光雷达-视觉联合初始化，用于高保真度建图；(3) 一种增量式滑动窗口策略，用于实时地图优化；(4) 使用IESKF的紧耦合多传感器融合框架。虽然现有的基于高斯的SLAM系统通常能实现实时定位，但在实时地图更新方面存在困难，我们的系统利用传统多传感器融合的优势，在保持高频地图更新的同时实现了紧耦合的里程计。值得注意的是，GS-LIVO是第一个成功部署在NVIDIA Jetson Orin NX平台上的基于高斯的SLAM系统，展示了其在实际机器人应用中的潜力。大量实验表明，GS-LIVO在室内和室外环境中都实现了优越的性能，减少了内存消耗和优化时间，同时与现有方法相比，保持了高质量的渲染效果。我们的八叉树高斯地图有效地表示了场景，但未来的研究可以探讨基于视距、结构复杂性和纹理丰富度的自适应细节层次技术。此外，对于均匀区域，合并颜色相似的高斯点云可能进一步优化内存使用和计算效率。然而这些高级体素管理机制将引入额外的复杂性，需要进行仔细研究。

相关阅读：2024年度历史文章大汇总

以上内容如有错误请留言评论，欢迎指正交流。如有侵权，请联系删除