一种适用于动态场景的多层次地图构建算法

作者：Xinggang Hu

在动态场景中视觉SLAM中的定位和地图构建面临着巨大的挑战。近年来，许多优秀的研究作品已经提出了有效的定位问题解决方案。然而，关于在动态场景中构建长期一致地图的优秀作品相对较少，这严重阻碍了地图应用的发展。为了解决这个问题，我们设计了一个针对动态场景的多层次地图构建系统。在这个系统中采用了多目标跟踪算法、DBSCAN聚类算法和深度信息来矫正目标检测的结果，准确提取静态点云，构建密集的点云地图和八叉树地图。我们提出了一个专门针对动态场景的平面地图构建算法，涉及动态环境中平面的提取、过滤、数据关联和融合优化，从而创建一个平面地图。此外引入了一个专门针对动态场景的物体地图构建算法，包括物体参数化、数据关联和更新优化。对公共数据集和实际场景的大量实验验证了本研究中构建的多层次地图的准确性，以及所提出算法的鲁棒性。此外，通过利用构建的物体地图进行动态物体跟踪，我们展示了算法的实际应用前景。

主要贡献

本文提出了一种适用于动态场景的多层次地图构建算法，如图1所示的系统框架。首先，利用YOLOX[8]获取场景的语义信息，采用多目标跟踪算法对漏检进行补偿，利用DBSCAN密度聚类算法和深度信息进一步优化潜在移动物体的检测边界框。随后，我们提取点云和平面，并利用主成分分析（PCA）和最小外接矩形对物体进行参数化。此外，对点云、平面和物体进行过滤。接下来，基于我们之前的研究[9]提供的相机位姿，我们进行点云拼接和融合，对平面和物体进行数据关联和更新优化，随后，将稠密点云地图转换为八叉树地图。最终实现一个多层次地图的构建，包括稠密点云地图、八叉树地图、平面地图和物体地图，从而丰富了地图的应用场景。图1显示了用于动态场景的多层次地图构建算法的系统框架，通过在公开可用的数据集和实际场景中进行的实验，充分验证了我们算法的有效性。

图1. 适用于动态场景的多层次地图构建算法的系统框架，浅绿色部分是输入模块，负责输入RGB图像和深度图像。深绿色部分是预处理模块，主要负责获取和预处理语义信息。黄色、蓝色和棕色模块是地图构建模块，分别代表了构建稠密点云地图和八叉树地图、平面地图、物体地图的一般过程。紫红色部分是输出模块，负责输出地图构建模块构建的多层次地图。

本文的贡献总结如下： 

基于矫正后的物体检测结果对点云进行过滤，构建了仅包含静态元素的纯净点云地图和八叉树地图。

提出了一种在动态场景中构建平面地图的方法，实现了对环境结构的感知。 

提出了一种在动态场景中构建物体地图的方法，使SLAM能够满足更高级的需求，如机器人环境理解、物体操纵和语义增强现实。 

就我们所知，在动态场景中，本文是第一篇构建平面地图的工作，也是第一篇准确参数化物体并构建准确完整的轻量级物体地图的工作。

内容概述

几何地图的构建 

A. 稠密点云地图和八叉树地图的构建 

在存在语义先验信息的情况下，可以根据语义类别删除目标检测框或语义掩码中的点云，从而构建仅包含静态因素的稠密点云地图。然而，仅依赖于原始的语义结果，语义信息的“漏检”和“欠分割”问题可能导致动态对象的不完整去除。本文使用YOLOX进行语义信息获取以解决这个问题，为了解决“漏检”问题，本文利用多目标跟踪算法进行漏检补偿，为了解决“欠分割”问题，首先采用DBSCAN聚类算法在潜在移动对象的边界框内提取前景点。随后，基于邻近像素沿着检测框边界和前景点的深度信息，适当扩展检测框。为了避免DBSCAN聚类引起的错误，我们将检测框的所有四个方向设置为扩展限制，限制为50个像素。在关键帧中，提取并映射到3D世界坐标系中的潜在移动对象的校正边界框外的像素。然后，基于我们先前研究提供的相机姿态，将不同关键帧提取的点云进行拼接和融合，随后通过体素网格滤波进行下采样，为了提高存储效率并支持导航和避障等任务，点云地图转换为八叉树地图。

B. 平面地图的构建 

使用PEAC算法 [30] 用于平面提取，获取当前相机坐标系中平面的参数和点云，然后提取平面的边缘点。随后，利用PCL点云库对平面进行二次拟合，获取精化后的参数和内点，接着移除平面边缘点的异常值，在此过程中，根据深度信息、内点比例以及与目标检测框的位置关系等各种因素对平面进行过滤。完成平面地图初始化后，遍历当前帧中检测到的平面和地图中已存在的平面，建立数据关联。然而，在复杂的动态场景中，检测到的平面常常会出现显著的错误和随机性，导致平面数据关联失败。随着更多的观测，未成功关联的两个平面将朝着正确的方向进行优化，使得后续关联变得更加容易。因此，在局部地图构建线程中，对地图中的平面进行成对比较。如果两个平面满足上述关联条件，它们将被视为潜在的未关联。然后，将观测次数较少的平面合并到观测次数较多的平面中并进行优化,随后，将观测次数较少的平面从地图中移除。

构建对象地图 

A. 对象参数化和数据关联

由于要建模的对象通常属于背景，且远离相机，提取的地图点通常数量稀少且质量较差，使用聚类算法进行异常值去除是不可行的。因此每帧中使用密集点云进行对象建模，并利用DBSCAN密度聚类算法处理点云。在当前帧k中，对于每个检测到的实例，我们对地图中的每个对象实例进行关联判断。运动IoU，投影IoU，3D-IoU和非参数统计是常见的对象数据关联策略。尽管它们存在限制，但在集成时，这些策略可以相互补充，从而产生更强大、准确和多功能的对象数据关联算法。

图2. 地图点的异常值剔除。（a）确定桌面平面。（b）根据点到平面的距离剔除异常值。（c）使用隔离森林算法剔除异常值。

B 对象的更新与优化 

我们分别使用密集点云和稀疏地图点来对检测实例和对象实例进行参数化，这种方法弥补了单个帧中地图点不足的缺点和多个帧中密集点云的显著时间消耗。在成功的数据关联之后，地图点和参数将会更新，随后，利用对象的地图点与平面或与对象关联的平面之间的距离以及孤立森林算法，从这些地图点中去除异常值，如图2所示。

实验

我们在TUM RGB-D数据集上评估了我们的算法性能，并在实际场景中应用算法进行动态物体跟踪。本研究的主要重点是地图构建，由于测试序列没有提供真值地图，实验主要旨在定性地展示地图构建结果。我们的算法在一台搭载i9-12900H CPU、3060 GPU和16GB内存的笔记本电脑上运行。

几何地图的构建 

稠密点云地图和八叉树地图的构建结果如图3所示。可以观察到，由于缺乏处理动态物体的模块，ORB-SLAM2算法无法在高动态场景中进行定位和地图构建。在低动态场景中，该算法保留了动态物体的点云。由于物体检测中的漏检和完全覆盖潜在运动物体的边界框的挑战，通过删除位于原始潜在运动物体检测边界框内的点云构建的稠密点云地图包含了这些物体的大量残留痕迹。

图3. 点云地图和八叉树地图。顶部一行显示了使用ORB-SLAM2算法和密集建图模块构建的稠密点云地图。第二行展示了使用先前研究的方法 [9] 作为定位模块构建的稠密点云地图，其中排除了位于潜在运动物体检测区域内的点云。第三行展示了我们算法构建的稠密点云地图。底部一行展示了我们算法生成的八叉树地图。

图4中构建的平面地图在动态场景中准确地感知静态背景平面结构。这可以应用于高级场景，如增强现实，并作为地标来增强相机姿态估计的准确性。

图4：相邻放置的重复物体的生成地图，左侧的图像提供了序列的概览。

物体地图的构建 

我们在TUM数据集的8个动态序列上评估了物体地图构建的性能，如图5所示。为了验证物体地图构建的准确性，我们将构建的物体模型叠加在稠密地图上，并将它们投影到图像平面上。在高动态场景中，我们的算法能够准确地对场景中的几乎所有物体进行建模，不受相机不同的运动模式和环境中动态物体的影响。然而，在低动态场景中，两个人一直坐在桌子旁边，导致了静态物体和背景的严重遮挡。因此，我们的算法对于某些物体缺乏足够的观测，导致了一些物体的建模不够准确，这是不可避免的。实验结果表明，我们的算法在物体参数化、物体数据关联和物体优化策略方面具有很强的有效性，通过克服动态物体的影响，构建的物体地图为后续的应用，如语义导航、物体抓取和增强现实，提供了有力的支持。

图5. 物体地图。常规形状的物体，如显示器、书籍和键盘，使用立方体表示，而不规则形状的物体，如椅子、瓶子和泰迪熊，使用二次曲面表示。

在真实环境中的稳健性测试 

我们还使用Realsense D435i相机在真实世界场景中对我们的方法进行了测试，以验证其有效性和稳健性。在实验中，一个人在相机的视野范围内进行了不规则的运动。为了验证算法的稳健性，我们评估了两种相机运动方式：1）从场景的一端到另一端的移动；2）几乎静止。多级地图构建的结果如图6所示。实验结果表明，我们的算法能够在物体和相机的不同运动状态下构建准确的稠密点云地图、八叉树地图、平面地图和轻量级物体地图。

图6. 真实世界场景中的多级地图构建结果，在上面一组图像中，相机从场景的一端移动到另一端；在下面一组图像中，相机几乎保持静止。图像(a)、(b)和(c)分别代表稠密点云地图、八叉树地图和平面地图。图像(d)展示了轻量级物体地图，其中物体被叠加在稠密点云地图上（图像(e)），并投影到图像上（图像(f)），以展示物体地图构建的有效性。

动态物体跟踪实验 

我们进一步将构建的物体地图应用于动态物体跟踪，使用Pico Neo3设备捕获场景图像，并利用我们的算法构建物体地图。在这种情况下，通过立体匹配获得地图点的深度信息，这些计算仅在关键帧上进行，以确保实时性能。构建的物体地图如图7(a)所示。一旦构建了物体地图，用户可以选择目标物体进行跟踪。当用户移动物体时，系统使用KCF单物体跟踪和光流跟踪算法计算物体的实时姿态。图7(b)-(d)展示了书、键盘和瓶子的动态跟踪结果。实验结果表明，我们的算法能够在动态环境中准确建模物体，为物体跟踪提供准确的物体模型和姿态，从而对实际应用非常有价值。此外，这也突显出我们的算法不依赖于特定设备，展示了其稳健性和通用性。

图7. 实际场景中的物体建模和动态跟踪。

总结

本文提出了一个针对动态场景量身定制的多层地图构建算法。我们成功地构建了包含静态背景和物体的稠密点云地图、八叉树地图、平面地图和物体地图，能够在动态干扰存在的情况下实现，这丰富了移动机器人的环境感知能力，并扩展了在动态环境中构建地图的应用场景。广泛的实验证明了我们算法的准确性和稳健性，而动态物体跟踪实验进一步证实了其实用性，在未来，我们计划考虑除人类以外的其他可移动物体的真实运动，并利用平面和物体作为地标来优化相机姿态，进一步提高定位精度。

编辑：黄飞