史上最快，仅需10ms的动态点云剔除方法

论文题目：

A Fast Dynamic Point Detection Method for LiDAR-Inertial Odometry in Driving Scenarios

论文作者：

Zikang Yuan, Xiaoxiang Wang, Jingying Wu, Junda Cheng, Xin Yang

导读： 本文实现了一种十分快速的动态点剔除方法，仅需要10ms以内就能剔除扫描点云中的动态点，十分便捷高效。©️【深蓝AI】编译

1. 摘要

现有基于3D点云的动态点检测与移除方法存在显著的时间开销，这使得它们难以适应于LiDAR-惯性里程计系统。本文提出了一种基于标签一致性的动态点检测与移除方法，用于处理自动驾驶场景中的行人和移动车辆，并将所提出的动态点检测与移除方法嵌入到一个自设计的LiDAR-惯性里程计系统中。三个公开数据集上的实验结果表明，该方法可以在LiDAR-惯性里程计系统中以极低的计算开销（即1至9毫秒）实现动态点的检测与移除，同时达到与最先进的方法相当的保留率和拒绝率，并显著提高姿态估计的准确性。

2. 介绍

在自动驾驶领域，3D激光雷达（LiDAR）和惯性测量单元（IMU）是用于实时定位的关键传感器。LiDAR-惯性里程计（LIO）已成为实时状态估计的主要框架。大多数现有LIO系统假定环境是静态的，但在实践中，车辆通常会在包含行人和移动车辆的动态环境中行驶。动态对象的点云数据会干扰状态估计并影响地图重建质量。

为了解决动态环境对LIO性能的影响，之前的研究工作中已提出了一些3D点云的动态点检测方法，这些方法分为离线和在线两种。离线方法需要预建地图，而在线方法虽不要求预建地图，但计算成本高，难以实现实时运行。

因此，本文提出了一种基于标签一致性的动态点检测和移除方法，适用于自动驾驶场景中的移动车辆和行人。该方法首先快速分离地面点，并根据点是否位于地面进行标签化。对于位于移动对象上的点，其最近邻点的标签会出现不一致性。如果一个点自身是非地面点，但其最近邻点多为地面点，则判定为动态点，并从地图中移除。

实验结果表明，本方法在semantic-KITTI数据集上实现了先进的保留率和拒绝率，且计算时间显著低于其他在线方法。在ULHK-CA和UrbanNav数据集上的实验也证明了本方法能够显著提升估计姿态的准确性，超越了当前最先进的LIO系统。

本文的主要贡献有：

1)提出了一种低计算成本的动态点检测和移除方法；

2)将提出的方法整合到LIO系统中以增强估计姿态的准确性。

3. 相关工作

3D点云动态点检测与去除方法可分为离线和在线两类。

离线方法包括Removert、ERASOR和DORF等，需要预构建地图：Removert使用多分辨率距离图像来迭代恢复静态点；ERASOR通过伪占用概念来区分不同占用度的空间；DORF则采用由粗到细的框架来生成清洁的静态点云地图。

在线方法包括Dynamic Filter、Dynablox和RH-Map等，不依赖预构建地图：Dynamic Filter针对动态环境提出了在线去除框架；Dynablox能实现在复杂环境中稳健检测移动物体；RH-Map基于3D区域范围哈希映射结构来构建地图。

最近几年，机器人领域发展出了许多激光雷达惯性组合（LIO）系统。LIO-SAM将LIO里程计问题公式化为因子图；LINs在ESIKF框架内集成了6轴IMU和3D激光雷达；Fast-LIO通过优化卡尔曼增益计算来减少计算负担；Fast-LIO2引入了ikd-tree算法来提高效率；DLIO提高了状态预测精度；Semi-Elastic-LIO提供了一种半弹性的状态估计方法；SR-LIO通过扫掠重建方法增加了扫掠频率；RF-LIO和ID-LIO能够处理动态场景下的状态估计，但计算开销大，难以实时稳定运行。

此外，本文还用$(\cdot )w$、$(\cdot )l$和$(\cdot )o$分别表示世界坐标系、激光雷达坐标系和惯性测量单元坐标系中的3D点。在起始位置时，世界坐标系与惯性测量单元坐标系重合。

本文将第$i$次惯性测量单元测量所在时刻$t_i$的惯性测量单元坐标系表示为$o_i$，那么从激光雷达坐标系$l_i$惯性测量单元坐标系$o_i$的变换矩阵（即外部参数）表示为$T_{l_i}^{o_i}\in SE(3)$，其中$T_{l_i}^{o_i}$包含一个旋转矩阵$R_{l_i}^{o_i}\in SO(3)$和一个平移向量$t_{l_i}^{o_i}\in {\mathbb{R}}^3$。通常假设外部参数矩阵$T_{l_i}^{o_i}$已经通过离线方式被足够准确地标定，因此在后续过程中不再进一步优化。因此，本文简化记号$T_{l_i}^{o_i}$为$T_l^o$。

4. 系统详解

图2展示了本文系统的框架，该系统包括四个模块：点云处理、静态初始化、基于ESIKF的状态估计和动态点移除。

点云处理模块分离地面点并进行空间降采样；静态初始化模块利用IMU测量来估计状态参数；状态估计模块与SR-LIO的状态估计模块一致；动态点移除模块使用基于标签一致性的方法检测并移除动态点。

图中还显示系统维护两个全局地图：跟踪地图和输出地图，后者更彻底地过滤动态点。本文使用Hash体素地图来管理这些地图。

4.1 地面拟合

本文利用与LeGO-LOAM相同的地面分割方法来从当前输入扫描中分离出地面点，这种方法计算成本非常低，这对LIO系统来说非常重要。分离地面点的可视化在图3中展示，其中橙色点标记为“地面点”，白色点标记为“非地面点”。

4.2 背景分离

在执行标签一致性检测时，需要为当前扫描中的每个点找到最近邻。靠近车辆的点可以可靠地找到它们的最近邻，而远离车辆的点可能由于它们的位置尚未被重建而找不到最近邻。本文设定了30米的阈值，将距离车辆30米内的点定义为前景点，30米外的点定义为背景点。对于前景点和背景点，本文采用了针对其特性的基于标签一致性的动态点检测方案。

4.3 标签一致性检测

标签一致性检测基于动态对象接触地面的前提。

静态点通常能在全局地图中找到最近邻，而从动态物体扫描得到的LiDAR点通常找不到最近邻，因此被分类为动态点。对于可能找到地面点作为最近邻的点，本文根据最近邻中地面点的比例来决定是否将其分类为动态点。背景点可能因位置未被重建而找不到最近邻，对于这样的点，本文将它们标记为不确定点并放入容器中，待后续重构位置结构后再进行判断。对于前景点、背景点和不确定点，本文采用三种不同的模式进行标签一致性检测。

前景点模式： 如果最近邻的数量低于某个阈值5，则表明$p^w$的位置原本为空，因此将其分类为动态点。如果最近邻数量足够大（大于5），则计算所有最近邻中非地面点的比例。如果这一比例足够低（小于30%），$p^w$被分类为静态点并加入跟踪地图和输出地图。相反，如果比例大于30%，$p^w$被分类为动态点而不加入地图。前景点动态点检测结果的可视化见图5。

背景点模式： 如果最近邻的数量低于某个阈值5，本文无法确定这是由于动态点未能找到最近邻还是因为位置尚未被重建，从而阻止了最近邻的查找。这种点被标记为不确定点，随着车辆继续移动并在这些点的位置结构恢复后做出判断。为了确保状态估计过程中新获取的点云能够正确配准，有必要将不确定点纳入跟踪地图。这不会对状态估计的准确性有太大影响，即使背景点中有动态物体，扫描到它们的LiDAR点也非常稀疏。至于最终的输出地图，必须尽可能少包含动态点，因此对于不确定点的判断将在之后进行。当最近邻数量足够大（大于5）时，处理方式与前景点相同，静态点加入跟踪地图和输出地图。背景点动态点检测结果的可视化见图6。

不确定点模式： 随着车辆继续前进，全局地图中先前未重建的位置的几何结构信息得以恢复（如图7所示）。当不确定点容器中的某点$p_u^w$接近车辆的当前位置（小于30米），其周围几何结构信息极有可能已经被重建。此时本文可以判断$p_u^w$是否为动态点。如果最近邻数量低于某个阈值5，表明$p_u^w$的位置原本为空，导致$p_u^w$被分类为动态点。如果最近邻数量大于5，则计算所有最近邻中非地面点的比例。如果这一比例远小于30%，则将其分类为静态点并加入输出地图。反之，如果比例不低于30%，则将其分类为动态点而不包括在内。如果一个不确定点连续10次扫描都距离车辆位置超过30米，则直接将其分类为静态点并加入输出地图。

5. 实验

本文在三个自动驾驶场景数据集上评估了所提出方法的整体性能：Semantic-KITTI、ULHK-CA和 UrbanNav。

Semantic-KITTI 数据集由一个64线Velodyne LiDAR收集而成，每个LiDAR点都有其独特的语义标签。因此，Semantic-KITTI用于评估本文提出的基于标签一致性的动态点检测和移除方法的保留率（PR）和拒绝率（RR）。ULHK-CA数据集由一个32线Robosense LiDAR和IMU收集，UrbanNav数据集由一个32线Velodyne LiDAR和IMU收集。这两个数据集用于评估动态点检测和移除对位姿估计绝对轨迹误差（ATE）的改进。所有实验均在一个配备Intel Core i7-11700和32 GB RAM的消费级计算机上完成。

5.1 与最先进方法的PR和RR比较

本文将基于标签一致性的动态点检测和移除方法与三种最先进的三维点云基础的动态点检测和移除方法进行了比较，即Removert、Erasor和Dynamic Filter。表I和表II的结果显示，本文的基于标签一致性的动态点检测和移除方法在更高的PR和RR方面对比Dynamic Filter在几乎所有序列上表现出更好的性能。尽管Dynamic Filter在序列00上实现了比本文更高的RR，但本文的结果与他们的非常接近，仅相差0.36%。

5.2 与最先进方法的ATE比较

本文将提出的动态点检测和移除方法集成到一个自设计的LIO系统中以获得Dynamic-LIO，并在ULHK-CA和UrbanNa数据集上将其与四种最先进的LIO系统进行比较，即LIO-SAM、Fast-LIO2、RF-LIO和ID-LIO。Dynamic-LIO都有环路检测模块，并使用GTSAM来优化因子图。表III的结果显示，本文的Dynamic-LIO在MarketStreet和RussianHill上的精度优于 LIO-SAM、Fast-LIO2、RF-LIO和ID-LIO。由于RF-LIO既不是开源的也没有在UrbanNav数据集上进行测试，本文无法获得它在序列TST和Whampoa上的结果。虽然ID-LIO在UrbanNav数据集上实现了比本文系统更小的 ATE，但本文开源代码的做法更好地证明了本文结果的可复现性。

5.3 对不确定点的消融研究

在本文的系统中，包含不确定点的目的是尽可能多地移除动态点，从而增加输出地图中静态点的比例。本节通过比较包含和不包含不确定点情况下Dynamic-LIO的PR和RR值来验证包含不确定点的必要性。表IV和表V的结果显示，包含不确定点可以略微提高本文动态点检测和移除方法的PR和RR。

5.4 对位姿估计的动态点移除的消融研究

本节通过比较包含和不包含动态点移除情况下Dynamic-LIO的ATE结果来评估动态点移除对位姿估计的有效性。表VI的结果显示，移除动态点可以显著提高本文Dynamic-LIO的位姿估计精度。

5.5 与最先进方法的时间消耗比较

本文比较了本文的基于标签一致性的动态点检测和移除方法与两种最先进的三维点云基础的动态点检测和移除方法的时间消耗，表VII的结果显示，无论是Dynamic Filter还是RH-Map都无法保证实时能力，而本文的方法可以稳定地实时运行。表VIII的结果显示，本文Dynamic-LIO的时间消耗明显小于RF-LIO和ID-LIO，而本文的系统运行速度大约是RF-LIO和ID-LIO的5倍。由于RF-LIO既不是开源的也没有在UrbanNav数据集上进行测试，本文无法获得它在序列TST和Whampoa上的结果。

5.6 各模块的时间消耗

本文评估了本文系统的运行时间分解（单位：ms）。对于每个序列，本文测试了点云处理（除地面拟合外）、位姿估计和动态点移除的时间消耗。动态点移除模块可以进一步分解为两个子步骤：地面拟合和标签一致性检测。表IX的结果显示，本文的系统仅需1~9ms 即可移除一次扫描中的动态点，而完成LIO其他任务所需的总时间是16~46ms。这意味着本文的方法可以在LIO系统中以极低的计算开销完成动态点检测和移除。

5.7 地图可视化

图8展示了本文的Dynamic-LIO在示例序列（semantic-KITTI-05）上重建静态点云地图的能力。如图8（a）所示，在移除动态点之前，移动物体的幽灵轨迹（绿色点）在地图上清晰可见。如图8（b）所示，在移除动态点之后，输出地图几乎不再含有幽灵轨迹。

6. 结论

本文提出了一种基于标签一致性的动态点检测和移除方法，该方法根据特定点与其最近邻的一致性来确定该点是否为动态点。本文将提出的动态点检测和移除方法嵌入到一个自设计的LIO系统中，该系统能够准确估计状态并以极低的成本排除动态物体的干扰。实验结果表明，所提出的基于标签一致性的动态检测和移除方法可以实现与最先进的动态点检测和移除方法相当的保留率（PR）和拒绝率（RR），同时确保更低的计算成本。此外，本文的Dynamic-LIO在动态环境下的运行速度大约是最先进的LIO系统的5倍。

编译｜蒙牛二锅头

审核｜Los

移步公众号【深蓝AI】，第一时间获取自动驾驶、人工智能与机器人行业最新最前沿论文和科技动态👇

深蓝AI·赋能自动驾驶+机器人+AI