RD-VIO: 动态环境下移动增强现实的稳健视觉惯性里程计

文章：RD-VIO: Robust Visual-Inertial Odometry for Mobile Augmented Reality in Dynamic Environments

作者：Jinyu Li , Xiaokun Pan , Gan Huang, Ziyang Zhang, Nan Wang, Hujun Bao, Guofeng Zhang

编辑：点云PCL

摘要

通常对于视觉或视觉惯性里程计系统来说，处理动态场景和纯旋转问题通常是具有挑战性的。在这项工作中，我们设计了一种新颖的视觉惯性里程计（VIO）系统，称为RD-VIO，来处理这两个问题。首先提出了一种IMU-PARSAC算法，它可以在两个阶段的过程中鲁棒地检测和匹配关键点。在第一阶段，利用视觉和IMU测量将地标与新的关键点进行匹配，从匹配中收集统计信息，然后在第二阶段引导关键点之间的匹配。其次，为了处理纯旋转问题，我们检测运动类型，并在数据关联过程中采用适应性延迟三角化技术，将纯旋转帧转换为特殊的子帧，在解决视觉惯性捆绑调整时，它们为纯旋转运动提供了额外的约束。在公共数据集和在线比较中评估了提出的VIO系统。实验证明提出的RD-VIO在动态环境中具有明显的优势。源代码链接：https://github.com/openxrlab/xrslam。

主要贡献

如图1所示，提出的VIO系统RD-VIO可以适应纯旋转运动和大型移动物体，这在许多其他VIO/VISLAM系统中很容易导致发散，例如VINS-Mobile。我们在公共数据集上测试了所提出的系统，并将其与许多最新的VIO系统进行了比较。实验结果表明，我们提出的系统不仅能够产生准确的跟踪结果，而且能够以更为稳健的方式实现。本文的主要贡献如下：

• 提出了一种新颖的IMU-PARSAC算法，用于检测和消除动态场景中的运动异常值，可以明显提高跟踪的稳健性。
• 提出了一种新颖的滑动窗口中的子帧策略，以有效地减少纯旋转运动下的漂移。
• 整个系统的源代码已发布，以造福社区，包括核心VIO算法和用于移动增强现实应用的iOS项目。

图1. 提出的RD-VIO可以在具有纯旋转运动的动态场景中稳健地工作，并且优于一些其他最新的VIO/VI-SLAM系统，如VINS-Mobile。

内容概述

首先从一个基线VIO系统开始，该系统基于PVIO，但不使用平面先验，基线VIO也将用于比较，我们系统的流程如图2所示。在该系统的基础上，我们进行了修改，检测纯旋转并正确三角化地标，然后将纯旋转帧组织成子帧，并相应地优化姿态。

图2. RD-VIO的流程图

A. 滑动窗口VIO

系统采用了滑动窗口方法，因此首先介绍了一个具有滑动窗口优化的基线VIO系统（Baseline-VIO），并定义了大部分符号。基线VIO系统通过在窗口中保留最近的若干关键帧，运行捆绑调整以融合视觉和惯性测量，并边缘化过时的帧，就像一个多帧窗口沿着时间滑动一样工作。

在跟踪过程中，系统保留了一定数量的最近关键帧和它们观察到的地标。这些关键帧和地标的状态通过视觉惯性捆绑调整进行细化。帧的状态包括位置、方向、速度以及陀螺仪和加速度计的偏差。地标的状态则采用逆深度参数化。系统假设相机内部参数和相机与IMU之间的外参数是恒定的。在捆绑调整中，系统最小化了所有重投影误差和运动测量误差的总和，以优化状态。IMU测量也被纳入考虑，并通过IMU预积分计算相关成本项。最后，系统通过边缘化旧关键帧的状态来限制计算复杂度。

VIO的初始化包括对重力向量的追踪、全局尺度的解决以及初始状态的确定。首先，选择一系列初始帧，并对这些帧进行仅视觉的SfM（Structure from Motion），该结果给出了这些帧的相对姿态，尺度是任意的。然后，IMU测量与SfM结果进行对齐。通过对齐，我们可以使用[36]中介绍的方法解决重力向量和初始尺度。最后使用完整的捆绑调整来找到最佳的初始状态。

系统使用KLT算法检测和跟踪关键点。如果关键点已经有关联的地标，系统会预测其在下一帧的位置，并将其作为KLT跟踪的初始位置。对于尚未求解的新帧，系统通过积分IMU测量来外推姿态。为了去除异常匹配，系统使用RANSAC估计本质矩阵和单应性矩阵。接着介绍了如何使用视觉惯性PnP来获得新帧状态的初始估计。最后，说明了系统如何将新帧条件性地标记为关键帧，并在滑动窗口中进行优化或清除，以应对低位移问题。

B. 异常值检测和去除

这里介绍IMU-PARSAC算法，利用IMU信息来区分移动元素和静态背景，这种区分增强了VIO跟踪的稳健性。我们的动态异常值移除方法分为两个阶段：基本的3D-2D匹配阶段（IMU-PARSAC）和一个可选的2D-2D匹配阶段，如图3所示。

图3. 移动异常值检测和去除策略：在必要的3D-2D阶段，当前帧基于光流跟踪与上一帧获得2D观测和3D点的初始匹配。经过IMU-PARSAC算法后，大多数异常值被滤除。在可选的2D-2D阶段，当前帧和滑动窗口中的关键帧逐帧使用原始PARSAC算法进行匹配。通过这种多视角交叉验证方法去除剩余的动态异常值。

在初始阶段将地图中的静态3D地标与新捕获的图像的2D关键点对齐。IMU预积分预测当前姿态，引导3D-2D匹配过程，如果地标稀缺，将从2D-2D匹配中导出新的地标。之后从3D-2D阶段收集误差统计信息，制定2D-2D PARSAC的动态阈值，这种策略抵消了移动物体产生的可变误差。在其核心，我们的方法论旨在将IMU测量融入到稳健的参数估计算法框架中，并充分利用相机和IMU的协同效益。

1. 3D-2D匹配阶段：系统介绍了在新帧到达时如何进行3D地标与2D关键点的匹配。首先系统假设这些地标在三角测量时是静态的，而异常匹配是由于错误的对应或对象开始移动造成的，提出使用从IMU测量中积分得到的姿态预测来辅助匹配。然后使用了改进的RANSAC算法进行迭代，筛选出共识对应集。在评估共识集质量时，系统考虑了观测时间先验，以确保对静态地标的更准确识别。最终根据共识集的质量选择出最佳的内点集，从而确保匹配的稳健性和准确性。
2. 2D-2D匹配阶段：系统描述了在滑动窗口策略中如何处理无法跟踪的地标，以及如何补充新的地标以保持足够的数量。在这个阶段使用了原始的PARSAC算法，但仍然存在接近退化的挑战。通过极线几何关系来识别异常值，并使用IMU数据帮助识别移动关键点。还介绍了如何根据3D-2D匹配训练极线距离阈值，并将其用于2D-2D匹配阶段的阈值设定。最后描述了如何追踪历史匹配，并根据一定条件将关键点标记为静态并进行三角测量。

C.纯旋转检测和延迟三角测量

在这一部分描述了一种用于检测纯旋转并延迟三角测量的方法。由于消费级手机IMU传感器存在较大噪声，因此系统设计了一种基于视觉的方法来检测纯旋转。为了实现这一目标，在特征跟踪中增加了第三次RANSAC,第三次RANSAC用于解算匹配点之间的旋转矩阵。具体地首先计算最新两帧之间的平移量 t，并计算共同地标 p 到 t 所在直线的距离 D。然后根据几何关系，计算出两个观测之间的最大角度，如果该角度小于预定义的阈值 θrot，则将最新帧标记为“纯旋转帧”，否则标记为“正常帧”。

图4. 基于角度的纯旋转检测的几何示意图，当两条观测射线和平移向量 t 形成等腰三角形时，最大的 θ 角被实现。

如果一个帧被标记为“纯旋转帧”，则说明它缺乏对新地标的深度观察。在这种情况下，系统将从该帧检测到的新关键点部分三角测量为地标，但只记录其起始帧和位置，而不估计其深度，在获取足够的深度观察后，系统会重新估算并更新这些地标。这种延迟三角测量的策略受到先前研究的启发，并可以有效提高地标的跟踪性能。值得注意的是，由于纯旋转帧无法提供深度观察，因此系统会通过其他方法获取早期的位置估计，例如视觉惯性PnP。然而，在某些情况下，深度观察可能仍然不足，这时系统的视觉方法会更加适用。

图5. 当相机停止跟踪产生的点云示例，蓝色点是延迟三角测量的地标。它们被投射为具有虚假1米深度的点以进行可视化。我们可以看到，在基准-VIO中，正常地标（红色）很少，因为它们的深度发散了。而有了延迟三角测量，可以跟踪更多的关键点。另一方面，SF-VIO可以保持深度稳定。

D. 子关键帧的滑动窗口

正如之前介绍的，我们不能填充滑动窗口以容纳纯旋转帧，也不能丢弃纯旋转帧，因为它们必须保留以持续估计IMU偏差，在系统中引入了一个子帧机制，允许一个关键帧携带一组子帧，如图6中每种情况的下部分所示（它们附加到的N-帧称为关键帧）。系统利用这种子帧策略来处理长序列的纯旋转帧。

图6. 在滑动窗口尾部添加新帧的帧管理规则。（a，d，g，i）是初始情况，而（b，e，f，h，j）是添加新帧后的结果。

滑动窗口结构中的帧管理策略

• 添加新帧时，滑动窗口中的最后一个关键帧始终是一个N-帧。
• 在同一子帧窗口中，不会同时存在N-帧和R-帧。
• 根据最新子帧窗口中的帧类型和新帧的类型，会出现四种情况：RR、NN、RN 和 NR。
• 对于不同情况，采取不同的处理方式：追加新帧、转换子帧为关键帧、添加新帧作为关键帧或添加新帧作为子帧。
• 当最后一个关键帧没有子帧时，新帧将被添加为子帧。
• 当跟踪到的关键点数量低于某个阈值时，新帧将被添加为N-关键帧。

捆集调整主要包括以下几点内容

• 无新关键帧时的调整：当滑动窗口中没有新的关键帧时，不会进行完整的捆集调整。相反，只对最后一个子帧窗口中的状态进行优化，以进行快速更新。
• 包含N-帧的最后一个子帧窗口的处理：如果最后一个子帧窗口包含N-帧，则有足够的平移和深度观察。在这种情况下，使用相同的捆集调整，但将固定关键帧和这些关键帧中观察到的地标。这样，只有在最后一个子帧窗口中观察到的新地标以及这些子帧的状态会被优化。
• 包含R-帧的最后一个子帧窗口的处理：如果最后一个子帧窗口中填满了R-帧，则会处理一系列预积分，以更好地估计IMU偏差。在这种情况下，放弃了深度估计，并使用延迟三角测量的方向来调整子帧的方向。同时固定关键帧姿势和旧地标。然后进行整体优化，以稳定子帧姿势，并通过最小化IMU运动误差来更好地优化IMU偏差。
• R-型子帧窗口的压缩：如果R-帧的数量太多，将会导致求解速度变慢。因此，当R-帧的总数超过一定阈值时，会对子帧窗口进行压缩。此时，选择部分R-帧进行压缩，并使用它们之间的预积分来提高求解速度。
• 添加新关键帧时的处理：当向滑动窗口中添加新的关键帧时，将对所有关键帧进行完整的捆集调整。对于携带R-型子帧的关键帧，使用预积分链来进行调整。然后，在滑动窗口中保留一定数量的关键帧，并在边缘化过程中删除子帧。

实验

为了评估我们提出的方法的有效性和VIO系统的稳健性，我们进行了一系列实验。关于动态异常值去除策略，我们定性比较和分析了IMU-PARSAC算法与其他算法。为了解决由纯旋转引起的系统状态估计退化问题，我们研究了纯旋转的检测性能以及相机静止时系统的稳定性。最后，我们在公开可用的数据集上定量比较了我们的方法与当前最先进的VIO/VI-SLAM算法。在两个公开数据集上评估了我们的方法和其他最先进的系统。

• EuRoC数据集是用于VIO和SLAM算法的基准数据集。它包括由配备有立体摄像机和同步IMU的微型飞行器（MAV）捕获的高质量数据，涵盖了各种室内场景。该数据集提供了通过运动捕捉系统获得的地面真值姿势，从而可以评估估计轨迹的准确性。
• ADVIO（Advanced Visual-Inertial Odometry）数据集是一个开放的基准数据集，用于评估视觉惯性算法。它包含各种真实世界场景，包括不同的室内/室外环境、光照条件和动态物体干扰，可用于评估各种算法的稳健性、准确性和实时性能。

异常值移除

我们对IMU-PARSAC在手工场景和公共数据集ADVIO上进行了定性和定量评估。我们的手工场景包括静态背景和前景的移动物体，有些物体偶尔会遮挡视野的大部分区域，以测试IMU-PARSAC的能力。图7比较了两种不同场景下几种异常值移除方案的效果，包括传统的鲁棒估计器RANSAC、动态对象分布先验估计器PARSAC以及我们提出的IMU-PARSAC。我们还比较了使用IMU预积分预测姿势来消除视觉观测中的异常值的能力。我们对当前帧中可见的2D点和地图中的3D点执行PnP几何估计，以确定观察到的2D点是否对应于移动物体。

图7. 几种异常值移除方法的定性比较：（a）使用IMU预积分预测姿势来识别异常值（b）传统的鲁棒估计器RANSAC（c）动态对象分布先验估计器PARSAC（d）我们提出的IMU-PARSAC算法。我们将2D观察结果可视化，并根据内点掩码将它们标记为绿色表示内点，红色表示异常值。

纯旋转检测

为了仔细研究纯旋转检测和稳定效果，我们依赖于EuRoC数据集提供的高质量地面真实数据。我们从地面真实数据中计算运动速度并绘制速度曲线。对于每个检测到的R帧，我们添加了一个表示其时间点的红色线。对于所有序列，都存在长时间的停止期。我们的方法几乎可以将这些时期的所有帧标记为R帧。实际上，在MH_01_easy、MH_02_easy和MH_05_difficult中，只有少数几帧是异常值。这是由于背景中出现了移动物体。除了停止期，我们还可以看到许多速度局部最小值被成功检测为R帧。MH序列中出现的场景很大，V1_01_easy和V2_01_easy中的整体运动速度较慢。因此，我们可以在许多局部最小点中看到稀疏标记的R帧。为了进一步检查我们的纯旋转检测方法的速度范围，我们在图8中为每个序列绘制了R帧和N帧的热图。

稳定效果

作者通过对比实验结果，展示了SF-VIO在处理停止场景时的优势。在EuRoC数据集中，作者观察到所有序列都存在长时间的停止情况。通过将结果轨迹对齐并最小化整体RMSE，作者比较了SF-VIO和Baseline-VIO在序列MH_05_difficult上的误差曲线。结果显示，Baseline-VIO的误差曲线会出现抖动，这是由于缺少深度观测导致的地标位置不准确，IMU积分误差不断累积所致。相比之下，SF-VIO能够适应停止情况，保持跟踪位置在原地锁定，从而产生平稳的误差曲线。作者还通过可视化速度曲线和R帧检测结果，进一步说明了SF-VIO在处理停止场景时的有效性。

图9显示了序列MH_05_difficult前20秒的定位误差曲线

定性比较

表1列出了我们在这些算法上收集的所有EuRoC RMSE。与基线VIO相比，SF-VIO在许多序列上都显示出了显著的改进。

我们测量了系统每个模块的运行时间。我们将VINS-Mono配置为具有8帧大小的滑动窗口，并停用了其后端，以确保两个系统之间的公平比较。VINS-Mono和RD-VIO均在配备有Intel i7-7700 CPU @3.6GHz和16GB内存的计算机上执行。不同模块的结果如表2所示。

表3列出了ADVIO数据集的准确性和完整性结果。与没有动态物体移除策略的SF-VIO相比，RD-VIO在ADVIO数据集上显示出显着更好的RMSE，并在RD-VIOs1和RD-VIO中的大多数序列中取得了最佳准确性。

表4显示了3种算法的绝对位置误差（APE）（以毫米为单位）及其相应的鲁棒性值，较小的值表示性能更好。与ARKit和ARCore相比，在典型的相机运动静态场景中，我们的系统有稍大的APE。然而，在快节奏的场景中（例如A1和A2情况），它与ARKit和ARCore表现相当。

图11显示了上述算法生成的轨迹和VICON记录的轨迹。可以清楚地看到，在这些具有挑战性的场景中，RD-VIO实现了更稳定、更健壮的跟踪。需要注意的是，ARKit和ARCore都是综合的VI-SLAM系统。它们受益于广泛的工程优化，涵盖了硬件、软件和芯片级增强。相比之下，我们的研究着重于打造一种轻量级和稳健性之间取得平衡的紧凑型视觉惯性里程计系统。

图11显示了在A3（左）和A4（右）场景中的VICON、ARCore、ARKit和RD-VIO的轨迹。为了确保清晰可视化，重叠区域手动淡化。

我们在真实场景中插入了一个虚拟立方体和其他一些虚拟对象，图12显示了两个AR示例，还将其与VINS-Mobile进行了比较，后者是最好的开源移动AR系统之一，它们都在iPhone X上运行。实验结果表明，RD-VIO在纯旋转条件和动态场景下表现出优越性。

图12. 移动设备上的AR效果

总结

本文提出了一种强大且新颖的VIO系统，可以高效处理动态场景和纯旋转运动，通过使用IMU-PARSAC算法，以两阶段过程移除动态特征点。这种方法使我们的系统能够有效地应对剧烈的场景变化。对于纯旋转运动，设计了一个子帧结构，并使用延迟三角测量技术。这两种方法都在退化运动场景中显著改善了我们的系统。在EuRoC和ADVIO数据集上取得了明显更好的结果，这证明了我们系统的有效性，我们的算法在计算成本上也表现良好，可以实时在移动设备上运行，iPhone X上的AR演示进一步展示了算法在挑战性场景中的稳健性，它还展示了算法在移动AR应用领域的能力。系统仍然存在一些局限性，在设备长时间处于极具挑战性的场景中，它可能无法很好地工作。特别是当没有有效的视觉观测作为输入时，我们的系统将不可避免地失去跟踪，在这种情况下，结合其他一些算法可能会有所帮助，例如纯惯性里程计或无线跟踪。