文章：LIO-PPF: Fast LiDAR-Inertial Odometry via Incremental Plane Pre-Fitting and Skeleton Tracking
作者：Xingyu Chen, Peixi Wu, Ge Li and Thomas H. Li
编辑：点云PCL
代码：https://github.com/xingyuuchen/LIO-PPF想·
欢迎各位加入知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。
公众号致力于点云处理，SLAM，三维视觉，高精地图等领域相关内容的干货分享，欢迎各位加入，有兴趣的可联系dianyunpcl@163.com。未经作者允许请勿转载，欢迎各位同学积极分享和交流。
摘要
作为智能移动机器人的重要基础设施，激光雷达惯性里程计(LiDAR-Inertial odometry，LIO)通过跟踪激光雷达扫描数据提供了状态估计的基本能力，高精度的跟踪通常涉及最小化点到平面距离的k最近邻搜索，然而，这样做的代价是维护大规模的局部地图，并且为每个点执行k最近邻平面拟合。在这项工作中通过节省这些不必要的开销来降低LIO的时间和空间复杂性，在技术上设计了一个平面预拟合(PPF)流程来跟踪3D场景的基本骨架。在PPF中，平面不是针对每个扫描帧单独拟合的，更不用说对每个点进行拟合了，而是在场景“流动”时进行增量更新，与k最近邻不同，PPF对噪声和非严格平面更具有鲁棒性，主要采用了迭代主成分分析(iPCA)进行优化。此外，我们引入了一个简单而有效的sandwich layer来消除虚假的点对平面匹配，我们的方法在5个公开数据集中的22个序列上进行了广泛测试，并在3个现有的最先进的LIO系统中进行了评估，与此相比，LIO-PPF只需消耗原始局部地图大小的36%即可实现高达4倍的更快残差计算和1.92倍的整体帧率，同时保持相同水平的准确性。我们完全开源了我们的代码，地址为https://github.com/xingyuuchen/LIO-PPF。
图1. (a) 重建的场景，(b) 在快速旋转下的两个连续扫描，(c) 跨越扫描的大型平面形成场景的基本骨架，并揭示其整体几何结构，(d) kNN需要大量本地地图，否则大多数点无法找到邻居以拟合平面，而我们使用基本骨架来表示场景进行点匹配，搜索区域被扩大到平面级别，无需进行kNN搜索。
主要贡献
总结来说，本研究做出了以下贡献：
• 我们提出通过平面预拟合(PPF)跨越LiDAR扫描来表示场景的基本骨架，用于点匹配。我们展示了：
(i) PPF能够自然地减小本地地图的规模，并消除大部分冗余的kNN搜索和平面拟合；
(ii) kNN对噪声和非严格平面的非鲁棒性。
• 在PPF中，我们不是为每个点/扫描拟合平面，而是利用IMU和LiDAR扫描的顺序性实现增量式平面更新；迭代PCA使得PPF对噪声和非严格平面比kNN更具鲁棒性；我们引入了一个简单而有效的夹层层，用于排除错误的点到平面匹配。
• 工程上的贡献包括完全开源的LIO-PPF。在5个开放数据集中的22个序列上进行的实验显示，与原始方法相比，我们的PPF将本地地图大小最多减少了64%，计算残差速度提高了4倍，整体帧率提高了最多1.92倍，并且仍然保持了相同水平的精度。
主要内容
增量式平面预拟合与追踪
这里将首先介绍我们高效的平面预拟合方法，然后描述基于PPF的相应骨架追踪算法，最后提出一个多层来在复杂场景中提高算法的鲁棒性。
图2. 平面预拟合和跟踪流程概览
增量式iPCA平面预拟合
该方法的核心可以从三个方面描述：预拟合、迭代PCA和增量式。
1）预拟合：每个扫描帧中的大面积表面形成了3D场景的基本骨架，并揭示了其全局几何特征，我们称之为骨架平面，并且独立地维护它们而不是通过kNN进行拟合，首先从提取的表面特征点或原始的去畸变点云中获取输入。然后采用迭代的拟合-去除-拟合方式在Pi中提取所有的骨架平面。对于单个平面拟合，我们使用经典的RANSAC和NAPSAC，NAPSAC假设同一类中的样本比类外的点更接近彼此。
2) 迭代PCA优化：用户PCA确定是否有3个以上的点能够拟合一个平面，对于PPF，它可以更有用，由于LiDAR采样、IMU噪声、偏差及其随机游走的噪声，去畸变点云是有噪声的。即使在严格的实际平面（例如墙壁）中，点与相应平面之间仍然存在显著的偏移。另一方面，一些不是严格平面的平坦表面也可能具有较低的局部曲率（例如不平坦的地面，小角度的弧线）。图3展示了kNN在这些情况下的退化问题。kNN策略仅使用4∼5个最近邻点，意味着拟合的平面由一个较小的局部空间确定。当遇到大噪声或非严格的平面时，局部空间无法表征整体形式，从而误导了配准的优化目标（见图3左）
图3. 描述了在追踪带有较小局部曲率的非严格平面时，kNN退化的情况，kNN策略最小化点到局部平面的距离，然而，由4个最近邻点确定的局部空间无法反映整体几何形态，相反，iPCA方法迭代地提取所有全局平面点的主要骨架，导致更快的收敛。
3）增量拟合：相邻的LiDAR扫描的几何结构并不会有很大的变化，而是通常相似的，受到这一特性的启发，我们可以增量更新骨架平面，从而节省不必要的RANSAC迭代。
基于骨架的跟踪
一种基于“Plane Pre-Fitting”（PPF）的轨迹跟踪方法。首先通过PPF方法来提取3D场景中的基本骨架，即由大的平面构成的全局几何结构。每个大平面都经过迭代的拟合和优化得到。在拟合的过程中，所有符合条件的点被分为三类：严格的内点、半内点和外点。严格的内点是完全符合当前平面的点，半内点是部分符合的点，而外点则是不符合当前平面的点。接下来，当PPF收敛后，剩余的点被认为是散乱的点，没有与任何大平面匹配。这些散乱的点被放入一个集合中，记为Θ。在这里，“Skeleton-based Tracking”指的是通过基本骨架来跟踪并匹配后续的LiDAR扫描数据。在跟踪过程中，对于大平面内的严格内点p~，可以在平面空间F中进行搜索它们的对应点。而对于散乱点Θ中的点p~，搜索仍然发生在点级别，即在局部地图中搜索k个最近邻点，然后计算这些散乱点p~与这些邻近点构成的懒惰拟合平面块fknn之间的距离。这样，通过将点集Ω和Θ分别与对应的平面空间F和局部地图中的邻近点关联，就可以实现高效的残差评估和跟踪过程。
夹层处理
当进入新的场景时，可能在平面空间F中没有与新拟合的基本骨架平面上的点匹配，而且，由于平面交叉，如果仅依赖点到平面的距离，可能会出现错误匹配。这在包含小夹角的平面上更为严重。例如，如图5a所示的复杂地形，地面高程由于斜坡缓慢上升，假设这是第一次拟合斜坡平面，因此它尚未放入平面空间F中，斜坡上的点可以轻易通过（10）中的检查，从而错误地匹配到地面平面。
图5.（a）一个包含不同高程地面的场景（b）（a）中斜坡地面的示意图和PPF中使用的夹层处理层，根据到中心平面的距离，点被分为三组：严格内点、半内点和外点。
为了处理这个问题引入了一个额外的半内点处理层（图5b），在进行扫描匹配之前，我们简单地丢弃所有的半内点，这可能导致潜在的错误匹配。半内点层的厚度被设置为，以使异常点几乎不可能产生错误匹配。另一方面，这也有助于防止嘈杂的“厚”平面点产生多个基本骨架平面-我们仅使用严格符合中心平面的点作为基本骨架。这种移除方法非常简单，因为这些模糊的点的比例很小，它们的负面影响远大于它们对跟踪的贡献。
实验
数据集和实验设置
所有实验均在一台配置现代的台式计算机上进行，该计算机运行Ubuntu 18.04 LTS操作系统，使用一颗英特尔Core i7-9700K处理器（3.60GHz × 8核心）和16 GiB内存，没有使用GPU，我们采用OpenMP 库进行并行计算，为了与之前的方法进行公平比较，在所有实验中启动与待比较方法相同数量的线程进行并行计算，我们首先进行了对比实验，以分析PPF配准策略和基于kNN的配准策略。然后，我们在真实场景中广泛评估了基于PPF的LIO的精度和效率。我们在5个公开数据集上进行了测试，包括UTBM robocar数据集、KITTI 、LIOSAM 、ULHK 和NCLT，共计22个序列。
图6. 模拟实验设置，我们生成具有不同标准差的高斯白噪声的平面，使用Levenberg-Marquardt算法，采用不同的点匹配策略进行配准。
PPF与kNN的比较研究
验证了图3中得出的结论：PPF收敛所需的迭代次数比kNN少，并且每次迭代所需的时间至少少一个数量级。使用带有高斯白噪声的平面进行实验，这些噪声在现实世界中可能来自LiDAR采样、带有噪声IMU测量的点云去畸变等。为了模拟机器人的自我运动，我们设定两个平面之间的距离，目标是通过配准这两个扫描，使得同一物体的不同扫描能够在地图中重叠，结果如图7所示。
图7. 模拟实验结果，(c) 左侧柱状图：kNN；右侧柱状图：PPF，所有实验在单一计算机上运行。
图8 使用LIOSAM-3序列绘制麻省理工学院校园地图的结果
效率评估
我们通过分析每个步骤的运行时间来评估LIO系统的效率，为了研究PPF对LIO系统的效益，我们首先重新开发了LIO-SAM，并比较了使用kNN和PPF的效率。我们在表格I中列出了每个步骤的详细时间成本。
准确性评估
准确性通过绝对姿态误差（APE）和相对姿态误差（RPE）的均方根误差（RMSE）指标来衡量，在我们的实验中由evo计算，我们首先将我们的算法与LIO-SAM的准确性进行比较，如表II所示。
表III中比较了4个系统在9个序列中的准确性。由于LIO-SAM需要9轴IMU，而这在UTBM数据集中不可用，因此没有显示结果。
图9. 位于密歇根大学北校区的大规模场景的估计轨迹和重建地图，与谷歌地图对齐。
图10中比较了估计的轨迹可视化结果，也显示了使用PPF的轨迹在大多数序列中保持相同的准确性（有时甚至更加准确，例如KITTI-7序列）。
图10. 估计轨迹和GT（Ground Truth）轨迹，从左上角到右下角依次是：NCLT-1，NCLT-2，LIOSAM-5，KITTI-7，LIOSAM-1和UTBM-5。
总结
本文研究了激光雷达-惯性里程计系统的计算，并揭示了大部分kNN搜索、平面拟合和大型局部地图的冗余性，为了实现更高效的激光雷达扫描跟踪，我们设计了一个平面预拟合流程来实现基本的骨架跟踪。展示了kNN对噪声和非严格平面的不稳健性，并引入了iPCA来处理这些问题。此外，利用惯性测量和激光雷达扫描帧的时序性，骨架平面可以逐步更新，进一步加快了PPF的速度。对于平面包含小角度的复杂场景，引入了一个简单而有效的夹层层来消除虚假的点到平面匹配，在5个开放数据集中的22个序列上进行的实验表明，相比之下，我们的骨架跟踪只消耗原始局部地图大小的最低36％，在残差计算方面提高了4倍，并在总体帧率上提高了1.92倍，同时保持了相同的精度水平，为了为SLAM社区做出贡献发布了完整C++实现。
资源
自动驾驶及定位相关分享
【点云论文速读】基于激光雷达的里程计及3D点云地图中的定位方法
自动驾驶中基于激光雷达的车辆道路和人行道实时检测（代码开源）
更多文章可查看：点云学习历史文章大汇总
SLAM及AR相关分享
结构化PLP-SLAM：单目、RGB-D和双目相机使用点线面的高效稀疏建图与定位方案
以上内容如有错误请留言评论，欢迎指正交流。如有侵权，请联系删除
扫描二维码
关注我们
让我们一起分享一起学习吧！期待有想法，乐于分享的小伙伴加入知识星球注入爱分享的新鲜活力。分享的主题包含但不限于三维视觉，点云，高精地图，自动驾驶，以及机器人等相关的领域。
分享与合作方式：微信“cloudpoint9527”（备注：姓名+学校/公司+研究方向） 联系邮箱：dianyunpcl@163.com。
为分享的伙伴们点赞吧！
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
