DynaTM-SLAM：针对动态环境设计的鲁棒语义视觉SLAM算法

DynaTM-SLAM: Fast filtering of dynamic feature points and object-based localization in dynamic indoor environments

原文链接

摘要： 由于科学技术的进步，已经开发出许多先进的同步定位与建图（SLAM）算法。然而，它们在复杂的现实场景中的实际适用性受到对象静止的假设的严重限制。因此，提高 SLAM 算法在动态环境中的准确性和鲁棒性至关重要。使用语义分割或对象检测对动态环境中的 SLAM 进行了大量研究，但这些方法的一个主要缺点是，如果可移动对象是静态的，它们可能会消除静态特征点，或者如果静态对象是静态的，则可能会使用动态特征点。物体被移动。 本文提出了DynaTMSLAM，一种鲁棒的语义视觉SLAM算法，专为动态环境而设计。 DynaTM-SLAM将目标检测和模板匹配技术与滑动窗口相结合，快速有效地过滤掉真实的动态特征点，大大减少动态目标的影响。我们的方法使用对象检测而不是耗时的语义分割来检测动态对象。此外，在线构建对象数据库，通过对静态对象实施语义约束来联合优化相机位姿、地图点和对象。该方法充分利用了静态对象语义信息的积极作用，并提高了动态环境中自我运动估计的准确性。在TUM RGBD数据集上进行了实验，结果表明在动态场景中性能有显着提升。

---

引言：

• 静态假设会导致错误得姿态估计和闭环检测，降低SLAM得性能。
• DS-SLAM通过结合语义分割和移动一致性检查来减少动态物体的影响，但其效果有限且高度依赖于阈值设置，主要依靠物体内动态点的比例来判断物体是否静态或动态。DynaSLAM使用语义分割和 ORB 功能通过多视图几何方法检测动态对象。应用区域生长算法从孤立的离群点中找到整个动态区域。如果一个对象，即使是剩余的静态对象有异常值，算法也会认为整个区域正在移动，从而使得异常值检测对其性能至关重要。
• 语义分割对于动态slam来说较为耗时
• YOLO-SLAM
• 大多数关注动态场景的SLAM算法主要关注消除动态物体以优化性能，而未能充分利用静态物体语义信息的潜在优势。
• 建议当预定义的可移动对象保持静态时使用来自预定义的可移动对象的静态特征点，并在预定义的静态对象移动时排除来自预定义的静态对象的特征点。这改进了位姿估计和闭环检测，从而提高了 SLAM 算法的性能。在本文中提出了一种基于 ORB-SLAM2 框架的在线算法，该算法可以过滤动态对象并使用静态对象的更多信息来优化相机位姿。对 RGB 图像执行对象检测后，应用受滑动窗口约束的模板匹配来检测动态对象而不是其预定义标签。另外，针对静态对象建立数据关联，用于给系统添加对象级语义约束。对象级语义约束是通过视觉序列中对象的质心距离来实现的。

贡献：

1. ORB-SLAM2 框架中集成了对象检测线程。该系统对RGB图像进行目标检测，利用模板匹配和滑动窗口过滤掉真实的动态特征点，从静态目标中提取语义信息。这样可以准确辨别物体当前的运动状态，避免直接过滤预定义的运动物体。本文的方法由于其耗时短而具有优势。此外，我们的方法甚至能够识别物体的微小运动，例如人的轻微振动。
2. 提出了一种通过结合静态对象的语义约束来优化相机姿势的算法。该算法利用静态对象的语义和几何信息建立对象级数据关联，构建在线对象数据库。该算法充分利用了静态对象的语义信息，并且不需要任何预定义的数据库。

---

在实际应用中，准确的位姿估计和困难条件下的可靠性能是评估机器人自主性的关键因素。 ORB-SLAM2在大多数实际情况下都表现出优异的性能。 ORB-SLAM2 是适用于单目、立体和 RGB-D 相机的完整 SLAM 系统，包括地图重用、闭环和重新定位功能。系统可以在各种环境下在标准CPU上实时工作。因此，DynaTMSLAM是在ORB-SLAM2框架的基础上开发的，通过集成动态物体消除模块和静态物体的语义约束来适应动态场景。 DynaTM-SLAM的整体框架如图1所示。对每个输入帧执行对象检测和 ORB 特征点提取。 YOLOv7是一种比之前版本更先进、更准确、更快速的目标检测算法，可以满足各种应用场景的要求。因此，本文采用 YOLOv7 作为 SLAM 系统的目标检测算法。该算法可以实现每秒30帧以上的检测率，且检测精度较高。然后使用滑动窗口机制和模板匹配算法去除动态特征点。跟踪相机姿态时，会考虑基于相似对象之间质心距离的语义约束。

1. 滑动窗口&模板匹配
   快速准确的物体运动状态检测对于动态环境中 VSLAM 的稳健姿态跟踪至关重要。仅通过观察两个相邻帧之间的差异来准确确定物体的运动状态仍然具有挑战性。本文通过比较一段时间内图像同一区域得相似度，如果区域相似，则该区域中存在得对象被识别为静态，在此基础上，使用模板匹配算法（Template matching techniques in computer vision: theory and practice）来实时估计物体得运动状态。本文采用标准相关系数匹配来最小化光照对计算结果得影响：
   
   其中 I 和 T 分别表示源图像和目标图像，(x, y) 表示像素坐标。
   

DynaTM-SLAM 采用滑动窗口机制，每隔一定时间对图像进行比较，利用模板匹配算法检测物体运动的显着差异。窗口存储固定长度的连续帧。随着帧的进展，新的帧被存储在窗口中，而旧的帧被逐渐删除。图2展示了模板匹配和滑动窗口的结合，用于检测可能移动的物体。滑动窗的头部帧表示为帧i，尾部帧表示为帧j。从第j帧到第i帧的变换矩阵用Tij表示，位姿变化可以通过式（2）计算：

2. 数据关联
   

数据关联是指将当前帧中检测到的对象与地图中已构建的对象进行匹配和关联。这一步非常关键，因为它可以让SLAM算法根据检测结果找到地图中对应的物体，并融合当前帧和地图的信息，从而提高定位和地图构建的准确性和鲁棒性。对象检测模块检测当前帧中的对象，并通过将相应的深度图像与2D边界框融合来生成对象相应的3D点云。由于基于2D边界框提取的3D点云包含背景信息，因此根据密度对3D点云进行聚类以排除背景，然后与数据库中的对象进行匹配。聚类的可视化如图3所示。以监视器为例，从边界框提取的点云对应于黑点云。然后，我们对黑色点云进行基于密度的聚类，并获得更精细的物体点云，它对应于紫色点云。DynaTM-SLAM同时构建对象数据库并进行实时数据关联。使用公式（3）计算物体的质心：

更新数据库有两种情况。当前帧构造的对象称为对象A，而数据库中同一类别的对象称为对象集B。第一种场景，如果数据库中不存在对象集B，则直接将对象A添加到数据库。第二种场景，如果数据库中存在对象集合B，则计算对象A与对象集合B中每个对象的质心距离；如果计算出的最小质心距离小于阈值，则认为对象匹配成功A 和计算最小质心距离的对象。否则，匹配失败，对象A被添加到数据库中。另外，当两个对象匹配成功时，如果对象B的点云数量小于对象A的40%，则对象A将替换数据集中的对象B。这是因为对象A被认为更全面。此外，数据库中在三​​个关键帧中连续观察少于三个的对象也将被消除。

3. 静态物体的捆绑调整
   
   DynaTM-SLAM的BA因子图如图4所示。因子图的变量节点是相机位姿$C=T_{c_i{w}_{i=1}}^M$，静态物体的质心 O = { c p j } j = 1 N O = \{^cp_j \}^N_{j = 1} O={cpj​}j=1N​，以及3D地图点 p = { w p k } k = 1 K p = \{^wp_k \}^K_{k= 1} p={wpk​}k=1K​。变量集用 ψ 表示，可以表示为 ψ ={ C, O, P}。因此，非线性最小二乘优化问题就是最小化3D poi的重投影误差:
   

---

实验

表1显示，DynaTM-SLAM（A+B）在所有序列中的误差都低于DynaTM-SLAM（A）和DynaTM-SLAM（B）。这三种方法的性能都优于ORB-SLAM2。这表明消除动态特征点对于动态场景中准确的自我运动估计至关重要。此外，静态对象提供的强约束使 BA 优化能够产生更好的估计。表中的前四个序列是人们走路的高动态序列，后两个序列是坐在椅子上的人的低动态序列。针对高动态序列，DynaTM-SLAM（A+B）不仅消除了动态特征点，还增加了对静态对象的语义约束，使系统性能得到显著提升。对于低动态序列，ORB-SLAM2本身具有良好的性能，但由于DynaTM-SLAM（A+B）增加了对静态对象的语义约束，因此其性能略好。

图5显示了DynaTM-SLAM（A）、DynaTM-SLAM（B）、DynaTM-SLAM（A+B）和ORB-SLAM2估计的轨迹与fr3_walking_xyz序列上的地面实况的比较。很明显，ORB-SLAM2和DynaTM-SLAM（B）的轨迹明显偏离真实值，而DynaTM-SLAM（A）和 DynaTM-SLAM（A+B）的轨迹接近真实值。DynaTM-SLAM可以有效地过滤动态特征点，并结合静态对象的语义约束，在高动态环境中表现出优异的性能。相反，ORB-SLAM2和DynaTM-SLAM（B）将序列中行走者的特征添加到姿态估计中，这显着影响了系统的定位精度。在下文中，DynaTM-SLAM（A+B）用于与其他算法的比较。


表2、表3和4显示了我们的系统与动态环境中几种最先进的 SLAM方法之间的比较。由于误差值是统一定义的，因此直接使用作者提供的数据。ORB-SLAM2 基于静态假设，并将动态特征点添加到姿态估计中。因此，它在动态环境中表现不佳。我们的方法和其他高级算法都集成了动态特征滤波，与 ORB-SLAM2 相比，使我们能够在动态序列中表现良好。对于第一和第三高动态序列，DynaTM-SLAM的性能更胜一筹，尤其是对于fr3_walking_static序列。由于捕获此序列的相机是静止的，DynaTM-SLAM可以通过提供稳定的对象检测和模板匹配来准确确定物体的运动状态。对于fr3_walking_xyz序列，由于YOLO-SLAM保留了动态对象边界框内的静态点，因此其性能略优于DynaTM-SLAM。然而，在第四个序列fr3_walking_halfsphere中，捕捉此序列的相机在一个直径为一米的小半球上移动。有大的旋转、翻转和其他情况。目前，我们还没有处理较大的旋转情况，因此会对对象检测和模板匹配产生一些影响。在性能方面，DynaTM-SLAM的性能略差于DynaSLAM、DS-SLAM、DRSO-SLAM、RDMO-SLAM和YOLO-SLAM。对于其余的低动态序列，DynaTM-SLAM在ATE指标上的性能略好于其他算法，这主要是因为DynaTM-SLAM在静态对象上增加了语义约束。

实际上，有效且准确地识别动态对象对于提高系统性能至关重要。例如，一个人可以在室内场景中行走或坐着，这意味着预定义的移动对象实际上可能是运动的或静止的。图6显示了区分动态对象的三种不同方法的结果。绿色点表示静态点，而红色点表示动态点。左边的人坐在椅子上一动不动，而右边的人则略微向前移动。图6（a）描述了通过使用深度学习获得的结果。由于深度学习方法基于对对象的预定义假设，因此它可能会错误地将图像中的两个人归类为动态人。图6（b）说明了采用深度约束的方法。然而，由于它依赖于预测深度和实际深度之间的差异来判断，因此对于运动最小的物体可能会发生错误分类，从而导致图像中的两个人都被归类为静态。图6（c）展示了我们使用对象检测和模板匹配的方法，我们的方法对微小的物体运动高度敏感，可以精确地确定物体的实时运动状态。

该算法在现场使用预定义的动态对象进行了测试，这些对象可以是静态的，也可以是动态的。这包括步行和坐着的人的序列。DynaTM-SLAM可以准确地检测和确定它们的状态，无论它们是静止的还是移动的。在图 7 和图8中，静态和动态特征点用绿色和红色方块表示。在图7（a）中，当两个人都处于静止状态时，他们的特征点是绿色的。在图7（b）中，当两个人都处于移动状态时，他们的特征点是红色的。如果左边的人在移动，而右边的人是静止的，左边的人的特征点是绿色的，右边的人的特征点是红色的，如图7（c）所示。

该算法还可以区分移动或保持静止的预定义对象。在fr3_walking_static序列中，存在静止物体被移动的实例，例如椅子被人移动。如图8（a）所示，左边椅子的特征点在移动时变为红色，而右边的椅子保持静止，保留其绿色特征点。在图8（b）中，两把椅子都是静止的，它们的特征点是绿色的。

该算法还可以促进跟踪过程中目标的精确重建和优化。重建物体的相对位置反映了该方法的定位精度。此外，对象重建可用于后续应用，例如路径规划、虚拟增强现实和混合现实。图9说明了SLAM系统运行时构建的静态对象和映射点。图中的红色和黄色点云代表两个显示器，而绿色和蓝色点云代表两个键盘。鼠标由绿色键盘旁边的橙色小点云表示，而书籍由蓝色键盘旁边的棕色小点云表示。准确估计物体的大小和相对位置。

由于ORB-SLAM2 框架中包含了对象检测线程，因此预计对象检测不会显著影响SLAM的时间消耗，并且可以与其他方法相媲美。表5比较了本文的方法与TUM数据集上其他方法的时间消耗。本文的方法已经在两个不同的GPU平台上进行了测试，数据显示，与其他使用语义分割的方法相比，本文的方法需要更少的对象检测时间。因此，与使用语义分割进行动态对象过滤的方法相比，本文的方法具有时间优势。DS-SLAM主要专注于动态对象过滤，从而降低时间消耗。相比之下，DynaSLAM除了动态对象模块外，还引入了一个背景填充模块，导致整体时间消耗相对较高。表中该算法的多视图几何和背景填充对应的时间消耗与fr3_walking_rpy序列相关联。本文的方法引入了两个主要模块：动态对象过滤和静态语义对象定位。与ORB-SLAM2和DS-SLAM 相比，这导致总时间消耗更高，但远低于DynaSLAM和YOLO-SLAM。