【Fast-LIO系列】Fast-LIO、Fast-LIO2、Faster-LIO系列特点分析

1. FAST-LIO核心贡献

【FAST-LIO】FAST-LIO论文详解

1.将IMU和Lidar特征点紧耦合在一起

2.使用反向传播考虑到了运动补偿

3. 基于IESKF中的 卡尔曼增益更新 K

将IEKF的卡尔曼滤波 $P{H}^T(HP{H}^T+R{)}^{-1}$ 转为 $(H^T{R}^{-1}H+P^{-1}{)}^{-1}{H}^T{R}^{-1}$ ，这样做的好处是原本公式中求逆中的矩阵是观测维度的，新公式求逆中的矩阵是状态维度的，需要求逆H的矩阵维度减小了(因为在实际情况中，激光特征点的数量维度要远大于状态量的维度)

详细的证明见blog【FAST-LIO】FAST-LIO论文详解

详细的推导过程：
根据矩阵逆引理[27]，我们可以得到：

${\left(P^{-1}+H^T{R}^{-1}H\right)}^{-1}=P-P{H}^T{\left(HP{H}^T+R\right)}^{-1}HP$

将上述代入公式(20)，我们可以得到：

$$\begin{gathered} K={\left(H^T{R}^{-1}H+P^{-1}\right)}^{-1}{H}^T{R}^{-1} \\ =P{H}^T{R}^{-1}-P{H}^T{\left(HP{H}^T+R\right)}^{-1}HP{H}^T{R}^{-1} \end{gathered}$$

现在注意 $HP{H}^T{R}^{-1}=\left(HP{H}^T+R\right)R^{-1}-I$。将其代入上述，我们可以得到标准卡尔曼增益公式(18)，如下所示。

$$\begin{gathered} K=P{H}^T{R}^{-1}-P{H}^T{R}^{-1}+P{H}^T{\left(HP{H}^T+R\right)}^{-1} \\ =P{H}^T{\left(HP{H}^T+R\right)}^{-1}. \end{gathered}$$

在FAST-LIO算法中，通过应用矩阵逆引理（Matrix Inverse Lemma），将原本需要在观测维度上进行求逆的矩阵转换为在状态维度上进行求逆。具体到上述公式中的求逆矩阵维度变化如下：

1. 原始卡尔曼增益公式：
   $K=(H^T{R}^{-1}H+P^{-1}{)}^{-1}{H}^T{R}^{-1}$
   这里，$H^T{R}^{-1}H$ 的维度是状态维度乘以状态维度，而 $P^{-1}$ 的维度同样是状态维度乘以状态维度。

2. 应用矩阵逆引理后：
   ${\left(P^{-1}+H^T{R}^{-1}H\right)}^{-1}=P-P{H}^T{\left(HP{H}^T+R\right)}^{-1}HP$
   在这个公式中，$HP{H}^T+R$ 的维度是观测维度乘以观测维度，${\left(HP{H}^T+R\right)}^{-1}$ 需要在观测维度上求逆。

3. 维度变化：

   • 在原始公式中，需要在观测维度上求逆的矩阵是 $H^T{R}^{-1}H$，其维度通常远大于状态维度。
   • 在应用矩阵逆引理后，需要求逆的矩阵变为 $HP{H}^T+R$，其维度是观测维度乘以观测维度，但通过巧妙的代数操作，实际上避免了直接在这个大矩阵上进行求逆。

4. 最终的卡尔曼增益：
   $K=P{H}^T{\left(HP{H}^T+R\right)}^{-1}$
   这里，尽管看起来仍然需要在观测维度上求逆，但实际上通过前面的步骤，已经将求逆的负担从观测维度转移到了状态维度，大大减少了计算量。

在FAST-LIO中，这种转换使得算法能够更高效地处理大量的观测数据（如激光雷达点云），同时保持了状态估计的准确性。通过这种方式，算法能够处理更高频率的数据更新，提高了SLAM系统的整体性能。

本文的IEKF的更新频率是基于雷达的采样频率k，一帧雷达点云估计出一个后验状态。其中状态方程是IMU离散传播。观测方程是scan-submap匹配的，特征用点和面特征。

每帧状态量会做反向传播利用 $\Delta T$ 传播到特征点时间戳下的状态 $T_j$ (一帧雷达点云包含若干个IMU状态，两个IMU状态之间包含若干个特征时间戳，在计算时考虑到了不同的IMU状态)，增量式更新是SLAM研究里非常核心的话题。然后将特征点经过外参 ${}_L^T$ T进行运动补偿，并将得到的点转到全局坐标系下，并与submap进行scan-submap匹配，计算点线，点面的残差，并将观测方程通过变换后的IEKF进行迭代估计。得到增量式距离 $x_k^{k+1}$ ，直到每次迭代的增量都小于阈值，以代表收敛。

2. FAST-LIO2核心贡献(2021年)

paper: FAST-LIO2: Fast Direct LiDAR-inertial Odometry

对于FAST-LIO2而言，相较于FAST-LIO，做了如下改进：

1. 不用线，面特征点而使用全局点云

2. 使用ikd-tree存储点云

文章中提到的第一点，通过原始点云与地图的配准Q，可以有效地利用环境中的细微特征，从而提高准确性，同时不使用特征提取也可以更好地适应不同的激光雷达。另一块就是设计了增量k-d树数据结构ikd-Tree，以高效地表示大型稠密点云地图，除了高效的最近邻搜索外，新的数据结构还支持增量地图更新（即点插入、下采样、点删除）和以最小计算成本进行动态平衡。同样ikd-Tree支持下采样，在不影响精度的同时能够相较于八叉树，R-树，nanoflann,k-d树等传统的方法获得更快的计算精度。得益于ikd-Tree，Fast-LIO2不再是类似LOAM般的提取edge特征与plane特征，而是直接将每个三维点与地图配准。因此，其能够较稳定地运行在一些较难提取手工特征的场景中。此外FAST-LIO2的状态估计是从FAST-LIO继承的紧耦合迭代卡尔曼滤波器(IEKF)，FAST-LIO2的流程如下图所示，顺序采样的激光雷达原始点首先在10ms（用于100Hz更新）和100ms（用于10Hz更新）之间的时间段内累积。累积的点云称为扫描数据，为了执行状态估计，新扫描中的点云通过紧耦合迭代卡尔曼滤波框架配准到大型局部地图中维护的地图点（即里程计），大型局部地图中的全局地图点由增量k-d树结构ikd树组织。

3. ikd-Tree

众所周知,kd树类结构的优势在于可以严格地查询K近邻,也可以以范围或盒子形式来查询最近邻(range search/box search),查询过程中可以设置最大距离等限制条件,实现快速的近似最近邻查找(Aproximate Nearest Neighbor, ANN)。而ikd-Tree在point-wise和block-wise，通过对结点新加了deleted, treedeleted, pushdown，treesize, invalidnum属性，进而减小了插入，删除，检索，re-insert的时间复杂度，并达到增量更新的目的；并且能够通过设置的参数，检测到二叉树不平衡时，进行重建。

3. Faster-LIO 核心贡献

Faster-LIO: Lightweight Tightly Coupled Lidar-Inertial Odometry Using Parallel Sparse Incremental Voxels

FASTER-LIO作为FAST-LIO2的升级版，通过一些优化措施进一步提升了处理速度。该算法不再采用复杂的基于树的数据结构来组织空间点云，而是采用了一种名为增量体素(iVox)的点云空间数据结构，这种结构是从传统的体素演变而来，支持点云的增量插入和并行近似k-NN查询。以下是文章中提到的一些改进要点：

1. 使用增量体素(iVox)存储点云：在存储点云时，采用被动删除LRU（最近最少使用）策略来删除体素。在检索过程中，会考虑体素内的点云。在每个体素内部，根据点云的密度，选择使用线性方法或PHC（一种点云处理算法）进行处理，当点云数量较多时，倾向于使用PHC。

2. 与FAST-LIO2相比的实验结果：精度方面两者相当，但FASTER-LIO通过牺牲一小部分配准精度来减少处理时间。在大多数数据集上，处理时间减少了约一半。

LIO系统的计算流程通常包括：从IMU获取初步估计，将雷达数据与历史数据进行配准，最后使用某种状态估计算法进行滤波或优化。由于IMU处理部分的差异不大，因此LIO系统的计算效率主要取决于点云处理算法和后端算法。

点云最近邻的数据结构对于点云配准至关重要。点云配准的核心任务是计算给定点与历史点云的最近邻，通常需要依赖某些高效的最近邻数据结构。这些数据结构主要分为基于树的（treelike）和基于体素的（voxellike）。对于LIO系统而言，最近邻问题是一个低维、增量式的问题。作者认为，增量式的体素结构更适合LIO系统。

FASTER-LIO以FAST-LIO2为基础进行改进。实验表明，FAST-LIO2的主要耗时在于IEKF+ICP的迭代过程。当使用iVox替代iKd-tree后，这一过程的耗时显著减少。在UTBM数据集上，IEKF迭代的时间从大约20ms降低到了5-8ms，这同时也意味着ICP搜索的效率得到了提升。