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 \neq 缺乏平面匹配。例如,在图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)的两个连续扫描。(c) 跨越扫描的大面形成了场景的基本骨架,并揭示了其整体几何结构。(d) kNN需要大型的局部地图,否则大多数点无法找到邻居来拟合平面。而我们使用基本骨架来表示场景以进行点匹配。搜索区域扩大到平面级别,不需要kNN搜索。

2. 符号和预备知识

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

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

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

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

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

3. 增量iPCA平面预拟合(最重要的部分,如何对平面完成拟合)

我们的方法的核心可以从三个方面描述:预拟合,迭代PCA和增量式处理。

  1. 预拟合:每个扫描的大面积表面形成了3D场景的基本骨架,并揭示了其全局几何特征。我们将它们称为骨架平面,它们是独立维护的,而不是由kNN惰性拟合。我们首先从提取的表面特征点[5]或原始的去畸变点云[3],[4]中获取输入。然后,我们采用迭代拟合-去除-拟合的方式提取$P_i$中的所有骨架平面。对于单个平面拟合,我们使用经典的RANSAC[18]和NAPSAC[23]。NAPSAC假设同一类中的样本彼此之间比类外的点更接近。因此,同一超球内的样本更有可能适合最佳候选模型,但是这又需要昂贵的kNN来保证。因此,在大多数情况下,简单地增加RANSAC迭代次数即可达到相同的精度。一旦拟合了一个骨架平面,所有点都根据它们到平面的距离被划分为严格的内点、半内点(第5节中分析)和离群点。所有半内点直接被丢弃,离群点再次被馈入这个过程。由于骨架跟踪对大面积平面很有效,因此当无法提取更多合格的平面或剩余点太少时,循环终止。

  2. 迭代PCA精化:将PCA应用于kNN可追溯到[8],其唯一目的是确定是否有超过3个点可以适合一个平面。对于PPF来说,它可能更有用。由于LiDAR采样,IMU噪声,偏差及其随机漫步的噪声,去畸变点云很嘈杂。即使在严格的真实平面(例如墙壁)中,点与相应平面之间仍存在非常重要的偏移量。另一方面,一些不严格的平面也可能具有低的局部曲率(例如不平坦的地面,小角度的弧线)。图3展示了这些情况下kNN退化的问题。kNN策略仅使用4∼5个最近邻居,这意味着拟合的平面由一个小的局部空间确定。当遇到大噪声或不严格的平面时,局部空间无法描述整体形态,从而误导了注册的优化目标(见图3左侧)。相反,预拟合方式可以利用所有平面点的空间信息来产生其全局骨架平面。特别是,基于当前拟合的平面,在其所有严格内点的协方差矩阵上执行特征分解,与最小特征值相关的特征向量对应于精化的平面,其可理解为由所有严格支持旧平面的点新投票。新的严格内点相应地计算,该过程循环3次[25]。我们在图3中得出结论,这种迭代精化带来的PPF比kNN更快的收敛。

图3. 当跟踪具有小局部曲率的非严格平面时,kNN退化的示意图。 kNN策略最小化点到局部平面的距离。然而,由4个最近邻居确定的局部空间无法反映整体几何形状。相反,iPCA方法迭代地提取所有全局平面点的主要骨架,导致更快的收敛。

  1. Incremental Fitting:相邻LiDAR扫描的几何结构并不会有太大变化,通常是相似的。受到这个属性的启发,骨架平面可以被增量更新,以便节省不必要的RANSAC迭代。我们首先通过迭代将两个连续的LiDAR扫描之间的IMU测量积分(离散时间,一阶保持):


其中,$k ∈ [i,j]$是连续的LiDAR采样瞬间$i$和$j$之间的IMU采样瞬间。$∆t$是IMU采样间隔。 $\hat{ω}_k$和$\hat{a}_k$是$B$系下时间$k$的原始IMU角速度和加速度测量值。 IMU陀螺仪偏差$b_ω$和加速度计偏差$b_a$被建模为随机游走[26],它们的导数是高斯白噪声。 $g$是$W$中的重力向量。有关方程(3-7)的详细推导,请参见[27]。
接下来,对于每个传入的LiDAR扫描$P_j$,每个当前骨架平面$f_{L_i}$都用于在$P_j$中获取相应的初始骨架猜测$f_{init}$:

$R^L_B$和$t^L_B$是LiDAR和IMU之间的外参。需要注意的是,IMU积分$∆T$的精度不需要非常高,因为它仅提供猜测。如果初始猜测通过了内点检查,则接受$f_{init}$。一旦被接受,$f_{init}$还将进行5次iPCA优化。处理完初始猜测后,开始朴素的RANSAC。我们强调,这种技术特别适用于具有高光束数的LiDAR,例如KITTI数据集中的Velodyne HDL-64E,拟合算法可以提高4倍。

4. 基于骨架平面的跟踪(对上面一节的扩充)

基于PPF,我们描述了有效的残差评估标准。在迭代拟合过程中,经过每次骨架平面的精细化,我们将其严格的内点放入一个集合中,表示为$Ω$。当PPF收敛时,剩余的点被认为是分散的。我们将包含分散点的集合表示为$Θ$。请注意,半内点被丢弃,因此我们有$Ω∪Θ⊆P$。对于$Ω$中的每个骨架点 $\tilde{p}$,在骨架平面空间$F$中搜索其对应物。$F$在$W$中实现为一个滑动窗口。残差被测量为点到最近平面的距离:


对于$Θ$中的 $\tilde{p}$,搜索仍然发生在点级别,即构建本地地图的kd树或iVox来搜索$k$个最近邻点,并且残差基于$\tilde{p}$和懒惰拟合的平面补丁$f_{knn}$ [3],[5]之间的距离:

接下来,扫描匹配算法通过求解最优变换来注册最新的LiDAR扫描。

然后,扫描匹配算法使用Levenberg-Marquardt [5],[28]或IEKF [3]求解最佳变换来注册最新的LiDAR扫描。然后,为准备下一个LiDAR扫描,我们将所有新配对的$f_{L_i+1}$转换为$W$,即$f_W = (T^{−1}_{i+1})> f_{L_i+1}$,并将$f_W$放入滑动窗口$F$。请注意,尽管$Ω$中的点数量超过$Θ$,但$Ω$的搜索空间(即$F$)比$Θ$的搜索空间小了约三个数量级。

5. 三明治层(Sandwich Layer,这部分则是对半内点这一概念的分析)

当进入新场景时,可能会在新拟合的骨架平面上没有在$F$中找到匹配平面。相反,由于平面相交,仅依靠点到平面距离可能存在误匹配。对于包含角度较小的平面,情况更为严重。例如,在图5a中的复杂地形中,由于坡度,地面高程缓慢上升。假设这是第一次拟合坡面的扫描,因此尚未将其放置在$F$中,因此斜坡上的点可以轻松通过(10)中的检查,因此错误地匹配地面平面。为了处理这个问题,我们引入了一个额外的半内点层(图5b)。在扫描匹配之前,我们简单地舍弃所有的半内点,这可能会导致潜在的误匹配。半内点层的厚度被设置为$ε$,这样离群值就几乎无法产生误匹配。另一方面,这也有助于防止嘈杂的“粗”平面点产生多个骨架平面——我们仅使用严格的中央平面点作为骨架。这种移除是如此直接的原因是这些模棱两可的点的比例很小,而它们的负面影响远大于它们对跟踪的贡献。

图5.(a)地面高程不同的场景。(b)(a)中的斜坡地面和PPF中使用的夹层层的示意图。根据到中心平面的距离,点被分为三组:严格内点、半内点和外点。

6. 参考链接

[https://mp.weixin.qq.com/s/LVRNeqT7cwloAEmHRfbGxQ](