0.基本信息

题目：《iG-LIO: An Incremental GICP-Based  Tightly-Coupled LiDAR-Inertial Odometry》

来源：2024 IEEE RAL

开源：https://github.com/zijiechenrobotics/ig_lio

摘要：一种基于GICP的快速LIO系统

本文提出了一种增量式的基于广义迭代最近点( Generalized Iterative Closest Point，GICP )的LiDAR -惯性紧耦合测程法( LIO )，iG - LIO将GICP约束和惯性约束整合到一个统一的估计框架中。iGLIO使用基于体素的表面协方差估计器来估计扫描的表面协方差，并使用增量体素图来表示周围环境的概率模型。这些方法成功地减少了协方差估计、最近邻搜索和地图管理的时间消耗。从机械式激光雷达和固态激光雷达采集的大量数据集被用来评估所提出的LIO的效率和准确性。 尽管iG - LIO在所有数据集上都保持了相同的参数，但结果表明它比Faster - LIO更有效，同时保持了与最先进的LIO系统相当的精度。

Introduction

LiDAR（激光雷达）和惯性测量单元（IMU）的融合在自主导航系统中被广泛应用，尤其是在没有绝对定位（如GNSS，全球导航卫星系统）的未知环境中。这种融合技术是实现安全导航、SLAM（同时定位与建图）前端处理以及大规模地图构建的关键技术。近年来，LiDAR里程计（LO）和LiDAR-惯性里程计（LIO）的效率和精度通过改进地图管理和配准方法得到了显著提升。然而，现有方法在处理大规模点云数据、稀疏点云或小视场（FOV）LiDAR时仍面临效率和鲁棒性的挑战。

本文提出的 iG-LIO 是一种基于增量广义迭代最近点（GICP）的紧耦合LiDAR-惯性里程计，旨在解决以下问题：

1. 现有GICP-based LIO方法在小视场或稀疏点云场景下的鲁棒性不足。
2. 传统基于kd-tree的最近邻搜索在处理密集点云时效率较低。
3. 现有方法在构建局部地图和估计表面协方差时计算成本高，难以实现实时配准。

iG-LIO 通过创新的地图管理方法（增量体素地图）和表面协方差估计器（VSCE），在保持高效的同时提升了在不同场景（密集点云、稀疏点云、小视场）的鲁棒性和精度。

现有问题

1. 地图管理问题

• 传统方法：如LOAM及其变种使用 kd-tree（k维树）组织点云的局部地图空间结构。kd-tree适合快速搜索最近邻点，但当局部地图更新时，重建kd-tree的计算成本较高（因为地图可能包含数千到数百万点）。
• FastLIO2的改进：提出了一种增量kd-tree（ikd-tree），支持动态插入和重新平衡，降低了重建kd-tree的时间成本。然而，ikd-tree的最近邻搜索复杂度为 O(m log n)（其中n是点云数量，m是维度），在处理大规模点云时仍难以满足实时性要求。
• 体素化方法：一些方法（如Faster-LIO、AdaLIO）通过体素化（voxelization）组织点云，将空间划分为体素网格，最近邻搜索的查询时间降为 O(1)，显著提高了配准速度。AdaLIO在Faster-LIO基础上引入自适应策略，解决了狭窄环境中配准发散的问题，并在2023年Hilti SLAM挑战赛中获得第一。

2. 配准方法问题

配准方法是LIO的核心，现有配准方法包括：

• 基于几何特征的度量：如LOAM及其变种，依赖点到线、点到面等几何约束。
• 基于密集表面表示的度量：如某些方法利用密集点云的表面信息。
• 基于概率分布的度量：如广义迭代最近点（GICP），通过为每个点估计表面协方差，改进传统ICP（迭代最近点）算法，减少错误对应点的影响。

GICP的优点与局限性：

• GICP通过估计点云的表面协方差，结合点到线、点到面、面到面等几何特征，提升了配准精度。
• Voxel-based GICP（VGICP）：将体素化与GICP结合，可在CPU上以30Hz的频率处理1.5万个点。然而，现有GICP-based LIO方法存在以下问题：
  1. 非紧耦合：与IMU数据的融合不够紧密，导致在小视场LiDAR（如固态LiDAR或室内场景）中鲁棒性不足。
  2. 表面协方差估计：传统GICP依赖kd-tree进行最近邻搜索，计算成本高，且在稀疏点云或小视场场景中，表面协方差估计精度下降。
  3. 实时性挑战：在密集点云场景中，构建局部地图和估计表面协方差的计算成本高，难以实现实时配准。

3. 稀疏点云与小视场场景的挑战

• 小视场LiDAR（如固态LiDAR，采样频率100Hz）或室内环境的点云通常稀疏，点与点之间的距离较远，导致表面协方差估计不准确，进而影响配准精度。
• 在密集点云场景中，GICP依赖kd-tree的最近邻搜索效率较低，难以满足实时性需求。

为了解决上述问题，iG-LIO提出了一系列创新方法，具体包括以下贡献：

1. 紧耦合GICP与IMU约束

• iG-LIO将GICP的配准约束与IMU测量约束通过**最大后验估计（MAP）**紧密结合。这种紧耦合设计提升了系统的鲁棒性，特别是在小视场或稀疏点云场景中。
• 相比现有GICP-based LIO（如DLIO），iG-LIO更充分地利用了IMU的原始测量数据，增强了系统的稳定性。

2. 体素化表面协方差估计器（VSCE）

• iG-LIO提出了一种体素化表面协方差估计器（VSCE），用于高效且准确地估计点云的表面协方差。
• 与传统方法的对比：
  • 传统GICP依赖kd-tree进行最近邻搜索，计算复杂度高（O(m log n)）。
  • VSCE利用体素结构，将最近邻查询时间降为O(1)，显著提高了密集点云场景下的处理效率。
  • 在稀疏点云和小视场场景中，VSCE通过优化体素化策略，保持了表面协方差估计的精度（详见实验部分III-B3、III-B5、III-C1）。
• 优势：VSCE在密集点云中减少了计算时间，同时在稀疏点云中维持了配准精度，解决了GICP在不同点云密度场景下的效率与精度平衡问题。

3. 增量体素地图

• iG-LIO设计了一种增量体素地图，用于表示周围环境的概率模型。
• 与非增量方法的对比：
  • 传统方法（如DLIO）每次更新地图时需要重建整个空间结构，计算成本高。
  • 增量体素地图支持动态更新，显著降低了最近邻搜索和地图管理的耗时（详见实验部分III-A）。
• 优势：通过增量更新，iG-LIO在处理大规模点云时保持了高效性，同时支持概率模型的构建，提升了环境表示的鲁棒性。

4. 开源与实验验证

• iG-LIO的源代码已开源至GitHub，方便社区使用和进一步开发。
• 通过在不同视场LiDAR（包括密集和稀疏点云）上收集的广泛数据集进行实验，验证了iG-LIO的性能：
  • 效率：iG-LIO比Faster-LIO更高效，尤其在密集点云场景中。
  • 精度：在多种场景下与现有最先进的LIO系统（如AdaLIO等）相比具有竞争力。
  • 鲁棒性：即使在所有数据集上使用相同的参数配置，iG-LIO也能保持稳定的性能。

1. INCREMENTAL GICP-BASED TIGHTLY COUPLED LIDAR-INERTIAL ODOMETRY

A. System Overview

        图2展示了所提出的里程计的框图。从IMU积分开始，iG - LIO补偿扫描的运动失真，生成先验约束，并输出IMU速率里程计。然后，基于体素的表面估计器对无失真扫描进行采样，以估计每个点的表面协方差。接下来，扫描通过哈希索引实现体素图中的最近邻搜索，以实现GICP约束。先验约束和GICP约束都被集成到一个MAP中，以估计传感系统的状态。最后，将扫描增量插入到体素图中，以更新每个体素网格的分布。MAP的紧耦合loss function如下：

        其中，残差 rGICP 和 rprior 分别表示 GICP constraints 和 prior constraints。矩阵 ΩGICP 和 Ωprior 分别表示其信息矩阵. 

B. IMU Constraints

        IMU积分预测先验估计并传播误差状态的协方差。定义离散时间区间为，iG - LIO通过中值积分预测先验估计 .

        其中，ωm 和 αm 是IMU原始观测. 向量 gw 为世界坐标系下重力常量。为了表达公式（2）中先验约束的不确定性，iGLIO传播误差状态的协方差。此过程类似于IESKF的前向传播。

其中，n为IMU加速度和角速度原始观测的高斯白噪声，b为加速度和角速度bias。误差状态的协方差传播公式如下，其中Q为处理噪声的协方差.

        iGLIO状态将一直由IMU积分得到，直到收到一帧激光的观测，（2）中的先验约束如下：

C. Voxel Map

        在每次测量更新后，iG - LIO增量式地将激光点添加到体素图中，并更新每个体素的概率模型来表示周围环境。该方法在如下方面提高了效率和准确性。

D. Measurement Update

E. The iG-LIO Algorithm

实验

数据集

NCLT（Michigan大学校园长期数据集，使用Velodyne HDL-32E）、NCD（Newer College数据集，使用Ouster OS1-64）、ULHK（香港城市动态环境数据集，使用Velodyne HDL-32E）、BG（Botanic Garden植物园数据集，使用Velodyne VLP-16和Livox AVIA）、AVIA（FastLIO2和r3live提供的Livox AVIA手持数据集）、GDUT（自采集数据集，使用Livox AVIA）。

硬件平台

i7-10875H CPU（2.30 GHz × 16核）和32 GB RAM上运行，使用Ubuntu 18.04的ROS系统。

baseline

FastLIO2 、FasterLIO  和 DLIO

A. 效率

效率评估通过每个扫描的平均运行时间和有效特征点数来衡量。iG-LIO的VSCE使用26个邻近0.5 m体素估计表面协方差。

结果分析（Table II）：

• 在100 Hz数据集（avia_2 和 avia_3）中，Faster-LIO略快于iG-LIO，因为高频采样导致点云稀疏，减少计算量。
• 在其他数据集（如NCLT、NCD、ULHK、BG）中，iG-LIO比Faster-LIO快1.2～1.5倍、比FastLIO2快2.3～2.7倍、比DLIO快1.5～3.5倍。
• DLIO在非重复环境中重建子地图，导致时间比FastLIO2更长（除NCD外）。
• iG-LIO的有效特征点数适中，确保效率与精度平衡。

优化细节（Fig. 3）：

• 优化阶段：iG-LIO使用std::for_each执行for循环，并通过tbb::parallel_reduce并行化Hessian矩阵和残差积累，比Faster-LIO更高效。
• 去畸变阶段：iG-LIO移除冗余逻辑，减少点云内存拷贝操作，提升性能。

解释：iG-LIO的增量体素地图和VSCE的O(1)查询复杂度，使其在密集点云（如Ouster OS1-64的NCD）中表现出色。相比kd-tree-based方法，体素化并行处理更适合多核CPU。

---

B. 精度和鲁棒性

本节在各种环境中验证iG-LIO的精度，使用绝对姿态误差（APE，Table III）和端到端漂移（Table IV，用于无真值的数据集AVIA和GDUT）。APE计算使用evo工具包，端到端误差评估轨迹起点和终点的闭合度。

注意事项：

• DLIO结果可能与原始论文不同，因为开源代码有修改。
• FastLIO2在NCD起始处调整滤波点数以避免滑动，与[2]一致但不同于[23]。
• iG-LIO在所有数据集使用相同参数，展示鲁棒性。

1) 结构化环境：

• 数据集：NCLT（长期校园序列，如nclt_1: 2012-01-15, 7.58 km）、NCD（学院序列，如ncd_1: 01_short_experiment, 1.61 km）、GDUT（自采集结构化序列）、AVIA（avia_2 和 avia_3: 手持Livox AVIA, 100 Hz户外短序列, 0.14 km 和 0.09 km）。
• 结果：NCLT中iG-LIO的APE约为0.5%～1.1%，与FastLIO2相当。NCD中iG-LIO精度与他人相当，但速度更快。DLIO在密集NCD中性能下降。
• avia_2（100 Hz）：Faster-LIO和FastLIO2有初始漂移，iG-LIO最小漂移并返回起点，因为紧耦合和GICP处理非重复扫描。

解释：结构化环境（如建筑物、道路）提供丰富几何特征，iG-LIO的scan-to-map配准增强全局一致性。

2) 剧烈运动：

• 数据集：ncd_4（06_dynamic_spinning, 0.09 km），操作员手动翻转系统，最大角速度183 deg/s。
• 结果：由于NCD真值有3 cm静态误差[NCD]，iG-LIO和FastLIO2的APE视为相同。但iG-LIO的地图更清晰（Fig. 4(a)和(b)）。
• 改进：iG-LIO使用中点积分补偿运动畸变，而FastLIO2使用Euler方法，导致畸变残留。

解释：剧烈运动测试IMU融合，紧耦合MAP估计限制不可观测状态。

3) 室内环境：

• 数据集：avia_1（hku_main_buiding, 0.96 km，大规模室内外手持Livox AVIA）。
• 结果：Faster-LIO和FastLIO2在狭窄空间漂移（Table III），FastLIO2在转弯处地图不一致（Fig. 4(c)）。iG-LIO返回原点，地图一致。
• 原因：小视场LiDAR在狭窄环境中观察不足，iG-LIO的VSCE和GICP减少错误对应。

解释：室内狭窄空间测试鲁棒性，iG-LIO的概率模型处理小FOV优于几何特征方法。

4) 动态环境：

• 数据集：ULHK（城市动态序列，如ulhk_1: HK-Data20190117, 0.60 km，密集人群和动态物体，使用Velodyne HDL-32E）。
• 结果：DLIO性能下降（Table III），iG-LIO保持精度，通过卡方检验移除不合适平面近似和匹配错误。

解释：动态物体（如行人、车辆）引入噪声，iG-LIO的异常值移除机制提升鲁棒性。

5) 非结构化环境：

• 数据集：BG（植物园序列，如bg_1: 1006-01, 0.76 km，使用Velodyne VLP-16和Livox AVIA，振动、密集树木、狭窄路径）。
• 结果：Faster-LIO和FastLIO2在Livox AVIA上有显著漂移（Table III），因为小FOV导致重力方向与陀螺仪偏置耦合。iG-LIO固定重力方向约束姿态估计，在不同FOV系统中的APE变异最小。

解释：非结构化（如森林）缺乏清晰特征，VSCE和局部平面近似增强适应性。

---

C. 消融实验

消融实验验证关键组件：VSCE的有效性、GICP vs NDT、GICP vs VGICP。

1) VSCE的有效性：

• 变体：iG-LIO* 使用kd-tree范围搜索（半径为体素滤波分辨率的2倍）估计表面协方差[GICP],[VGICP]。
• 结果：iG-LIO的面到面度量比例比iG-LIO*低12%（Table II），但精度相当（Tables III、IV），效率更高。Fig. 3显示iG-LIO在协方差估计时间上优越，因为哈希索引O(1)并使用libtbb并行。

解释：VSCE的体素化比kd-tree更高效，尤其在密集扫描中。

2) GICP约束 vs NDT约束：

• 变体：NDT-LIO 将(10)中的协方差设为零，保留(9)，分辨率0.5 m。
• 结果：GICP优于NDT（Tables III、IV）。在nclt_5（2013-01-10, 1.14 km）不同分辨率下（Fig. 5），两者绝对平移误差随分辨率增大而增，但NDT增幅更快。

解释：GICP的表面协方差建模更鲁棒，尤其在不合适分辨率下。

3) GICP约束 vs VGICP约束：

• 变体：iVG-LIO 基于VGICP，使用kd-tree找20个最近点估计协方差，体素更新为(16)（累积协方差和）。
• 结果：在avia_1（室内）和avia_2（100 Hz）中，iG-LIO地图保留更精细结构细节（Fig. 6）。iVG-LIO在稀疏扫描中协方差估计不准，因为远距离点无法建模局部表面，且(16)累积估计误差影响配准。

解释：VGICP适合密集扫描，但iG-LIO的VSCE和增量更新更适合稀疏和小FOV场景。