激光SLAM总结——Fast LIO / Fast LIO2 / Faster LIO

激光SLAM总结——Fast LIO / Fast LIO2 / Faster LIO

在之前的工作中有接触过LOAM，最近在工作中又接触到Faster LIO相关的工作，于是想着对Fast LIO / Fast LIO2 / Faster LIO这一系列工作进行一个简单的总结，以加深自己对激光SLAM算法的理解，之前总结过的一些和激光SLAM算法相关的博客还有，感兴趣的同学可以一起学习讨论：
激光SLAM总结——VLOAM / LIMO算法解析
激光SLAM总结——LIO-Mapping / LIOM / LINS / LIO-SAM算法解析

1. Fast LIO

FAST LIO是2021年提出的一个激光和IMU紧耦合迭代卡尔曼滤波框架，论文的贡献主要是提供了一个新的卡尔曼增益计算公式，新公式不依赖于测量维度，仅依赖于状态维度，从而达到高效的计算

Fast LIO是基于LINS进行进一步优化，那么LINS有什么问题呢？LINS是紧耦合框架，点云构建的残差被直接写入了观测方程，点云残差构建如下： f i ( x b k + 1 b k ) = { ∣ ( p ^ i l k − p a l k ) × ( p ^ i l k − p b I k ) ∣ ∣ p a l k − p b l k ∣  if  p i l k + 1 ∈ F e ∣ ( p ^ i k − p a l ) T ( ( p a k − p b k ) × ( p a l − p c l ) ) ∣ ∣ ( p a l k − p b l k ) × ( p a l k − p c l c ) ∣  if  p i l k + 1 ∈ F p f_{i}\left(\mathbf{x}_{b_{k+1}}^{b_{k}}\right)=\left\{

$$\begin{array}{cl} \frac{\left|\left(\hat{\mathbf{p}}_{i}^{l_{k}}-\mathbf{p}_{a}^{l_{k}}\right) \times\left(\hat{\mathbf{p}}_{i}^{l_{k}}-\mathbf{p}_{b}^{I_{k}}\right)\right|}{\left|\mathbf{p}_{a}^{l_{k}}-\mathbf{p}_{b}^{l_{k}}\right|} & \text { if } \mathbf{p}_{i}^{l_{k+1}} \in \mathbb{F}_{e} \\ \frac{\left|\left(\hat{\mathbf{p}}_{i}^{k}-\mathbf{p}_{a}^{l}\right)^{T}\left(\left(\mathbf{p}_{a}^{k}-\mathbf{p}_{b}^{k}\right) \times\left(\mathbf{p}_{a}^{l}-\mathbf{p}_{c}^{l}\right)\right)\right|}{\left|\left(\mathbf{p}_{a}^{l_{k}}-\mathbf{p}_{b}^{l_{k}}\right) \times\left(\mathbf{p}_{a}^{l_{k}}-\mathbf{p}_{c}^{l_{c}}\right)\right|} & \text { if } \mathbf{p}_{i}^{l_{k+1}} \in \mathbb{F}_{p} \end{array}$$ \right. fi​(xbk+1​bk​​)=⎩ ⎨ ⎧​ ​palk​​−pblk​​ ​ ​(p^​ilk​​−palk​​)×(p^​ilk​​−pbIk​​) ​​ ​(palk​​−pblk​​)×(palk​​−pclc​​) ​ ​(p^​ik​−pal​)T((pak​−pbk​)×(pal​−pcl​)) ​​​ if pilk+1​​∈Fe​ if pilk+1​​∈Fp​​残差的维度和点的数量相关，假设残差的维度为$m$，在计算卡尔曼增益过程中会计算残差相对状态变量的线性化矩阵${\mathbf{H}}_k$，其维度为$m\times n$，其中$n$为状态变量维度，进而计算${\mathbf{H}}_k{\mathbf{P}}_k{\mathbf{H}}_k^T+\mathbf{V}$时维度将变为$m\times m$，在计算卡尔曼增益的过程中是需要对该矩阵进行求逆，可以预见当用于计算残差的特征点数量特别大时，计算卡尔曼增益也会变得相对耗时，在Fast-LIO就是通过SMW恒等式解决了该问题，使得计算卡尔曼增益不再依赖于观测维度，下面我们进一步看看具体是怎么做的

1.1 总体框架

Fast LIO的总体框架如下图所示：

大致流程是将激光雷达输入到特征提取模块，可获得平面和边缘特征，将提取后的特征和IMU测量值进行状态估计，输出$10HZ$到$50HZ$的状态估计结果，并根据状态估计结果将特征点注册到全局地图。

1.2 Iterated Kalman Filter

下面进行Fast LIO的公式推导：

假设IMU系为Body系，激光系到IMU系为刚体变换${}^I{\mathbf{T}}_L=\left({}^I{\mathbf{R}}_L,{}^I{\mathbf{p}}_L\right)$

IMU的连续运动学方程为： G p ˙ I = G v I , G v ˙ I = G R I ( a m − b a − n a ) + G g , G g ˙ = 0 G R ˙ I = G R I ∣ ω m − b ω − n ω ⌋ Λ , b ˙ ω = n b ω , b ˙ a = n b a

$$\begin{aligned}{ }^G \dot{\mathbf{p}}_I & ={ }^G \mathbf{v}_I,{ }^G \dot{\mathbf{v}}_I={ }^G \mathbf{R}_I\left(\mathbf{a}_m-\mathbf{b}_{\mathrm{a}}-\mathbf{n}_{\mathrm{a}}\right)+{ }^G \mathbf{g},{ }^G \dot{\mathrm{g}}=\mathbf{0} \\ { }^G \dot{\mathbf{R}}_I & \left.={ }^G \mathbf{R}_I \mid \boldsymbol{\omega}_m-\mathbf{b}_{\boldsymbol{\omega}}-\mathbf{n}_\omega\right\rfloor \Lambda, \dot{\mathbf{b}}_{\boldsymbol{\omega}}=\mathbf{n}_{\mathbf{b} \boldsymbol{\omega}}, \dot{\mathbf{b}}_{\mathbf{a}}=\mathbf{n}_{\mathbf{b a}}\end{aligned}$$ Gp˙​I​GR˙I​​=GvI​,Gv˙I​=GRI​(am​−ba​−na​)+Gg,Gg˙​=0=GRI​∣ωm​−bω​−nω​⌋Λ,b˙ω​=nbω​,b˙a​=nba​​其中${}^G{\mathbf{p}}_I$，${}^G{\mathbf{R}}_I$是IMU的位置和姿态，${\mathbf{a}}_m$，${\mathbf{\omega }}_m$为IMU的测量，${\mathbf{b}}_{\mathbf{a}}$，${\mathbf{b}}_{\omega }$为随机游走高斯噪声

IMU的离散运动学方程为$${\mathbf{x}}_{i+1}={\mathbf{x}}_i⊞\left(\Delta t\mathbf{f}\left({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i\right)\right)$$其中： x ≐ [ G R I T G p I T G v I T b ω T b a T G g T ] T ∈ M \mathbf{x} \doteq\left[

$$\begin{array}{llllll} { }^G \mathbf{R}_I^T & { }^G \mathbf{p}_I^T & { }^G \mathbf{v}_I^T & \mathbf{b}_{\boldsymbol{\omega}}^T & \mathbf{b}_{\mathbf{a}}^T & { }^G \mathbf{g}^T \end{array}$$ \right]^T \in \mathcal{M} x≐[GRIT​​GpIT​​GvIT​​bωT​​baT​​GgT​]T∈M$$\mathbf{u}\doteq {\left[\begin{array}{ll}{\mathbf{\omega }}_m^T& {\mathbf{a}}_m^T\end{array}\right]}^T$$$$\mathbf{w}\doteq {\left[\begin{array}{llll}{\mathbf{n}}_{\omega }^T& {\mathbf{n}}_{\mathrm{a}}^T& {\mathbf{n}}_{\mathrm{b}\mathbf{\omega }}^T& {\mathbf{n}}_{\mathrm{ba}}^T\end{array}\right]}^T$$$$\mathbf{f}\left({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i\right)=\left[\begin{array}{c}{\mathbf{\omega }}_{m_i}-{\mathbf{b}}_{{\omega }_i}-{\mathbf{n}}_{{\omega }_i}\\ {}^G{\mathbf{v}}_{I_i}\\ {}^G{\mathbf{R}}_{I_i}\left({\mathbf{a}}_{m_i}-{\mathbf{b}}_{{\mathbf{a}}_i}-{\mathbf{n}}_{{\mathbf{a}}_i}\right)+{}^G{\mathbf{g}}_i\\ {\mathbf{n}}_{\mathbf{b}{\omega }_i}\\ {\mathbf{n}}_{{\mathbf{ba}}_i}\\ 0_{3\times 1}\end{array}\right]$$

下面迭代扩展卡尔曼滤波进行状态估计，定义误差状态如下： x ~ k − 1 ≐ x k − 1 ⊟ x ‾ k − 1 = [ δ θ T G p ~ I T G v ~ I T b ~ ω T b ~ a T G g ~ T ] T \widetilde{\mathbf{x}}_{k-1} \doteq \mathbf{x}_{k-1} \boxminus \overline{\mathbf{x}}_{k-1}=\left[

$$\begin{array}{llllll} \delta \boldsymbol{\theta}^T & G \widetilde{\mathbf{p}}_I^T & G \widetilde{\mathbf{v}}_I^T & \tilde{\mathbf{b}}_{\boldsymbol{\omega}}^T & \tilde{\mathbf{b}}_{\mathbf{a}}^T & { }^G \widetilde{\mathbf{g}}^T \end{array}$$ \right]^T x k−1​≐xk−1​⊟xk−1​=[δθT​Gp ​IT​​Gv IT​​b~ωT​​b~aT​​Gg ​T​]T其中，${\mathbf{x}}_{k-1}$为真实状态，$\boxminus {\overline{\mathbf{x}}}_{k-1}$为标称状态，${\tilde{\mathbf{x}}}_{k-1}$为误差状态

前向传播流程如下：$${\hat{\mathbf{x}}}_{i+1}={\hat{\mathbf{x}}}_i⊞\left(\Delta t\mathbf{f}\left({\hat{\mathbf{x}}}_i,{\mathbf{u}}_i,0\right)\right);{\hat{\mathbf{x}}}_0={\overline{\mathbf{x}}}_{k-1}\text{. }$$其中上述计算的是标称状态的传播，计算过程中不考虑的噪声，状态的协方差通过误差状态进行传播，并吸纳这部分噪声，如下，误差状态定位如下：： x ~ i + 1 = x i + 1 ⊟ x ^ i + 1 = ( x i ⊞ Δ t f ( x i , u i , w i ) ) ⊟ ( x ^ i ⊞ Δ t f ( x ^ i , u i , 0 ) ) ≃ ( 23 ) F x ~ x ~ i + F w w i .

$$\begin{aligned} \widetilde{\mathbf{x}}_{i+1} & =\mathbf{x}_{i+1} \boxminus \widehat{\mathbf{x}}_{i+1} \\ & =\left(\mathbf{x}_i \boxplus \Delta t \mathbf{f}\left(\mathbf{x}_i, \mathbf{u}_i, \mathbf{w}_i\right)\right) \boxminus\left(\widehat{\mathbf{x}}_i \boxplus \Delta t \mathbf{f}\left(\widehat{\mathbf{x}}_i, \mathbf{u}_i, \mathbf{0}\right)\right) \\ & \stackrel{(23)}{\simeq} \mathbf{F}_{\tilde{\mathbf{x}}} \widetilde{\mathbf{x}}_i+\mathbf{F}_{\mathbf{w}} \mathbf{w}_i . \end{aligned}$$ x i+1​​=xi+1​⊟x i+1​=(xi​⊞Δtf(xi​,ui​,wi​))⊟(x i​⊞Δtf(x i​,ui​,0))≃(23)Fx~​x i​+Fw​wi​.​其中：$${\mathbf{F}}_{\overline{\mathbf{x}}}=\left[\begin{array}{cccccc}\mathrm{Exp}\left(-{\hat{\mathbf{\omega }}}_i\Delta t\right)& 0& 0& -\mathbf{A}{\left({\hat{\mathbf{\omega }}}_i\Delta t\right)}^T\Delta t& 0& 0\\ 0& \mathbf{I}& \mathbf{I}\Delta t& 0& 0& 0\\ -{}^G{\hat{\mathbf{R}}}_{I_i}{\lfloor {\hat{\mathbf{a}}}_i]}_{\wedge }\Delta t& 0& \mathbf{I}& 0& -{}^G{\hat{\mathbf{R}}}_{I_i}\Delta t& \mathbf{I}\Delta t\\ 0& 0& 0& \mathbf{I}& 0& 0\\ 0& 0& 0& 0& \mathbf{I}& 0\\ 0& 0& 0& 0& 0& \mathbf{I}\end{array}\right]$$$${\mathbf{F}}_{\mathbf{w}}=\left[\begin{array}{cccc}-\mathbf{A}{\left({\hat{\mathbf{\omega }}}_i\Delta t\right)}^T\Delta t& 0& 0& 0\\ 0& 0& 0& 0\\ 0& -{}^G{\hat{\mathbf{R}}}_{I_i}\Delta t& 0& 0\\ 0& 0& \mathbf{I}\Delta t& 0\\ 0& 0& 0& \mathbf{I}\Delta t\\ 0& 0& 0& 0\end{array}\right]$$那么协方差前向传播公式为：$${\hat{\mathbf{P}}}_{i+1}={\mathbf{F}}_{\overline{\mathbf{x}}}{\hat{\mathbf{P}}}_i{\mathbf{F}}_{\overline{\mathbf{x}}}^T+{\mathbf{F}}_{\mathbf{w}}\mathbf{Q}{\mathbf{F}}_{\mathbf{w}}^T;{\hat{\mathbf{P}}}_0={\overline{\mathbf{P}}}_{k-1}$$在完成IMU前向传播后，一方面会根据前向传播的结果对激光点云去运动畸变，另一方面会使用激光点云计算残差构建观测方程。

Fast LIO中使用的是点到面的残差，残差${\mathbf{z}}_j^{\kappa }$计算如下：$${}^G{\hat{\mathbf{p}}}_{f_j}^{\kappa }={}^G{\hat{\mathbf{T}}}_{I_k}^{\kappa }{}^I{\mathbf{T}}_L{}^{L_k}{\mathbf{p}}_{f_j};j=1,\cdots ,m.$$$${\mathbf{z}}_j^{\kappa }={\mathbf{G}}_j\left({}^G{\hat{\mathbf{p}}}_{f_j}^{\kappa }-{}^G{\mathbf{q}}_j\right)$$其中$\kappa$为迭代卡尔曼滤波中的迭代次数。为了将残差${\mathbf{z}}_j^{\kappa }$直接加入滤波过程，因此需要对残差进行线性化，线性化过程如下，如果将点云测量过程中的噪声移除的话，测量方程如下：$$0={\mathbf{h}}_j\left({\mathbf{x}}_k{}^{L_j}{\mathbf{n}}_{f_j}\right)={\mathbf{G}}_j\left({}^G{\mathbf{T}}_{I_k}{}^{I_k}{\check{\mathbf{T}}}_{I_j}{}^I{\mathbf{T}}_L\left({}^{L_j}{\mathbf{p}}_{f_j}-{}^{L_j}{\mathbf{n}}_{f_j}\right)-{}^G{\mathbf{q}}_j\right)$$进行一阶近似后： 0 = h j ( x k , L j n f j ) ≃ h j ( x ^ k κ , 0 ) + H j κ x ~ k κ + v j = z j κ + H j κ x ~ k κ + v j

$$\begin{aligned} \mathbf{0} & =\mathbf{h}_j\left(\mathbf{x}_k,{ }^{L_j} \mathbf{n}_{f_j}\right) \simeq \mathbf{h}_j\left(\widehat{\mathbf{x}}_k^\kappa, \mathbf{0}\right)+\mathbf{H}_j^\kappa \widetilde{\mathbf{x}}_k^\kappa+\mathbf{v}_j \\ & =\mathbf{z}_j^\kappa+\mathbf{H}_j^\kappa \widetilde{\mathbf{x}}_k^\kappa+\mathbf{v}_j \end{aligned}$$ 0​=hj​(xk​,Lj​nfj​​)≃hj​(x kκ​,0)+Hjκ​x kκ​+vj​=zjκ​+Hjκ​x kκ​+vj​​其中${\mathbf{x}}_k={\hat{\mathbf{x}}}_k^{\kappa }⊞{\tilde{\mathbf{x}}}_k^{\kappa }$，${\mathbf{H}}_j^{\kappa }$是${\mathbf{h}}_j\left({\hat{\mathbf{x}}}_k^{\kappa }⊞{\tilde{\mathbf{x}}}_k^{\kappa },{}^{L_j}{\mathbf{n}}_{f_j}\right)$相对误差状态${\tilde{\mathbf{x}}}_k^{\kappa }$的雅可比，卡尔曼增益计算如下：$$\mathbf{K}=\mathbf{P}{\mathbf{H}}^T{\left(\mathbf{HP}{\mathbf{H}}^T+\mathbf{R}\right)}^{-1}$$其中$\mathbf{H}={\left[{\mathbf{H}}_1^{{\kappa }^T},\cdots ,{\mathbf{H}}_m^{{\kappa }^T}\right]}^T$，$\mathbf{R}=\mathrm{diag}\left({\mathbf{R}}_1,\cdots {\mathbf{R}}_m\right)$，${\mathbf{z}}_k^{\kappa }={\left[{\mathbf{z}}_1^{{\kappa }^T},\cdots ,{\mathbf{z}}_m^{{\kappa }^T}\right]}^T$，正如前面提到的$\mathbf{HP}{\mathbf{H}}^T+\mathbf{R}$的维度和观测维度相关，本文提出了一个新的卡尔曼增益计算公式：$$\mathbf{K}={\left({\mathbf{H}}^T{\mathbf{R}}^{-1}\mathbf{H}+{\mathbf{P}}^{-1}\right)}^{-1}{\mathbf{H}}^T{\mathbf{R}}^{-1}$$在这个公式中只需要进行两次求逆，但是求逆的维度都是状态变量的维度，状态变量的维度远小于测量维度，因此计算量也会小很多。公式的具体推导流程如下，根据SMW恒等式有：$${\left({\mathbf{P}}^{-1}+{\mathbf{H}}^T{\mathbf{R}}^{-1}\mathbf{H}\right)}^{-1}=\mathbf{P}-{\mathbf{PH}}^T{\left({\mathbf{HP}}^T+\mathbf{R}\right)}^{-1}\mathbf{HP}$$因此我们有：$$\begin{array}{rl}\mathbf{K}& ={\left({\mathbf{H}}^T{\mathbf{R}}^{-1}\mathbf{H}+{\mathbf{P}}^{-1}\right)}^{-1}{\mathbf{H}}^T{\mathbf{R}}^{-1}\\ & ={\mathbf{PH}}^T{\mathbf{R}}^{-1}-\mathbf{P}{\mathbf{H}}^T{\left(\mathbf{HP}{\mathbf{H}}^T+\mathbf{R}\right)}^{-1}\mathbf{HP}{\mathbf{H}}^T{\mathbf{R}}^{-1}\end{array}$$注意到$$\mathbf{HP}{\mathbf{H}}^T{\mathbf{R}}^{-1}=\left(\mathbf{HP}{\mathbf{H}}^T+\mathbf{R}\right){\mathbf{R}}^{-1}-\mathbf{I}.$$因此我们可以获得：$$\begin{array}{rl}\mathbf{K}& =\mathbf{P}{\mathbf{H}}^T{\mathbf{R}}^{-1}-\mathbf{P}{\mathbf{H}}^T{\mathbf{R}}^{-1}+\mathbf{P}{\mathbf{H}}^T{\left({\mathbf{HP}}^T+\mathbf{R}\right)}^{-1}\\ & ={\mathbf{PH}}^T{\left({\mathbf{HP}}^T+\mathbf{R}\right)}^{-1}.\end{array}$$计算完卡尔曼增益后，进一步我们就可以获得更新后的误差状态和协方差：$${\hat{\mathbf{x}}}_k^{\kappa +1}={\hat{\mathbf{x}}}_k^{\kappa }⊞\left(-{\mathbf{Kz}}_k^{\kappa }-(\mathbf{I}-\mathbf{KH}){\left({\mathbf{J}}^{\kappa }\right)}^{-1}\left({\hat{\mathbf{x}}}_k^{\kappa }\boxminus {\hat{\mathbf{x}}}_k\right)\right)$$当迭代收敛后标称状态更新为：$${\overline{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{\kappa +1},{\overline{\mathbf{P}}}_k=(\mathbf{I}-\mathbf{KH})\mathbf{P}$$以上就完成了Fast LIO的推导过程。

1.3 实验结果

作者在论文中比较了Fast LIO和LINS的效果，在使用相同数据集的前提下而LINS平均每帧激光提取特征点147个需要34.5ms，Fast LIO平均每帧激光提取特征点784个，用7.3ms的平均处理速度并取得了更高的精度。

2. Fast LIO2

Fast LIO2发表于2021年，是在Fast LIO上进一步优化的一篇工作，其主要贡献是：（1）直接将原始点注册到地图而不提取特征，这样有助于提供整个系统的准确性；（2）通过增量k-d树结构ikd-Tree维护映射，该结构支持增量插入、删除和动态重新平衡，与现有的动态数据结构相比有更优越的整体性能

2.1 总体框架

Fast-LIO2的整体框架如下：

其中State Estimation部分和Fast-LIO是基本一致的，下面我们主要总结下区别较大的Mapping部分

2.2 ikd-Tree

2.2.1 Map Management

1. 为了保证地图大小是可控的，只有在距离当前帧一定范围内的局部地图才会被保留，如下图所示：
   
   在图（a）中，Body系位于$P_0$位置，初始化维护地图范围长度为$L$，即蓝色方框内的区域；在图（b）中，当Body系移动到$P^{\prime }$时，维护地图范围将从蓝色方框的区域移动到绿色方框的区域。其中$r=\gamma R$，$R$为激光的FOV范围，$\gamma$为大于1的参与，$d=(\gamma -1)R$。

2.2.2 Tree Structure and Construction

1. ikd-Tree是一个二分搜索树，和当前很多k-d树只在leaf nodes存储点的结构不同，ikd-Tree会在leaf nodes和internal nodes都对点进行存储；
2. 在ikd-Tree中一个point只和一个node关联，point相关的属性存储在node上；
3. ikd-Tree的数据结构如下图所示：
   
   其中axis为用来进行空间划分的轴，treesize用于记录用当前节点为根节点的子节点的数量，invalidnum用于记录子节点被删除的节点的数量，如果点从地图中被删除，那么对应的节点的deleted标志位将被置为true，如果所有的子节点都已经被删除，那么treedeleted标注位将被置为true，range用于记录所有子节点对应的点覆盖的范围，具体来说记录的是一个包括所有子节点对应的点的矩形的最大最小值坐标。
4. ikd-Tree的构建过程和普通kd-Tree的构建过程类似：循环依序取数据点的各维度来作为切分维度，取数据点在该维度的中值作为切分超平面，将中值左侧的数据点挂在其左子树，将中值右侧的数据点挂在其右子树。 递归处理其子树，直至所有数据点挂载完毕

2.2.3 Increment Updates

1. Incremental Updates分为Point-wise操作和Box-wise操作，Point-wise指的是对一个点进行Insert、Delete或者Re-insert操作，Box-wise是对一个Box内的所有点进行Insert、Delete或者Re-insert操作

2. Point Insertion with On-tree Downsampling，对于Point插入操作算法如下：
   
   Point插入过程中会伴随着On-tree Downsample的操作，假定Downsample的分辨率为$l$，根据分辨率$l$可以将空间划分为若干个的立方体${\mathbf{C}}_D$，新插入点$\mathbf{p}$会计算属于哪个${\mathbf{C}}_D$，算法计算离${\mathbf{C}}_D$中心${\mathbf{p}}_{\text{center }}$最近的点${\mathbf{P}}_{\text{nearest}}$，然后将${\mathbf{C}}_D$里已经存在的点删除，然后将${\mathbf{P}}_{\text{nearest}}$重新插入ikd-Tree。插入操作则是在ikd-Tree中递归地找到一个空节点，对于递归过程中涉及到的每一个非空节点会更新其节点属性。

3. Box-wise Delete using Lazy Labels，在删除过程中，节点并不会立即被删除，而是delete标志位和treedeleted标志位做延后删除，判断节点是否要删除的流程如下：
   
   假定要删除的范围是${\mathbf{C}}_O$，根据节点的range属性获得的范围是${\mathbf{C}}_T$，如果${\mathbf{C}}_O$和${\mathbf{C}}_T$不存在交集，则直接返回，如果${\mathbf{C}}_O$完全包含${\mathbf{C}}_T$，则将${\mathbf{C}}_T$包含的所有节点全部删除，如果${\mathbf{C}}_O$包含部分${\mathbf{C}}_T$递归到子节点做进一步判断。

2.2.4 Re-balancing

1. Balancing Criterion，ikd-Tree的子树在满足如下两个条件时会激活平衡过程
   $\alpha$-balanced criterion：$$S(\text{ T.leftchild })<{\alpha }_{\text{bal }}(S(T)-1)$$$$S(T.rightchild)<{\alpha }_{\text{bal }}(S(T)-1)$$$\alpha$-deleted criterion：$$I(T)<{\alpha }_{del}S(T)$$其中${\alpha }_{bal}\in (0.5,1)$，${\alpha }_{\text{del }}\in (0,1)$，$S(T)$指的是$T$为根节点子树的节点个数，$I(T)$指的是以$T$为根节点子树的无效节点的个数。前者是为了维持整颗树的最大高度，后者则是及时将无效节点移除以保证其他增量操作的高效。
2. Rebuild & Parallel Rebuild，子树的重建过程如下图所示：
   第一步是子树进行展平，在展平的过程中将无效节点进行移除，第二步对展平后的节点进行重建。如果子树的规模过大，重建过程可能会导致延迟，为了避免这个问题，作者提出重建算法如下：
   
   算法会先判断需要重建的规模，如果规模小于某个阈值是直接在主线程中进行重建，如果规模过大，则新建一个线程进行重建，在新线程的重建过程中，会对Insert操作和Delete操作进行上锁，这些操作在主线程中进行缓存，在新线程重建完成后，再将这些操作在新线程复现一遍，因此新线程中重建的子树除了更加平衡，其他信息和主线程中的子树是一致的。

2.2.5 K-Nearest Neighbor Search

1. 在ikd-Tree的数据结构中记录了每个子树的覆盖的范围，因为在ikd-Tree进行K-Nearest Neighbor Search过程中会建立一个priority queue，其中记录在搜索过程中到目标点的距离。在K-Nearest Neighbor Search过程中，遍历到每一个节点，会优先计算以这个节点为根节点的子树覆盖的范围到目标点的距离，如果距离大于priority queue中的最大值，那么则停止对这个子树的搜索，通过这种方式提高搜索效率。

2.3 实验结果

2.3.1 时间复杂度

1. 对${\mathbf{C}}_D=L_x\times L_y\times L_z$范围的Box-wise Delete和Search时间复杂度：$$O(H(n))=\{\begin{array}{ll}O(\log n)& \text{ if }{\Delta }_{\min }⩾\alpha \left(\frac{2}{3}\right)(\ast )\\ O\left(n^{1-a-b-c}\right)& \text{ if }{\Delta }_{\max }⩽1-\alpha \left(\frac{1}{3}\right)(\ast \ast )\\ O\left(n^{\alpha \left(\frac{1}{3}\right)-{\Delta }_{\min }-{\Delta }_{\text{med }}}\right)& \text{ if }(\ast )\text{ and }(\ast \ast )\text{ fail and }\\ & {\Delta }_{\mathrm{med}}<\alpha \left(\frac{1}{3}\right)-\alpha \left(\frac{2}{3}\right)\\ O\left(n^{\alpha \left(\frac{2}{3}\right)-{\Delta }_{\min }}\right)& \text{ otherwise. }\end{array}$$其中$a={\log }_n\frac{S_x}{L_x}$，$b={\log }_n\frac{S_y}{L_y}$，$c={\log }_n\frac{S_z}{L_z}$，${\Delta }_{\min }$、${\Delta }_{\text{med }}$、${\Delta }_{\max }$为$a$、$b$、$c$中最小、中间和最大值
2. Point Insertion with On-tree Downsampling时间复杂度为$O(\log n)$
3. 单线程re-building时间复杂度为$O(n\log n)$，双线程re-building时间复杂度为$O(n)$
4. K-Nearest Neighbor Search时间复杂度为$O(\log n)$

2.3.2 效率对比

1. ikd-Tree效率对比如下：
   
2. Fast LIO2和其他SOTA方法效率对比如下：
   
3. Fast LIO和Fast LIO2的效率对比如下：
   
   可以看出来，随着ikd-Tree的加入，Fast-LIO2的效率相对于Fast LIO会提高不少，以上就是对Fast LIO2的总结

3. Faster LIO

Faster LIO是高博团队发表于2022年的一篇论文，其主要贡献是是基于Fast LIO2将ikd-Tree的数据结构更新为iVox，进一步提高了算法效率，在知乎上有一篇高博对这个工作的介绍，感兴趣的同学可以参考Faster-LIO：快速激光IMU里程计，本篇只做一个简单总结

3.1 iVox

如下图是iVox的数据结构定义：

iVov的底层是一个空间哈希映射：$$\mathbf{p}={\left[p_x,p_y,p_z\right]}^T$$$$\mathbf{v}=\frac{1}{s}{\left[p_x,p_y,p_z\right]}^T$$$$i{d}_v=\mathrm{hash}(v)=\left(v_x{n}_x\right)\text{ xor }\left(v_y{n}_y\right)\text{ xor }\left(v_z{n}_z\right)\mathrm{mod}N$$其中$p_x,p_y,p_z$是空间点坐标，$s$是栅格大小，$n_x,n_y,n_z$是大质数，因为质数不与其他数字存在公约数，可以减少因取模操作而产生的周期性模式，从而避免哈希冲突，$N$是哈希表的大小。

在存储上，这些Voxel以Vector的数据结构进行存储或者使用PHC的数据结构进行存储，根据存储结构的不同分别称为Linear iVox或者iVox-PHC。

在查找K近邻时，先计算被查找的点落在哪个体素中，然后看预定义的范围内是否存在有效的iVox。如果有，就把这些iVox内部的点也考虑进来。我们在每个iVox内部查找至多K个最近邻，再进行合并即可。线性的iVox只须遍历内部点，然后进行部分排序，再进行汇总。PHC的iVox则可以根据空间填充曲线上的索引值来查找最近邻。，使用这种方式进行最近邻搜索其效率更高，精度会有所下降，但是对于点云匹配来讲这种精度已经足够。

上述提到的PHC是一种建立高维数据与低维数据映射的方法，在查找最近邻时可以先把它看成这个iVox里的曲线上的ID，然后相关计算公式计算该ID周边的若干个近邻，如下图所示：
在论文中有补充到，由于Faster LIO会限制填入每个栅格的点的数量，因此PHC带来的收益并不是特别明显

3.2 增量地图更新

在Fast LIO2中使用LUR缓存进行局部地图管理，在进行近邻搜索时，记录哪些体素是最近使用过的，那么不怎么使用的体素自然被移动到队尾。我们会设置一个局部地图的容量，超过最大容量时，就删除那些很久未使用的体素。删除操作是针对整个体素的，内部的点会被全部删除。这种局部地图缓存策略也会让局部地图跟随车辆运动，而实际操作的地方更少。如下图所示：

3.3 实验结果

如下是不同数据结构插入和最近邻搜索的耗时对比：

如下图是耗时和召回的关系曲线：

最后是Faster LIO整个系统耗时和精度对比：


可以看到，在相同的精度下Faster LIO确实取得了更快的速度，以上是对Faster LIO算法的简单总结