高翔博士Faster-LIO论文和算法解析

说明

题目：Faster-LIO：快速激光IMU里程计

参考链接：
Faster-LIO：快速激光IMU里程计
iVox (Faster-Lio): 智行者高博团队开源的增量式稀疏体素结构

Faster-Lio是高翔博士在Fast系列的新作，对标基线是Fast-LIO2，核心是提出一种新的"数据结构"ivox，类似于Fast-LIO2中的ikd-tree，用于点云配准搜索和地图的增量更新。效果上整个LIO系统的用时相比Fast-LIO2有较大幅度的下降，精度上评估了每百米的APE和RPE，从结果上看有好有差。近年来的作品精度上似乎都大差不差，如LIO-SAM，FAST-LIO系列，更多的都是在卷实时性。

对于本文来说需要重点理解增量稀疏体素结构ivox和ivox-PHC,对于整个系统的流程可以参考FAST-LIO2。

摘要

本文提出了一种基于增量体素的激光惯性里程计(LIO)方法，用于快速跟踪机械式和固态激光雷达扫描。为了达到较高的跟踪速度，我们既没有使用复杂的基于树的结构来划分空间点云，也没有使用严格的k近邻(K-NN)查询来计算点匹配。相反，我们使用增量体素(iVox)作为我们的点云空间数据结构，它是在传统体素的基础上修改的，支持增量插入和并行近似k-NN查询。我们提出了线性iVox和PHC(伪希尔伯特曲线)iVox作为我们算法中的两个备选底层结构。实验表明，ivox的扫描速度在固态激光雷达中达到1000-2000Hz/秒，在32线旋转激光雷达中仅使用现代CPU扫描速度超过200hz，同时仍保持相同的精度水平。

一、简介

---

论文的出发点在于提高点云配准时的处理效率，在Fast-Lio2中使用了ikd-tree来提高点云的搜索效率，但kd-tree的维护以及挨个节点的遍历同样需要消耗一定的时间，因此提出一种基于稀疏体素的近邻结构iVox（incremental voxels），该方法可以有效的降低点云配准时的耗时，也不会影响LIO的精度表现，iVox是本文的核心内容。

相比与八叉树体素结构的完全体素化，iVox提供一种稀疏的体素表达，即只在空间点存在的地方建立和维护体素结构，因为空间点的位置是唯一的，可以使用一个哈系表来统一构建和维护体素结构，将空间点的坐标作为Key，将坐标带入哈系函数生成的唯一索引作为value。对于少量点匹配提出线性iVox结构，对于大量点的配准提出iVox-PHC，以便快速地查找任意近邻。

---

高速点云配准和三维重建是许多项目产品的关键模块——从高清地图(HD maps)[1] -[3]到自动驾驶汽车[4]，[5]。最常见的实时激光雷达跟踪方法，如LOAM[6]、LeGO-LOAM[7]和balm[8]，每次迭代大约需要100毫秒来处理一次激光雷达扫描。大多数传统的旋转激光雷达在这个速度下提供多线扫描。随着光学技术的发展，Livox和Cepton等现代固态激光雷达传感器可以提供100 Hz甚至更高频率的密集点云扫描[9]，[10]。因此，寻找一种高效的激光雷达跟踪方法已成为近年来重要的研究课题[11]，[12]。对于许多真实的机器人或车辆来说，LIO并不是系统中运行的唯一算法。所有活动模块必须共享计算资源。如果我们有一个更快、更轻的LIO，这个系统会更坚固。此外，还可以使用更快的LIO方法作为离线地图系统的前端，以帮助减少计算时间。

基于激光雷达或视觉的SLAM系统通常包括用于点云跟踪的实时前端和用于状态优化的后端[13]-[15]。在现实世界的激光雷达里程计系统中，人们还将以松耦合或紧耦合的形式将惯性和GPS测量融合到状态估计器中，以使系统对短时传感器故障更具鲁棒性[16]，[17]。从理论上分析整个复杂SLAM系统的计算成本并不容易。然而，一般来说，对于前端部分（LO/LIO模块），计算时间成本主要来自以下几个方面：

（1）所选的空间数据结构
由于传统的配准方法通常依赖于点云中的k近邻（k-NN）搜索，因此可以通过利用更快的k-NN数据结构来提高配准效率[19]。一些为不变空间数据库设计的数据结构(如R*-tree[20]、B*-tree[21])不适合实时LIO，因为LIO需要快速构建和查询速度。此外，由于点云是按顺序处理的，因此增量更新结构会更好。因此，体素和k-d树（及其变体）[22]、[23]是LO/LIO系统的更好选择。当然，体素和k-d树仍然有其局限性。体素几乎在恒定时间内易于构建和删除，但不能进行严格的k-NN搜索或范围搜索。k-d树可以提供严格的k-NN搜索和范围搜索结果，但需要额外的努力来摆动和平衡树。

*（2）状态估计器的创建
姿态估计器也会影响LIO模块的计算成本。在现代VIO或LIO系统中，人们倾向于在传统的单帧卡尔曼滤波器和全姿态图优化/BA之间选择折衷解决方案，如滑动窗口滤波器（SWF）[24]、多状态约束卡尔曼滤波器（MSCKF）[25]或迭代扩展卡尔曼滤波器（IEKF）[26]。它们都具有像滤波器一样的小计算成本和像优化方法一样的相对足够的精度的优点。

（3）匹配的残差度量
匹配点云的残差度量也会影响激光雷达系统的效率。在自动驾驶数据集中，点对平面和点对线模型通常比点对点模型表现更好，因为激光雷达点云是稀疏的，并且同一点并不总是可观测的。此外，对于[27]、[28]等基于特征的系统，将首先将点分类为几个语义类（地板、杆、平面），以加快匹配过程。这些特征减少了注册点的数量，但在这样的系统中，额外的特征提取时间是不可避免的。

这篇论文提出了一种基于稀疏、增量体素的LIO算法，称为Faster-LIO，它基本上是从FastLIO2[18]发展而来的。在LIO中使用稀疏体素代替k-d树（及其变体）的想法受到以下方面的启发：

1）严格的k-NN搜索和范围搜索对于残差计算是不必要的，特别是对于LIO系统，其中IMU测量可以获得大致准确的初始猜测。k-d树的主要优点是它能够通过用超平面有条件地划分高维空间来提供严格的k-NN和范围/框搜索结果。然而，在最坏的情况下，搜索算法可能会深入到非常遥远的分支中，以寻找潜在存在的最近邻居，这不太可能对局部平面系数估计有用。相反，基于体素的算法中的搜索范围被限制为预设值，使得丢弃这样的邻居不会影响大多数残差。

2）k-d树节点的构建、迭代、平衡、移除会影响LIO的效率，而体素不存在这些问题。我们在体素中使用保守的插入和被动删除策略，而不是在每个扫描处理阶段强制更新。

我们的贡献可以总结如下：

• 我们提出了稀疏增量体素（iVox）来组织点云，而不是树状结构。我们证明，与LIO中常用的ik-d树和其他数据结构相比，iVox可以实现更高的增量更新和k-NN搜索速度。
• 我们在iVox中提出了两种可供选择的底层结构：线性iVox和iVox-PHC。实验表明，当我们在每个体素中有更多的点时，iVox-PHC具有更好的计算效率，而当数量较少时，线性iVox表现更好。
• 我们使用并行k-NN搜索来构建一个紧密耦合的LIO系统，固态激光雷达的速度超过1500Hz，旋转机械激光雷达数据的速度超过200Hz（见图1），算法已经在github上进行了开源。

二、相关工作

最近的几项研究集中在快速点云配准上，其中还融合了惯性测量以形成LIO系统。我们在这里简要回顾一下这些作品，并讨论一下我们之间的区别。

LiTAMIN[2]和LiTAMIN2[29]提出了一种通过减少配准点的数量并将对称KL散度引入传统ICP的快速配准方法。它们的灵感来自著名的无损检测方法，该方法首先将点划分为单独的体素，然后在每个体素中执行正态分布变换。这封信报告了使用64线旋转激光雷达的Kitti数据集中约500–1000 Hz的频率。速度令人印象深刻，但主要是通过减少相关点的数量而不是使用更紧凑的数据结构进行邻居搜索来实现的。此外，它们没有开源实现，报告的准确性略低于传统方法[30]。

FastLIO[18]和FastLIO2[26]是在大规模场景中实现近100Hz的LIO系统。增量k-d树显著减少了树的更新时间，这也在我们的实验中得到了验证。通过使用SMW等式来减小观测方程的维数，进一步改进了迭代EKF中的卡尔曼增益计算。尽管如此，我们表明iVox甚至比FastLIO2中的ik-d树更快，同时实现了相同的精度水平。

对于旋转激光雷达，也有低延迟方法（LoLa SLAM[31]、LLOL[32]），它们不等待完全扫描，而是使用部分扫描数据来执行配准。这种加速是通过将扫描分割成几个模式来实现的，并且配准点的数量也比处理完整扫描小得多。不幸的是，这种方法只适用于旋转激光雷达，很难扩展到固态激光雷达。

如果目标平台配备GPU，则深度学习注册方法[33]、[34]和GPU加速注册[30]是SLAM中广泛使用的选择。凭借GPU的并行计算能力，大多数SLAM过程，如特征检测、特征匹配，甚至整个地图，都可以存储在GPU内存中，并以非常高的速度加速。然而，由于基本基础设施不同