【LOAM系列】八：Fast-Lio2论文笔记

Fast-Lio2

2022 8 IEEE Transactions on Robotics
Wei Xu; Yixi Cai; Dongjiao He; Jiarong Lin; Fu Zhang 港大火星实验室

论文

FAST-LIO2: Fast Direct LiDAR-Inertial Odometry

1.1 系统框架

紧耦合 迭代误差卡尔曼滤波，不提取特征（ 人工设计的特征对于不雷达的适配性能不同，也容易受环境影响），使用ikd-tree支持增量更新，雷达数据的一次scan可以是10ms~100ms之间，因此定位频率可以修改

1.2 LIO

与Fast-Lio一样，基于误差状态的迭代卡尔曼滤波，当时增加了外参的估计
KaTeX parse error: Got group of unknown type: 'internal'

运动模型

对应着IMU预积分与协方差矩阵传播
$$\begin{array}{c}\begin{array}{rl}{\mathbf{x}}_{i+1}& ={\mathbf{x}}_i⊞\left(\Delta t\mathbf{f}({\mathbf{x}}_i,{\mathbf{u}}_i,{\mathbf{w}}_i)\right)\end{array}\end{array}\text{\hspace{1em}(4)}$$

KaTeX parse error: Got group of unknown type: 'internal'

测量模型

运动补偿每个点投影到扫描结束时刻，在投影到全局地图与最近邻点拟合的平面计算残差(这里残差等于0是因为雷达系系的点减去了噪声的影响，因此就是真实值)
$$\begin{array}{c}\begin{array}{r}0={}^G{\mathbf{u}}_j^T\left({}^G{\mathbf{T}}_{I_k}{}^I{\mathbf{T}}_L\left({}^L{\mathbf{p}}_j+{}^L{\mathbf{n}}_j\right)-{}^G{\mathbf{q}}_j\right)\end{array}\end{array}\text{\hspace{1em}(8)}$$
对噪声（误差状态）的观测方程
$$\begin{array}{c}\begin{array}{r}0={\mathbf{h}}_j\left({\mathbf{x}}_k,{}^L{\mathbf{n}}_j\right)\triangleq {}^G{\mathbf{u}}_j^T\left({}^G{\mathbf{T}}_{I_k}{}^I{\mathbf{T}}_L({}^L{\mathbf{p}}_j+{}^L{\mathbf{n}}_j)-{}^G{\mathbf{q}}_j\right)\end{array}\end{array}\text{\hspace{1em}(9)}$$
IMU预积分的中状态传播，误差卡尔曼滤波
$$\begin{array}{c}\begin{array}{rl}{\hat{\mathbf{x}}}_{i+1}& ={\hat{\mathbf{x}}}_i⊞\left(\Delta t\mathbf{f}({\hat{\mathbf{x}}}_i,{\mathbf{u}}_i,0)\right);{\hat{\mathbf{x}}}_0={\bar{\mathbf{x}}}_{k-1}\\ {\hat{\mathbf{P}}}_{i+1}& ={\mathbf{F}}_{{\tilde{\mathbf{x}}}_i}{\hat{\mathbf{P}}}_i{\mathbf{F}}_{{\tilde{\mathbf{x}}}_i}^T+{\mathbf{F}}_{{\mathbf{w}}_i}{\mathbf{Q}}_i{\mathbf{F}}_{{\mathbf{w}}_i}^T;{\hat{\mathbf{P}}}_0={\bar{\mathbf{P}}}_{k-1}\end{array}\end{array}\text{\hspace{1em}(10)}$$

H矩阵：

迭代卡尔曼滤波

在迭代过程中还更新了协方差，有一些文论里面没有更新协方差，$A({)}^{-T}$就是右雅克比方程u就是R误差状态的增量的旋转向量
$$\begin{array}{c}\begin{array}{rl}{\mathbf{x}}_k\boxminus {\hat{\mathbf{x}}}_k& =\left({\hat{\mathbf{x}}}_k^{\kappa }⊞{\tilde{\mathbf{x}}}_k^{\kappa }\right)\boxminus {\hat{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{\kappa }\boxminus {\hat{\mathbf{x}}}_k+{\mathbf{J}}^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }\\ & \sim \mathcal{N}(0,{\hat{\mathbf{P}}}_k)\end{array}\end{array}\text{\hspace{1em}(14)}$$

计算卡尔曼增益，更新状态量
$$\begin{array}{c}\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}\\ {\hat{\mathbf{x}}}_k^{\kappa +1}& ={\hat{\mathbf{x}}}_k^{\kappa }⊞\left(-\mathbf{K}{\mathbf{z}}_k^{\kappa }-(\mathbf{I}-\mathbf{KH})({\mathbf{J}}^{\kappa }{)}^{-1}\left({\hat{\mathbf{x}}}_k^{\kappa }\boxminus {\hat{\mathbf{x}}}_k\right)\right).\end{array}\end{array}\text{\hspace{1em}(18)}$$
迭代到误差增量足够小时停止
$$\begin{array}{c}\begin{array}{rl}{\bar{\mathbf{x}}}_k& ={\hat{\mathbf{x}}}_k^{\kappa +1},{\bar{\mathbf{P}}}_k=\left(\mathbf{I}-\mathbf{KH}\right)\mathbf{P}.\end{array}\end{array}\text{\hspace{1em}(19)}$$
利用优化完的位姿将此帧的雷达点投影到世界坐标系下并插入ikd-tree中

1.3 ikd-tree

https://zhuanlan.zhihu.com/p/529926254

https://github.com/KennyWGH/ikd-Tree-detailed

增量构建的kd-tree数据结构（ikd-Tree），可以有效地表示一个很大的点云。除了最近邻搜索外，还支持增量地图更新（即点插入、树上降采样、点删除）和以最小的计算成本进行动态重新平衡。由于ikd-Tree的计算效率有所提高，直接将原始点匹配到地图上(不需要降采样），即使在剧烈的运动和非常杂乱的环境中，也能实现更准确可靠的扫描匹配。

直接配准原始点云的方法与广义ICP(G-ICP)非常相似，G-ICP假设环境是平滑的，因此可以视为一个平面，一个点被配准到地图上的一个小的局部平面上。广义icp的计算负荷通常是较大，基于正态分布变换(NDT)也配准了原始点，但NDT比ICP的稳定性较低，在某些场景中可能存在发散不收敛情况。

全局地图的维护

如果当前激光雷达的FOV越过地图边界，则从ikd-Tree中删除距离LiDAR姿态最远的历史点云(一旦越过边界获得更大地图范围触发ikd-tree重构最远点)。ikd-Tree跟踪一个具有一定长度（地图大小）的大立方体区域中的所有地图点（滑窗）并用于计算状态估计模块中的残差。优化后的姿态最终将新scan中的点配准到全局帧中，并通过以里程计的频率插入到ikd-Tree中，将它们合并到全局地图中。

新地图区域和旧地图区域之间的减法区域中的所有点将从ikd-Tree中删除。

ikd-tree的数据结构

一颗二叉搜索树，节点包含了点的属性(包括坐标，强度)+左右子节点的指针+当前节点所有(孙)子节点的个数+布尔变量表示该节点是否被删除+一个子树构成的长方体的两个顶点坐标

struct TreeNode{
    PointType point;  //当前点的坐标强度等信息
    TreeNode* leftchild, rightchild;//左右子节点的指针
    int axis; //分割空间的轴
    int treesize, invalidnum;//该节点所有子节点的总数，被删除的子节点总数
    bool deleted, treedeleted;//该节点是否删除，以该节点为根节点的树是否删除，tree表示删除
    CuboidVertices range;//所有子节点所在的长方体，存储了两个顶点的坐标
}

ikd-Tree 中充分利用了数据结构部分提到的 range信息 来进行剪枝加速，也即在每个节点处，除了计算节点本身与查询点的距离之外，也会分别判断左右两个子树的range 是否与目标解空间有重叠，只有有重叠的子树才会被继续递归搜索，没重叠的子树将直接被剪枝掉，实现搜索加速。

构造树

构建 ikd-Tree 类似于构建静态 k-dree。ikd-Tree 沿最长维度递归地分割中点处的空间，直到子空间中只有一个点。 Data Structure 中的属性在构建过程中被初始化，包括计算树的大小和（子）树的范围信息。

树的平衡判断

实现动态再平衡的前提条件是，首先要知道哪些 (sub)tree 是不平衡的。因此，ikd-Tree 提出了一个准则来随时判断任何一个 (sub)tree 平衡与否。这个准则包含两个小准则，如下：

a-balanced 准则描述的是我们最大能够容忍一个subtree的左右两侧有多不平衡，否则就会被执行重构；0.5是最平衡的
a-deleted描述的是最多能够容忍一个subtree中“被标记为删除、但是还没真正删除的点“的比例，否则就会执行重构。
当且仅当两项准则都符合时，才会认为一个 (sub)tree 是平衡的。

树的再平衡算法

双线并行重建，重建ikd树的时间复杂度为O(n )，对于单线程重建，时间复杂度为O(n log n)。

当ikd-Tree通过平衡性判据发现某一个(sub)tree不平衡时，就会触发对该(sub)tree的重建，以实现再平衡。这里又分为两种情况，如果该(sub)tree规模很小，重建时间花销可以忽略，就在主线程中直接重建，重建时先把(sub)tree的所有节点打乱重排，在这一过程中会拿掉标记删除的点（Flatten）；然后，对剩下的点按中值点法执行常规的kdtree构建过程；构建完成后，把新的子树替换到原来的位置，完成。

如果需要重建的(sub)tree规模很大，重建时间花销不可忽略；如果在主线程中执行重建，就会导致ikd-Tree一直被占用，外部的SLAM/LIO算法无法访问ikd-Tree执行查询或增删操作 —— 导致算法阻塞。
针对这种情况，ikd-Tree 采取了一种双线程策略，耗时的重建过程在额外的线程中执行，主线程仍旧对外提供查询和增删服务，其中涉及到“正在重建的(sub)tree”的增删操作会被额外缓存到一个叫作OperationLogger的容器中；待额外线程中完成了重建后，会从OperationLogger把增删操作“补”到新的(sub)tree上，这样就确保了新的(sub)tree没有漏掉任何过程；最后，把新子树替换到相应位置，完成。

树的增量更新

持两种类型的增量操作： 逐点操作和按框操作。 逐点操作在 kd-tree中插入、删除或重新替换单个点，而按框操作在给定的轴对齐长方体中插入、删除或替换所有点。在这两种情况下，点插入都与降采样相集成，从而将地图保持在预定的分辨率。

点插入与降采样 时间复杂度Olog(n)

首先将整个空间体素化，并明确新点落入哪个体素（目标体素）,也就是将待插入点划分为空间中的一个长方体内；
然后向ikd-Tree查询目标体素在哪一个长方体内，且该长方体是否已经有点以及有哪些点（查询过程参考box-wise delete）；
如果已经有点了，将已有点和新点一起排序，找出离体素中心最近的那个点，然后做判断：如果最近的点是已有点，意味着新点无必要再插入了，结束处理；如果最近的点是新点，则把已有点全部标记删除，并插入新点；
如果体素内尚不存在点，则直接插入新点。

ikd-Tree删除节点

在SLAM中，我们希望能够按照空间区域批量地删除点 —— 对应着 map跟随pose移动而移动。
在ikd-Tree中，这个操作叫作 box-wise delete，输入一个立方体区域给ikd-Tree，ikd-Tree删除这个区域内的所有点。*
box-wise delete 结合了range信息和 lazy delete 策略，以下图为例：红/绿/蓝色的框代表不同节点的range范围，灰色框代表我们期望删除的区域。
做删除时，从根节点开始，在每个节点处首先判断节点自身是否在灰色区域内，若是则标记删除；然后，判断两个子节点的range是否与删除区域有交叉，若无则直接剪枝，若有则重复上述过程；直至搜索完所有的子节点，完成所有被删除节点的标记删除。真正删除节点是需要等到这可树不平衡了重构时候才删除的

最近邻搜索的时间复杂度为Olog(n)