VDO-SLAM: A Visual Dynamic Object-aware SLAM System论文笔记

VDO-SLAM: A Visual Dynamic Object-aware SLAM System论文笔记

原文：https://arxiv.org/pdf/2005.11052.pdf
代码：https://github.com/halajun/VDO_SLAM

摘要

本文提出了一种鲁棒的视觉动态物体SLAM系统 VDO-SLAM，该系统利用语义信息实现对场景中动态刚体的精确运动估计和跟踪，而无需对物体的形状或几何模型有任何先验知识。该方法对环境中的动态目标和静态结构进行识别和跟踪，并将这些信息集成到一个统一的SLAM框架中。这样就可以对机器人的轨迹和目标的全SE(3)运动以及环境的时空图进行高度精确的估计。该系统能够从物体的SE(3)运动中提取线速度估计，为复杂动态环境中的导航提供了重要的功能。

方法

A. 背景和符号

1. 坐标系：

   变量名称	数学表达
   时间为k，在全局参考系 0 中机器人/相机和物体的 3D 位姿

2. 点：
   
   
   图2：符号和坐标系。实曲线表示相机和物体在惯性坐标系中的位姿;分别为0X和0L，虚线表示各自在固定自身体坐标系内的运动。实线表示惯性坐标系中的三维点，虚线表示相机坐标系中的3D点。

世界坐标系、物体坐标系和惯性坐标系之间的关系：

相机的位姿变换为实曲线，目标的位姿变换为虚曲线，目标点在对应的物体坐标系红点的虚线矢量表示

B. 相机位姿和目标运动估计

选择用于估计相机位姿和物体运动的成本函数与 3D-2D 重投影误差相关，并在图像平面上定义。基于这个误差项，我们提出了一种新的公式来联合优化光流以及相机姿态和物体运动，以确保对点的鲁棒跟踪。在建图模块中，3D误差成本函数用于全局优化，以确保3D结构和对象运动估计的最佳结果

1）相机位姿估计：

在k-1时刻的global reference frame中观测到的图像Ik中的

2）目标运动估计：

3）基于光流的联合估计：

C. 图优化

所提出的方法将动态 SLAM 表述为图优化问题，以细化相机位姿和物体运动，并构建包括静态和动态结构的全局一致地图。我们将动态 SLAM 问题建模为一个因子图，如图 3 所示。因子图公式非常直观，并且具有允许批量和增量求解器。

具有移动物体的物体感知 SLAM 的因子图表示。

• 黑色方块代表不同时间步长的相机位姿
• 蓝色代表静态点
• 红色代表不同时间步长的对象（虚线框）上的同一动态点
• 绿色代表时间步长之间的对象位姿变化。为了便于可视化，这里只画了一个动态点。
• 先验因子显示为黑色圆圈
• 里程计因子显示为橙色
• 3D点测量因子显示为白色
• 点运动因子显示为洋红色
• 平滑运动因子显示为青色圆圈。

四种类型的测量/观察被整合到一个联合优化问题中； 3D 点测量、里程计测量、动态对象上的点运动和对象平滑运动观察。

用 SE(3) 的李代数参数化 X 和 H（在 (16) 和 (17) 中代入 (8)，在 (18) 和 (19) 中代入 (20)），得到最小二乘的解成本由下式给出：

参数名称	数学表达
3D 点测量噪声协方差矩阵	Σz
里程计噪声协方差矩阵	Σo
运动噪声协方差矩阵	Σg
三元对象运动因子的总数	ng
滑运动协方差矩阵	Σs
平滑运动因子的总数	ns

(21) 中的非线性最小二乘问题是使用 Levenberg-Marquardt 方法解决的。

系统

系统由三个主要部分组成:图像预处理、跟踪和建图。

系统输入为RGB-D图像或双目图像，对于双目图，首先使用双目深度估计方法来提取深度信息，以生成深度图，并将结果数据视为RGB-D。

图4：VDO-SLAM系统概述。首先对输入图像进行预处理，生成实例级目标分割和密集光流。然后，这些特征被用于跟踪静态背景结构和动态对象上的特征。然后，根据特征轨迹估计的摄像机姿势和对象运动在全局批量优化中被细化，并且每一新帧都维护和更新局部地图。该系统输出相机姿势、静态结构、动态对象的轨迹以及它们的姿势随时间变化的估计。

A. 图像预处理

该模块需要完成两个具有挑战性的方面。第一，鲁棒地分离静态背景和对象，第二，确保动态对象的长期跟踪。为了实现这一点，我们利用计算机视觉技术的最新进展，例如语义级分割和密集光流估计，以确保有效的对象运动分割和鲁棒的对象跟踪。

1）对象实例分割：

实例级语义分割用于分割和识别场景中潜在的可移动对象。

2）光流估计：

稠密光流用于最大化移动物体上的跟踪点数量。利用密集光流，通过从语义掩码内的所有点采样，大大增加了目标点的数量。密集光流还用于通过传播分配给对象掩码上的每个点的唯一对象标识符来一致地跟踪多个对象。此外，如果语义分割失败，它允许恢复对象掩码；（一个使用稀疏特征匹配极其困难的任务）

B. 追踪

跟踪组件包括两个模块;

• 相机自运动跟踪包括特征检测和相机位姿估计子模块
• 目标运动跟踪包括动态目标跟踪和目标运动估计子模块。

1）特征检测：

为了实现相机姿态的快速估计，我们检测一组稀疏的角点特征，并利用光流对其进行跟踪。在每一帧，只有符合估计的摄像机运动的内部特征点被保存到地图中，并用于跟踪下一帧中的对应。如果内点跟踪的数量低于某个级别(默认为1200)，则会检测并添加新的特性。这些稀疏特征是在静态背景下检测的，即不包括被分割目标的图像区域。

2）相机位姿估计：

对于所有检测到的 3D-2D 静态点对应关系，使用 (13) 计算相机位姿。为了确保稳健估计，应用运动模型生成方法进行初始化。具体来说，该方法生成两个模型并根据重投影误差比较它们的内点数。一个模型是通过传播相机先前的运动来生成的，而另一个模型是通过使用带有 RANSAC 的 P3P [63] 算法计算新的运动变换来生成的。然后选择生成最多内点的运动模型进行初始化。

3）动态目标跟踪：

首先，分割对象分为静态和动态。

然后我们将动态对象关联到成对的连续帧中。

• 实例级对象分割允许我们将对象与背景分离。尽管该算法能够估计所有分割对象的运动，但动态对象识别有助于降低所提出系统的计算成本。这是基于场景流估计来完成的。具体来说，在获得相机位姿 0 Xk 后，描述帧 k - 1 和 k 之间 3D 点 0mi 的运动的场景流向量 fik 可以计算为 [64]：

  

  与光流不同，场景流（理想情况下仅由场景运动引起）可以直接决定某些结构是否在移动。理想情况下，对于所有静态3D点，场景流向量的大小应该为零。但是，深度和匹配中的噪声或错误会使实际场景中的情况复杂化。为了稳健地处理这个问题，**我们计算每个对象上所有采样点的场景流大小。如果某个点的场景流的幅度大于预定义的阈值，则认为该点是动态的。**在这项工作中进行的所有实验中，这个阈值都设置为0.12。如果“动态”点的比例高于某一水平(点总数的30%)，则对象被识别为动态的，否则被识别为静态的。如上所述，故意选择用于识别对象是否是动态的阈值，以更加保守，因为系统可以灵活地将静态对象建模为动态，并在每个时间步长估计零运动，然而，相反的将降低系统的性能。

• 实例级对象分割只提供单个图像对象标签。然后需要跨帧跟踪对象，它们的运动模型随着时间的推移而传播。我们建议使用光流来关联帧间的点标签。点标签与采样点所处的惟一对象标识符相同。我们保持一个有限跟踪标签集L⊂N，其中L∈L从L = 1开始，用于场景中第一个检测到的运动对象。当检测到更多的移动对象时，L中的元素数量会增加。静态对象和背景标记为l = 0。

  理想情况下，对于第k帧中的每个检测到的物体，其所有点的标签应该与第k−1帧中对应点的标签唯一对齐。然而，在实践中，这受到噪声、图像边界和遮挡的影响。为了克服这个问题，我们给所有的点分配对应的标签。对于一个动态对象，如果前一帧中最频繁的标签为0，这意味着该对象开始移动，出现在场景的边界，或重新出现遮挡。在本例中，对象被分配了一个新的跟踪标签。

4）运动目标估计

如上所述，物体在场景中通常是小块出现的，这使得很难获得足够的稀疏特征来稳健地跟踪和估计它们的运动。我们在对象掩码内每隔3个点采样一次，并在帧间跟踪它们。与摄像机姿态估计类似，只有内部点被保存到地图中，用于下一帧的跟踪。当跟踪对象点的数量下降到一定水平以下时，新的对象点被采样并添加。我们遵循相机初始化运动模型的相同方法来生成初始物体运动模型。

C. 建图

在建图模块中，构造和维护一个全局地图。同时，基于当前时间步长和以前时间步长的窗口，从全局地图中提取局部地图。两个地图都是通过批处理优化过程更新。

（批处理优化：将数据攒起来一起优化）

1）局部批处理优化

我们维护和更新局部地图。局部批量优化的目标是确保精确的相机姿态估计提供给全局批量优化。

局部建图使用一个包含最后nw帧信息的固定大小滑动窗口构建，其中nw为窗口大小，本文将其设为20。局部地图共享一些公共信息;这定义了不同窗口之间的重叠部分。我们选择只在窗口大小内局部优化相机姿态和静态结构

当构建局部地图时，执行因子图优化来细化局部地图中的所有变量，然后将它们更新回全局地图。

2）全局批处理优化

跟踪模块和局部批量优化的输出包括相机位姿、目标运动和内部结构。这些都保存在一个全局地图中，该地图由所有先前的时间步长构成，并随着每个新帧不断更新。在处理完所有输入帧后，基于全局地图构建因子图。

为了有效地探索时间约束，仅将跟踪超过 3 个实例的点添加到因子图中。该图被表述为方法中图优化部分中描述的优化问题。优化结果作为整个系统的输出。

3）从建图到追踪

维护地图为跟踪模块中的当前状态的估计提供历史信息，利用来自最后一帧的内点来跟踪当前帧中的对应关系，并估计相机姿势和对象运动。最后的摄像机和物体运动也可以作为可能的先验模型来初始化当前估计。此外，对象点通过在语义对象分割失败导致的“间接遮挡”的情况下传播其先前分割的掩码来帮助跨帧关联语义掩码以确保对对象的稳健跟踪。