FAST-LIO2：快速直接的激光雷达与惯导里程计
点云PCL免费知识星球，点云论文速读。
文章：FAST-LIO2: Fast Direct LiDAR-inertial Odometry
作者: Wei Xu∗1 , Yixi Cai∗1 , Dongjiao He1 , Jiarong Lin1 , Fu Zhang
编译：点云PCL
代码：https://github.com/hku-mars/FAST_LIO
来源：arxiv2021
文章仅做学术分享，如有侵权联系删文。未经博主同意请勿擅自转载。
摘要
本文介绍了FAST-LIO2：一种快速、鲁棒的、通用的激光雷达惯性里程计框架，FASTLIO2以高效紧耦合的方式迭代卡尔曼滤波器为基础，具有两个关键的新颖之处，可实现快速、稳健和精确的激光雷达的建图和导航。
第一该方法是直接将原始点云配准到地图（随后更新地图），而不提取特征，这样可以利用环境中的细微特征，从而提高准确性，这里不采用人工设计的特征提取模块，使其自然适应不同扫描模式的新兴激光雷达；
第二个主要创新点是通过增量k-d树数据结构kd-tree来维护地图，该结构支持增量更新（即点插入、删除）和动态平衡。与现有的动态数据结构（八叉树，R-树，nanoflann k-d树），kd树实现了卓越的整体性能，同时自然支持下采样，我们对来自各种开放式激光雷达数据集的19个序列进行了详尽的基准比较。FAST-LIO2比其他最先进的激光雷达惯性导航系统低得多的计算量实现了持续更高的精度，在小视场的固态激光雷达上也进行了各种真实世环境的实验，总体而言，FAST-LIO2计算效率高，鲁棒性强，多功能，同时仍能实现比现有方法更高的精度，FAST-LIO2和数据结构kd树的实现都是在Github上开源了。
https://github.com/hku-mars/FAST_LIO
主要贡献
在这项工作中，贡献如下：
1）开发了一种增量k-d树数据结构ikd树，以高效地表示大型稠密点云地图，除了高效的最近邻搜索外，新的数据结构还支持增量地图更新（即点插入、下采样、点删除）和以最小计算成本进行动态平衡。这些功能使ikd树非常适合于激光雷达里程计和地图应用，从而在计算训练平台上实现100 Hz里程计和建图，ikdTree数据结构工具箱在Github上是开源的（ https://github.com/hku-mars/ikd-Tree ）
2） 由于ikd树计算效率的提高，我们直接将原始点云配准到地图上，这样即使在剧烈运动和非常混乱的环境中，也能实现更精确和可靠的点云配准，我们将这种基于原始点的配准称为直接方法，消除了人工设计的特征提取，使系统自然适用于不同的激光雷达传感器；
3） 将这两项关键技术集成到我们最近开发的紧耦合激光雷达惯性里程计系统FAST-LIO中，系统使用IMU通过严格的反向传播步骤补偿每个点云的运动，并通过流形迭代卡尔曼滤波器估计系统的完整状态，为了进一步加快计算速度，使用了一个新的、数学上等价的卡尔曼增益计算公式，将计算复杂度降低到状态的维度，新系统被称为FAST-LIO2，并在Github开源以造福社区；
4） 作者进行了各种实验来评估开发的ikd树、直接点云配准和整个系统的有效性，在18个不同大小的序列上的实验表明，ikdTree相对于现有的动态数据结构在激光雷达里程计和建图中的应用，对来自各种开放式激光雷达数据集的19个序列进行了详尽的基准比较，结果表明FAST-LIO2以比其他最先进的激光雷达惯性导航系统低得多的计算量实现了更高的精度，最后，文章展示了FAST-LIO2对新兴固态激光雷达收集的具有挑战性的现实世界数据的有效性，这些数据的FoV非常小，包括剧烈运动和无结构环境中。
主要内容
图1 系统概述
FAST-LIO2的状态估计是从FAST-LIO继承的紧耦合迭代卡尔曼滤波器，FAST-LIO2的流程如图1所示，顺序采样的激光雷达原始点首先在10ms（用于100Hz更新）和100ms（用于10Hz更新）之间的时间段内累积。累积的点云称为扫描数据，为了执行状态估计，新扫描中的点云通过紧耦合迭代卡尔曼滤波框架配准到大型局部地图中维护的地图点（即里程计），大型局部地图中的全局地图点由增量k-d树结构ikd树组织。如果当前激光雷达的视场范围穿过地图边界，则距离激光雷达姿态最远的地图区域中的历史点将从ikd树中删除，因此，ikd树以一定长度（本文中称为“地图大小”）跟踪大立方体区域中的所有地图点，并用于在状态估计模块中计算残差。优化姿势最终将新扫描中的点云配准到全局帧，并通过以里程计的速率插入ikd树，将其合并到地图中。(更详细的模块介绍请查看https://t.zsxq.com/z7YBqzj)
开源方案涉及到的依赖项
ikd树：用于3D kNN搜索的最先进的动态KD树。
IKFOM：用于快速高精度流形卡尔曼滤波器的工具箱。
UAV Avoiding Dynamic Obstacles：FAST-LIO在机器人规划中的实现之一。
R2LIVE：使用FAST-LIO作为激光雷达惯性前端的高精度激光雷达惯性视觉融合工作。
UGV Demo：微分流形上轨迹跟踪的模型预测控制。
FAST-LIO-SLAM：FAST-LIO与点云上下文闭环检测模块的集成。
FAST-LIO-LOCALIZATION：FAST-LIO与重定位功能模块的集成。
实验
A 数据结构评估
增量更新、kNN搜索和总时间的每次扫描点云平均消耗时间的比较
不同树大小的数据结构比较结果
B 在数据集中的精度评估
具有高质量真值的序列中的绝对平移误差（RMSE，单位为米）
C 大规模环境中建图评估
每帧扫描点云基准的平均处理时间（毫秒）
大规模场景下的建图效果
处理激光雷达点云时单个模块的平均时间消耗（毫秒）
总结
本文提出了FAST-LIO2，这是一个直接而鲁棒的LIO框架，大大优于目前最先进的LIO算法，同时在各种数据集中实现了高效率或更好的精度，速度的提高是由于删除了特征提取模块和高效建图，开发并验证了一种新的增量k-d树（ikd-tree）数据结构，该结构支持动态点插入、删除和并行建图，在开放数据集上的大量实验表明，在激光雷达里程计kNN搜索的最新数据结构中，所提出的ikd树可以获得最佳的整体性能。作为建图效率的结果，在快速运动和稀疏场景中，通过在里程计中使用更多的点，精度和鲁棒性也得到了提高。FAST-LIO2的另一个好处是由于去除了特征提取，可适用于不同的激光雷达。