Citations： Y. Qiu, C. Wang, W. Wang,et al.AirDOS: Dynamic SLAM benefits from Articulated Objects[C].2022 International Conference on Robotics and Automation (ICRA).Philadelphia, PA, USA.2022：8047-8053.

Keywords： Visualization, Simultaneous localization and mapping, Three-dimensional displays, Tracking, Heuristic algorithms, Dynamics, Pose estimation

摘要

动态对象感知 SLAM （DOS） 利用对象级信息在动态环境中实现可靠的运动估计。现有的方法主要集中在从优化中识别和排除动态对象。在本文中，我们展示了基于特征的视觉SLAM系统也可以通过利用两个观察结果从动态铰接物体的存在中受益：（1）铰接物体的每个刚性部分的3D结构随着时间的推移保持一致;（2） 同一刚性零件上的点遵循相同的运动。特别是，我们介绍了AirDOS，这是一个动态的对象感知系统，它引入了刚性和运动约束来模拟铰接对象。通过联合优化相机姿势、物体运动和物体3D结构，我们可以纠正相机姿态估计，防止跟踪丢失，并为动态物体和静态场景生成4D时空图。实验表明，该算法提高了视觉SLAM算法在具有挑战性的拥挤城市环境中的鲁棒性。据我们所知，AirDOS 是第一个动态对象感知 SLAM 系统，展示了通过合并动态铰接对象可以改进相机位姿估计。

一、引言

同时定位和映射（SLAM）是许多机器人应用中的基础研究问题。尽管它在静态环境中取得了成功，但在动态世界中性能下降和缺乏鲁棒性已成为其实际应用的主要障碍[1]，[2]。为了应对动态环境的挑战，大多数SLAM算法采用消除策略，将移动物体视为异常值，并仅根据静态地标的测量值来估计相机的姿态[3]，[4]。该策略可以处理具有少量动态环境的环境，但无法解决具有挑战性的情况，其中动态对象覆盖了大视野，如图1（a）所示。

已经做出了一些努力，将动态对象包含在 SLAM 过程中。很少有方法试图估计简单刚性物体的姿势[5]，[6]或估计它们的运动模型[7]，[8]。例如，CubeSLAM [6] 引入了一个简单的 3D 长方体来模拟刚性对象。Dynamic SLAM [9]估计动态物体的3D运动。然而，这些方法只能涵盖特殊的刚性物体，例如立方体[6]和二次曲线[5]，并不能证明可以通过引入动态物体[7]-[9]来改善相机姿态估计。这引入了我们的主要问题：

我们是否可以利用SLAM中的移动对象来改善相机姿态估计，而不是过滤掉它们？

在本文中，我们将简单的刚性物体扩展到一般的铰接物体，定义为由一个或多个刚性部件（链接）组成的物体，这些刚性部件（链接）通过允许旋转运动的关节连接[10]，例如图2中的车辆和人，并利用铰接物体的属性来改善相机姿态估计。也就是说，我们共同优化（1）3D结构信息和（2）铰接物体的运动。为此，我们引入了（1）刚性约束，它假设位于同一刚性零件上的任意两点之间的距离随着时间的推移保持不变，以及（2）运动约束，它假设相同刚性零件上的特征点遵循相同的3D运动。这使我们能够构建包含动态和静态结构的 4D 时空地图。

总之，本文的主要贡献是：

• 为立体SLAM引入了一个名为AirDOS的新管道，以共同优化相机姿势，动态对象的轨迹和环境地图。

• 我们为一般动态铰接对象引入了简单而有效的刚性和运动约束。

• 我们推出了一个新的基准TartanAir Shibuya，在此基础上，我们首次证明了动态铰接物体可以有利于视觉SLAM中的相机姿态估计。
  

二、相关工作

近年来关于动态SLAM的研究大致分为三类：消除策略、运动约束和刚性约束，将分别进行综述。

A. 消除策略

此类别中的算法过滤掉动态对象，仅利用环境的静态结构进行姿势估计。因此，此类别中的大多数算法都应用RANSAC [11]和鲁棒损失函数[12]等消除策略来消除动态对象的影响。例如，ORB-SLAM [3] 应用 RANSAC 来选择和删除无法收敛到稳定姿态估计的点。DynaSLAM [13] 通过多视图几何和深度学习模块检测移动物体。这允许修复被动态对象遮挡的帧背景。Bârsan 等人 [14] 使用实例感知语义分割和稀疏场景流将对象分类为背景、移动或潜在移动对象。Dai 等人 [15] 利用地图点的距离相关性从静态背景分割动态对象。为了降低计算成本，Ji等人[16]将语义分割和几何模块相结合，将深度图像聚类到几个区域，并通过重投影错误识别动态区域。

B. 运动约束

此类别中的大多数算法估计动态对象的运动，但没有表明运动约束有助于相机姿态估计，因此在高度动态的环境中会受到影响。例如，Hahnel 等人 [17] 跟踪 SLAM 系统中的动态对象。

Wang等人[18]介绍了一种同步定位、建图和移动目标跟踪（SLAMMOT）算法，该算法使用基于动态贝叶斯网络的学习运动模型跟踪移动对象。Reddy等人[19]使用光流来分割移动的物体，并应用平滑的轨迹约束来强制物体运动的平滑性。Judd等人[8]提出了多运动视觉里程计（MVO），它同时估计相机姿势和物体运动。Henein等人[7]，[20]，[21]的工作，其中最近的是VDO-SLAM [20]，生成动态和静态结构图，并使用运动约束估计刚性运动物体的速度。Rosinol等人[22]提出了3D动态场景图来检测和跟踪动态场景中密集的人体网格。这种方法限制了人类进行一致性检查的最大步行速度。

C. 刚性约束

刚性约束假定同一刚体上点的成对距离随时间推移保持不变。它被应用于可追溯到 1980 年代的动态环境中分割移动物体。Zhang等人[23]提出使用刚性约束来匹配移动的刚体。Thompson等人[24]使用类似的刚性约束思想，并提出了用于移动刚性对象匹配的刚性几何测试。以前的研究利用刚性假设来分割移动的刚性物体，而在本文中，我们使用刚性约束来恢复物体的结构。

为了对刚性物体进行建模，SLAM++ [25] 在物体匹配和姿势优化中引入了预定义的 CAD 模型。二次二次曲线[5]利用双二次曲面作为3D对象表示，以表示对象地标的方向和比例。Yang和Scherer[6]提出了一个名为CubeSLAM的单目物体SLAM系统，用于3D长方体物体检测和多视图物体SLAM。如前所述，上述方法只能对简单的刚性物体（例如立方体）进行建模，而我们针对的是更通用的对象，即铰接对象，它们可以覆盖常见的动态对象，例如车辆和人类。

三、方法论

A. 背景和符号

（a） 刚性约束系数图。黑色节点表示相机姿势，蓝色节点表示动态点，红色节点表示刚性段长度。因此，青色和红色矩形表示点和刚度的测量值。（c） 可以根据身体部位的刚度对人体进行点和段建模。（b） 运动约束的因子图。橙色节点表示估计运动，绿色矩形表示运动约束。

B. 刚性约束

我们将人类建模为图3（b）所示的特殊铰接对象，其中每个人可以用14个关键点来描述，包括鼻子，肩膀，肘部，手，腰，膝盖，脚等。在实验中，我们使用现成的算法Alpha-Pose [27]检测人类的关键点。

C. 运动约束

运动约束同时估计对象的运动lT并执行每个要点lp我k遵循相同的运动模式 [7]。此运动模型lT假设对象是刚性的，因此，对于铰接对象，我们对铰接对象的每个刚性部分应用运动约束。在图3（c）中，我们显示了运动约束的因子图。

在高动态环境中，即使我们过滤掉运动物体，静态特征的跟踪也很容易被运动物体打断。通过强制运动约束，动态对象将能够为相机姿势的运动估计做出贡献。因此，当静态特征不够可靠时，运动物体可以校正相机的姿态估计，防止跟踪丢失。

D.光束法平差

光束法平差 （BA） 共同优化静态点、动态点、相机位姿和动态物体运动轨迹。

四、系统概述

我们在图中提出了框架AirDOS。III-B用于动态立体视觉SLAM，由三个模块组成，即预处理、跟踪和后端捆绑调整。

在预处理和跟踪模块中，我们首先提取ORB特征[28]并执行实例级分割[29]以识别潜在的移动对象。然后，我们通过跟踪静态特征来估计初始自我运动。对于像人类这样的铰接物体，我们执行Alpha-Pose [27]来提取人体关键点，并通过从立体图像中对相应的关键点进行三角测量来计算其3D位置。然后，我们使用PWC-net产生的光流[30]跟踪移动的人类。跟踪模块为相机姿势以及动态物体的物体姿势提供可靠的初始化。

在后端优化中，我们构建了一个由相机姿势、静态点、动态点和物体运动组成的全局地图。为了提高效率，我们对从共可见地标构建的共可见性图 [31] 中的动态对象执行局部束调整。与RANSAC的策略类似，我们基于刚性约束（3）和运动约束（5）消除了导致大误差的因素和边缘。这个过程有助于识别不匹配或错误估计的人类姿势。由于重复的静态特征观察，视觉 SLAM 算法通常只对选定的关键帧执行束平差。然而，在高度动态的环境中，如本文介绍的环境，这很容易导致动态对象跟踪的丢失，因此我们对每一帧都执行束调整以捕获完整的轨迹。

五、实验

A. 指标、基线和实现

我们使用绝对平移误差（ATE）来评估我们的算法。我们的方法与最先进的方法，ORB-SLAM [3] （1） 和 （2） 没有屏蔽潜在的动态对象，以及 RGB-D 动态 SLAM 算法 [20] 进行了比较。与第四节中描述的设置类似，我们修改了ORB-SLAM，通过动态特征的观察对每一帧执行BA，从而捕获移动物体的完整轨迹。在实验中，我们将相同的参数应用于AirDOS和ORB-SLAM，即每帧提取的特征点数，RANSAC的阈值以及重投影误差的协方差。

B. KITTI跟踪数据集的表现

KITTI 跟踪数据集 [32] 包含 50 个序列（29 个用于测试，21 个用于训练），具有多个移动对象。我们选择 6 个包含移动行人的序列。为了进行评估，我们使用IMU和GPS生成地面真相。如表I所示，我们的方法和ORB-SLAM的ATE在所有序列中都很小，这意味着两种方法在这些序列中表现良好。主要原因是运动物体相对较远且较小，并且这些序列中存在丰富的静态特征。此外，大多数序列都有简单的平移运动，这使得这些情况非常简单。

尽管相机轨迹相似，但我们的算法恢复了一个更好的人体模型，如图 5 所示。当人类远离摄像机时，ORB-SLAM 会产生嘈杂的人体姿势。这是因为刚性约束有助于恢复移动铰接对象的结构。此外，运动约束可以提高动态对象轨迹的精度。根据整个轨迹的观察结果，我们的算法可以恢复人体姿势并消除不匹配的动态特征点。

C. Tartanair Shibuya数据集的表现

我们注意到KITTI数据集中的移动对象仅覆盖了很小的视野。为了应对高度动态环境的挑战，我们构建了如图6所示的Tartanair Shibuya数据集，并证明我们的方法优于该基准测试中现有的动态SLAM算法。我们之前的工作TartanAir [33]是一个非常具有挑战性的视觉SLAM数据集，由双目RGB-D序列以及额外的每帧信息组成，如相机姿势，光流和语义注释。在本文中，我们使用相同的管道生成涩谷TartanAir，这是为了模拟图1所示的世界上最繁忙的东京涩谷道路交叉口。它涵盖了比TartanAir更具挑战性的视角和铰接物体的各种运动模式。

我们将TartanAir涩谷数据集分为两组：图6（a）中的站立人类和图6（b）中的路口，具有简单和困难的类别。每个序列包含 100 帧和 30 多个被跟踪的移动人物。在站立人类的序列中，大多数人类静止不动，而很少有人在空间中移动。在《道路交叉路口》中，有多个移动的人来自不同的方向。对于困难的序列，动态物体往往会突然进入场景，传统方法的视觉里程计很容易失效。

1） 评价
为了测试当视觉里程计被动态物体打断时，或者在由于照明变化等间接遮挡而导致分割可能失败的情况下，我们评估了两种设置的性能：在自我运动估计期间有和没有屏蔽动态特征。

如表II所示，使用人类掩码，与ORB-SLAM [39]和VDO-SLAM [5]相比，我们的算法在整体性能上分别提高了15.2%和3.20%。在序列II，IV和V中，ORB-SLAM和我们的算法都显示出良好的性能，其中所有ATE都低于0.04。我们注意到 VDO-SLAM 的性能不如 ORB-SLAM。这可能是因为VDO-SLAM严重依赖光流进行特征匹配，它可能会将背景特征与动态特征混淆。

我们的算法在不掩盖潜在移动物体的情况下也优于 ORB-SLAM。如表II的序列I、III、V和VI所示，我们的方法获得的精度比ORB-SLAM高0.0717、0.050、0.721和0.306。总体而言，我们实现了18.1%的改善。这是因为移动的物体很容易导致传统的视觉里程计失败，但我们从移动的铰接物体中获取观察结果来纠正相机姿势，并过滤掉不匹配的动态特征。

可以看出，在图 7 中，ORB-SLAM 被移动的人类打断，在进行大旋转时失败。通过跟踪移动的人类，我们的方法在转弯时优于ORB-SLAM。此外，更好的相机姿态估计反过来可以有利于移动物体的轨迹。可以看出，ORB-SLAM 生成的物体轨迹是嘈杂且不一致的，而我们的轨迹更平滑。通常，所提出的运动约束和刚性约束对困难序列有显著影响，其中ORB-SLAM由于动态对象而输出不准确的轨迹。

六、消融研究

我们进行了一项消融研究，以显示引入的刚度和运动约束的影响。具体来说，我们证明了运动约束和刚性约束通过束平差证明了相机姿态估计。

A. 执行

我们模拟遵循简单恒定运动模式的动态铰接对象，并使用高斯噪声初始化机器人的状态σ=0.05米 关于翻译σ=2.9∘轮换。我们还在机器人路径周围生成静态特征，并模拟具有有限视野的传感器。点的测量也有σ=0.05米.我们生成 4 组不同长度的序列，每组由 10 个序列组成，这些序列使用相同数量的静态和动态特征进行初始化。我们将静态与动态地标的比例设置为 1：1.8。

B. 结果

我们评估了 （a） 仅具有静态特征的束调整、（b） 无运动约束的束调整、（c） 无刚性约束的束调整以及 （d） 同时具有运动约束和刚性约束的束调整的性能。我们使用绝对平移误差 （ATE） 和旋转相对姿势误差 （RPE-R） 和平移 （RPE-T） 作为我们的评估指标。

如表III所示，运动和刚度约束都能够改善相机姿态估计，而当同时应用这两个约束时，可以获得最佳性能。一个有趣的现象是，刚性约束也可以有利于物体的轨迹估计。在I组中，我们通过100个重复实验来评估设置（b），（c）和（d）的动态点估计。我们发现设置（c）中动态对象特征点的ATE为：5.68±0.30低于设置 （b），而设置 （d） 比 （b） 低 5.71 ± 0.31。这是因为运动约束假定同一对象上的每个动态特征都遵循相同的运动模式，这要求对象是刚性的。从另一个角度来看，刚性约束为物体的三维结构提供了良好的初始化，因此间接地改善了对物体轨迹的估计。一般来说，消融研究证明，对动态铰接物体应用运动和刚度约束有利于相机姿态估计。

C. 计算分析

最后，评估了优化中刚性约束和运动约束的运行时间。后端优化是在C++中通过修改后的 g2o [34] 求解器实现的。使用与VI-A部分相同的设置，我们用10帧随机初始化了18个不同的序列。在每一帧中，我们可以观察到一个移动物体的 8 个静态地标和 12 个动态地标。在表IV中，我们显示了消融研究中I组每次迭代的（i）收敛时间（ii）运行时间。我们的方法需要 53.54 （mSec） 来收敛，这与仅具有重新投影误差的优化中的 39.22 （mSec） 相当。

在本文中，语义掩码[29]和人类姿势[27]被预处理为系统的输入。该实验是在具有 7GB RAM 的英特尔酷睿 i16 上进行的。

七、结论

在本文中，我们引入了刚性约束和运动约束来模拟动态铰接物体。我们提出了一种新的管道，即用于双目SLAM的AirDOS，它共同优化了动态物体的轨迹、环境地图和相机姿势，提高了动态环境中的鲁棒性和准确性。我们在KITTI跟踪和TartanAir涩谷数据集中评估了我们的算法，并证明了相机姿态估计和动态对象可以相互受益，特别是当存在激进的旋转或静态特征不足以支持视觉里程计时。