0. 简介

继LLOAM后，三维SLAM迎来了蓬勃的发展，最近一只FAST-SLAM为代表的3D-SLAM迎来了蓬勃的发展，FASTER-LIO也可以看到国内知名SLAMer高博的影子。为此我们来看一下FAST-SLAM这类SLAM算法的优势在哪里。

1. FAST-LIO

首先我们需要先明白FSAT-LIO相较于其他的LIO好处在哪里：

1. 将IMU和Lidar特征点紧耦合在一起
2. 使用反向传播考虑到了运动补偿
3. 将IEKF的卡尔曼滤波 $PH^ {T} (HPH^ {T}+R)^ {-1}$转为 $(H^ {T}R^ {-1}H+P^ {-1})^ {-1} H^ {T} R^ {-1}$ ，这样做的好处是原本公式中求逆中的矩阵是观测维度的，新公式求逆中的矩阵是状态维度的，需要求逆 $H$的矩阵维度减小了（因为在实际情况中，激光特征点的数量维度要远大于状态量的维度）

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

2. 流形的定义

原文中如图这样定义：

下面就是流形的计算公式，其中 $\mathbb{R}$代表了域， $Exp$代表了罗德里格斯公式： $Exp(r)=I+ \frac {r}{||r||} \sin (||r||)+ \frac {r^ {2}}{||r||^2} (1- \cos(||r||))$， $Log$是将李群转为李代数数据

$\boxplus: M \times M; \boxminus:M \times M \rightarrow R^ {n}$
$M=SO(3): R\boxplus r=RExp(r); R_ {1} \boxminus R_ {2} =Log( R_ {2}^ {T} R_ {1} )$
$M= \mathbb{R}^ {n}； a\boxplus b=a+b;a\boxminus b=a-b$

在矩阵中的运算如下图

$$\left[ \begin{matrix} R\\a \end{matrix} \right] \boxplus \left[ \begin{matrix} r\\b \end{matrix} \right] = \left[ \begin{matrix} R\boxplus r \\a + b \end{matrix} \right] ; \left[ \begin{matrix} R_1\\a \end{matrix} \right] \boxminus\left[ \begin{matrix} R_2\\b \end{matrix} \right] = \left[ \begin{matrix} R_1\boxminus R_2 \\a - b \end{matrix} \right]$$

根据上面的定义，很容易验证：
$(x\boxplus u) \boxminus x = u; x\boxplus (y \boxminus x) = y; \forall x,y \in M, \forall u \in \mathbb{R}^ {n}$

3. FAST-LIO2

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

1. 不用线，面特征点而使用全局点云
2. 使用ikd-tree存储点云

文章中提到的第一点，通过原始点云与地图的配准，可以有效地利用环境中的细微特征，从而提高准确性，同时不使用特征提取也可以更好地适应不同的激光雷达。另一块就是设计了增量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树组织。

4. 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的时间复杂度，并达到增量更新的目的；并且能够通过设置的参数，检测到二叉树不平衡时，进行重建。

与静态tree进行对比，增量更新时间复杂度降低，查询复杂度烧逊色于静态tree，如下图所示：

5. FASTER-LIO

FASTER-LIO作为FAST-LIO2的续作，通过一些处理将速率进一步提升，文中不使用复杂的基于树的结构来划分空间点云，而使用增量体素（iVox）作为我们的点云空间数据结构，它是从传统体素修改而来的，支持增量插入和并行近似k-NN查询。下面是一些文中的改进点：

…详情请参照古月居