经典文献阅读之--LIO-PPF(增量平面预拟合LIO)

0. 简介

自从ikd-tree出来后，现在越来越多的工作瞄准了增量式这种方法，比如说激光惯导里程计（LIDAR-Inertial Odometry，LIO）的高精度跟踪通常涉及最小化点到平面距离的k最近邻（kNN）搜索，然而，这样做的成本是维护大型局部地图并为每个点执行kNN平面拟合。在《LIO-PPF: Fast LiDAR-Inertial Odometry via Incremental Plane Pre-Fitting and Skeleton Tracking》中，我们通过节省这些不必要的成本来减少LIO的时间和空间复杂度，在技术上设计了一个平面预拟合（PPF）方法来跟踪3D场景的基本框架。在PPF中，平面不是为每个扫描单独拟合的，更不用说为每个点拟合了，而是随着场景“流动”而逐步更新，与kNN不同，PPF对噪声和非严格平面更具有鲁棒性，而且我们采用迭代主成分分析（iPCA）进行优化。此外，还引入了一个简单但有效的夹层，以消除假的点对平面匹配，并且完全开源了代码。

1. 文章贡献

本文中，与在搜索每个点的kNN之后懒惰地拟合平面相比，我们采用了一种更积极的平面预拟合方法。这个想法源于两个不必要的因素：
1）不必要的kNN：当前的LiDAR扫描跟踪[1]-[6]为每个单独的点搜索kNN以拟合平面，假设空间接近的点来自同一平面。然而，这个条件太严格了，即空间上远离的点也可能属于同一个平面，例如长墙。换句话说，kNN策略忽略了平面不总是在小的局部区域内这一事实。对于大平面来说，大多数kNN搜索是冗余的，因为它们最终拟合相同的平面。同时，来自连续LiDAR扫描的点通常来自同一个大平面的不同部分，即它们共享同一个高级别大平面对象。这给了我们一个提示：如果搜索发生在高级特征平面空间中，那么kNN的昂贵成本自然可以被节省。

2）不必要的大型局部地图：如果只保留几个扫描在局部地图中，NN可能会很远，因此很可能属于其他对象，导致kNN不准确地拟合匹配的平面。因此，扩大局部地图以保证足够接近的点对kNN对应关系[5]-[7]，如图1d所示。但是，我们注意到：即使只保留一个扫描在局部地图中，缺乏接近的$NN\ne$ 缺乏平面匹配。例如，在图1b中，大多数橙色墙体点未能在长墙上找到接近的NN，但是这面墙的平面可以轻松地从一个单独的先前扫描中的点（蓝点）预拟合。再次，这给了我们一个提示：如果搜索发生在高级特征平面空间中，一个单独的先前扫描就足以提供足够的点对平面匹配。总之，我们做出以下贡献：

• 我们建议通过LiDAR扫描之间的平面预拟合（PPF）来表示场景的基本骨架进行点匹配。我们证明了（i）PPF可以自然地缩小局部地图并消除大多数冗余的kNN搜索和平面拟合；（ii）kNN对噪声和非严格平面的不鲁棒性。
• 在PPF中，不是为每个点/扫描拟合平面，而是利用IMU和LiDAR扫描的顺序性来实现增量平面更新；迭代PCA使PPF比kNN更具有噪声和非严格平面的鲁棒性；引入了一个简单而有效的夹层来排除错误的点对平面匹配。
• 工程贡献包括完全开源的LIO-PPF。在5个开放数据集的22个序列上进行的实验表明，与原始方法相比，我们的PPF最多可以将本地地图大小减小64％，在残差计算方面实现4倍的加速，最多实现1.92倍的总体FPS，并仍然显示相同水平的精度。

图1. (a) 重建的场景。(b) 在快速旋转下(a)的两个连续扫描。© 跨越扫描的大面形成了场景的基本骨架，并揭示了其整体几何结构。(d) kNN需要大型的局部地图，否则大多数点无法找到邻居来拟合平面。而我们使用基本骨架来表示场景以进行点匹配。搜索区域扩大到平面级别，不需要kNN搜索。

2. 符号和预备知识

在本文中，我们将世界坐标系表示为$W$，LiDAR坐标系表示为$L$，IMU体坐标系表示为$B$。令$P_i$表示从扫描i接收到的去畸变的LiDAR点。一个3D平面可以由其法向量$n\in {\mathbb{R}}^3$和其中任意一点$p\in {\mathbb{R}}^3$来确定，或者更简洁地通过一个4维向量$f=[n^T,d{]}^T$来确定，使得$f\cdot \tilde{p}=0$，其中$\tilde{p}=[p^T,1{]}^T$。我们用变换矩阵$T\in SE(3)$来表示6自由度姿态，其中包含旋转矩阵$R\in SO(3)$和平移向量$t\in {\mathbb{R}}^3$。
定理1：$f$经过$T$的变换为：

证明：设 $\tilde{p}$ 是 $f$ 上的任意一点，且 ${\tilde{p}}^{\prime }$ 是其通过 $T$ 变换后得到的对应点，即${\tilde{p}}^{\prime }=T\tilde{p}$。那么我们有：

这意味着${\tilde{p}}^{\prime }$在$f^{\prime }$上。

下面我们将会介绍本文高效平面预拟合方法。然后，我们描述基于PPF的对应骨架跟踪算法。最后，提出了一个三明治层，以使算法在复杂场景中更加鲁棒。

图2. 预拟合平面和跟踪流程的概览。浅箭头 → 表示为准备下一个激光雷达扫描而进行的操作。

3. 增量iPCA平面预拟合（最重要的部分，如何对平面完成拟合）

…详情请参照古月居