Faster-LIO 的优势是在保持同等高精度的前提下,显著提升计算效率,以支持更极端的应用场景(如高速无人机)和更廉价的硬件平台。可以把 Faster-LIO 理解为用一把更快的“空间哈希筛子”(iVox)替换了原来精细的“动态平衡树”(ikd-Tree),从而在精度损失极小的情况下,实现了效率的显著提升
Faster-LIO 与 FAST-LIO2 的核心差异
两个框架在核心思想上同源,但关键区别在于维护地图和进行近邻搜索的数据结构,
对比维度 | FAST-LIO2 | Faster-LIO |
核心数据结构 | 增量KD-Tree (ikd-Tree) | 增量体素 (iVox) |
建图方式 | 动态树结构,需要平衡 | 基于空间哈希的稀疏体素网格 |
近邻搜索 | 精确的 kNN 搜索,计算量随地图增大 | 并行近似 kNN 搜索,速度极快 |
主要优势 | 精度高,数据结构成熟通用 | 计算效率更高,更易并行化 |
Faster-LIO的创新
iVox (Incremental Voxel)
iVox 是 Faster-LIO 速度飞跃的关键。它抛弃了传统的树形结构,采用一种更符合空间直觉和现代CPU硬件特性的“哈希表+体素”设计。设计思想:只在实际有地图点的空间区域动态创建和保留体素单元,形成一个稀疏的体素地图。通过空间哈希函数,将三维坐标快速映射到哈希表中的唯一索引,实现O(1)复杂度的体素查找。
iVox 的实现主要在 ivox3d.h/cpp 中。
它构建了一个稀疏的、增量更新的体素网格。核心类是IVoxNode,代表一个体素(立方体),其内部通常用一个 vector<PointType> 来存储落入该体素的所有点。而 IVox3D 类则管理一个哈希表 std::unordered_map<KeyType, IVoxNode*>, 将三维体素坐标映射到具体的节点。
在体素较大时,线性搜索内部所有点效率低。PHC (PHC近邻搜索加速
Pseudo Hilbert Curve) 的引入是点睛之笔:
原理:将一个大体素细分成更小的栅格,并为每个小栅格计算一条一维的希尔伯特空间填充曲线编码。这条曲线的美妙之处在于,三维空间上相邻的小栅格,其一维编码也接近。
代码实现:在IVoxNode 内部,除了存储原始点云,还维护一个按 PHC 编码排序的点云列表。当需要搜索点 p 的近邻时:
计算p 在大体素内对应的小栅格 PHC 编码 C_p。
在排序列表中,以C_p 为中心,向前后扩展一个固定窗口进行搜索。
窗口内的点由于编码邻近,它们在三维空间中也极可能是p 的真正近邻。
优势:将三维近邻搜索转化为一维有序列表上的局部遍历,极大减少了需要计算距离的点数,并具有出色的缓存局部性。
两种实现:(1)线性 iVox:每个体素内,点线性存储。当体素内点云较稀疏时,直接遍历搜索近邻,简单高效。(2)PHC iVox:当体素较大、内部点云密集时,使用伪希尔伯特空间填充曲线为体素内的细分空间建立一维索引。该曲线能保持空间邻近性,将三维近邻搜索转化为一维区间查询,大幅加速。
核心优势:
增量更新:新点云到来,只需插入对应的稀疏体素,无全局重构开销。
并行友好:不同体素间的近邻搜索相互独立,极易实现多线程并行,充分利用多核CPU。
Faster-LIO 算法流程与代码框架
Faster-LIO 沿用了 FAST-LIO2 中成熟的紧耦合迭代误差状态卡尔曼滤波框架,其核心流程如下图所示,可以分为以下几个关键步骤:
主要模块功能解析:
IMU预积分与点云去畸变:与FAST-LIO2一致,利用IMU数据对扫描期间的运动进行补偿,得到去畸变的单帧点云。
紧耦合状态估计:核心状态估计器仍是IESKF。状态变量包含位姿、速度、IMU零偏等。观测模型是关键:将去畸变点云中的每个点,在 iVox 维护的局部地图中搜索近邻(如5个最近点),构建点到面的距离残差。
地图管理与输出:状态优化后,将新点云插入 iVox。同时,采用滑动窗口策略,删除距离当前机器人位置最远的体素,以控制地图规模。最终输出高频(可高达1000Hz以上)的激光雷达里程计位姿。
代码框架核心模块解读
以开源代码https://github.com/gaoxiang12/faster-lio 为例,核心模块通常包括:
·
laserProcessing.cpp:负责接收原始点云,进行运动补偿和必要的滤波,输出去畸变点云。
·
·
imuProcessing.cpp:处理IMU数据,进行预积分,为状态预测提供增量。
·
·
ieskf.cpp(或 estimator.cpp):包含IESKF滤波器的核心实现,负责执行状态预测、观测更新、迭代优化的完整流程。
·
·
ivox3d.h/cpp:iVox 数据结构的实现核心。包含空间哈希函数、体素管理、以及并行近邻搜索(PHC或线性)的接口。
·
·
map.cpp:基于ivox3d 封装局部地图管理功能,包括点云插入、滑动窗口删除、以及为状态估计模块提供近邻搜索服务。
·
参数调整相关
参数文件 | 关键参数 | 作用与影响 | 典型值/调整建议 |
params.yaml | voxel_size | iVox 基础体素的尺寸。最重要的参数之一。值越小,地图分辨率越高,但近邻搜索涉及的体素数量增多。值越大,每个体素内点越多,PHC加速效果更明显,但地图会变得更粗糙。 | 0.2 - 1.0 米。室内环境建议使用较小值(如0.2m-0.5m),室外空旷环境可使用较大值(如0.5m-1.0m)。 |
params.yaml | ivox_search_method | 指定 iVox 内部使用的搜索方法。可选 IVOX_PHC 或 IVOX_LINEAR。 | 默认IVOX_PHC。当voxel_size 设置得很小,或环境导致点云极为稀疏时,可尝试切换为IVOX_LINEAR 进行对比。 |
params.yaml | max_iteration | IESKF 每次状态更新时的最大迭代次数。影响状态优化求解的收敛精度。 | 3-5次。通常3次已足够收敛。增加次数可能略微提升精度,但会显著增加单帧计算量。 |
params.yaml | cube_side_length | 滑动窗口局部地图的边长。决定了系统在内存中保留多少历史数据,直接影响内存消耗和长期运行的稳定性。 | 200米。此值需大于传感器的有效探测距离。设置过大会浪费内存,过小则可能导致机器人移动时丢失历史参考而定位失败。 |
params.yaml | filter_size_corner / filter_size_surf | 点云在预处理时的降采样分辨率(单位:米)。此步骤直接减少参与计算的点云数量。 | 0.5米。在保证环境特征不丢失的前提下,增大此值可以显著减少点数,是提升整体速度最有效的手段之一。 |
参数文件 | 关键参数 | 作用与影响 | 典型值/调整建议 |
params.yaml | voxel_size | iVox 基础体素的尺寸。最重要的参数之一。值越小,地图分辨率越高,但近邻搜索涉及的体素数量增多。值越大,每个体素内点越多,PHC加速效果更明显,但地图会变得更粗糙。 | 0.2 - 1.0 米。室内环境建议使用较小值(如0.2m-0.5m),室外空旷环境可使用较大值(如0.5m-1.0m)。 |
params.yaml | ivox_search_method | 指定 iVox 内部使用的搜索方法。可选 IVOX_PHC 或 IVOX_LINEAR。 | 默认IVOX_PHC。当voxel_size 设置得很小,或环境导致点云极为稀疏时,可尝试切换为IVOX_LINEAR 进行对比。 |
params.yaml | max_iteration | IESKF 每次状态更新时的最大迭代次数。影响状态优化求解的收敛精度。 | 3-5次。通常3次已足够收敛。增加次数可能略微提升精度,但会显著增加单帧计算量。 |
params.yaml | cube_side_length | 滑动窗口局部地图的边长。决定了系统在内存中保留多少历史数据,直接影响内存消耗和长期运行的稳定性。 | 200米。此值需大于传感器的有效探测距离。设置过大会浪费内存,过小则可能导致机器人移动时丢失历史参考而定位失败。 |
params.yaml | filter_size_corner / filter_size_surf | 点云在预处理时的降采样分辨率(单位:米)。此步骤直接减少参与计算的点云数量。 | 0.5米。在保证环境特征不丢失的前提下,增大此值可以显著减少点数,是提升整体速度最有效的手段之一。 |