VDO-SLAM: A Visual Dynamic Object-aware SLAM System

1. Abstract

场景刚性假设，也称为静态环境假设，经常被使用到SLAM算法中。如今大部分算法在复杂的动态环境下运行时，会考虑通过删除移动物体或单独跟踪移动物体来简化问题。这种强假设限制了自主移动机器人系统在涉及高度动态和非结构化环境等一系列重要现实场景中应用的发展。

本文介绍了一种鲁棒的动态目标感知SLAM系统（VDO-SLAM），该系统利用语义信息，可以在不具有目标形状或运动模型先验信息的条件下，对场景中的刚性物体进行运动估计。

该方法将环境中的动态和静态结构集成到一个统一的估计框架中，从而实现对于机器人相机和时空地图的精确估计。我们提供了一种从运动目标的位姿变化中提取速度估计值的方法，来实现（机器人在）复杂动态环境下的导航功能。我们在大量的室内和室外真实数据集上演示了该系统的性能。结果显示，与最先进的算法相比，我们的系统实现了持续且实质性的改进。本系统已经开放了源代码。

2. Introduction

用于复杂环境下的高质量三维虚拟模型，在机器人、智能交通、监视、检查、娱乐和电影制作、自然环境的勘探和监测等等一系列领域中的应用需求正在不断增大。创建这些模型需要重复地感知环境，并使用例如同时定位和建图(SLAM)或从运动中恢复结构(SfM)等估计技术将多种传感器测量的结果融合成一致的表示方式。SLAM是一个在机器人领域成熟的技术，它可以同时估计机器人的状态并构建其环境的模型。尽管存在许多精确和高效的解决方案，但目前的SLAM算法在高度动态的环境下依然很容易失效(Cadena et al. (2016))。

SLAM中处理动态物体的传统方法:

1. 将任何与移动物体相关的传感器数据作为异常值处理，并将其从估计过程中去除(Hahnel et al. (2002, 2003);Wolf and Sukhatme (2005); Zhao et al. (2008); Bescoset al. (2018))，
2. 使用传统的多目标跟踪方法检测运动目标并单独跟踪它们(Wang et al. (2003); Miller and Campbell (2007); Rogerset al. (2010); Kundu et al. (2011))。

前者排除了场景中动态物体的信息，只生成静态地图。后者的精度取决于相机的位姿估计，然而在复杂的动态环境中这种方法很容易失效，因为（在这种场景下）都不一定存在可靠的静态结构。随着越来越多的自主系统被应用到动态环境中，作为目前大多数开源SLAM算法和视觉里程计（VO）算法基础的场景刚性假设（或者称为静态环境假设）不断地遭受着挑战。

一个典型的SLAM系统由处理来自传感器原始数据的前端模块，和将获取的信息(原始的和隐式的高级信息)集成到概率估计框架中的后端模块组成。简单原语，例如特征的三维位置等，通常被用来描述环境（信息）。这主要是由于在SLAM估计问题中这些（特征）点很容易被检测、跟踪和集成。其他原语，如线条与平面 (de la Puente and Rodr´ıguez-Losada (2014);Kaess (2015); Henein et al. (2017); Hsiao et al. (2017))甚至是目标 (Mu et al. (2016); Salas-Moreno et al.(2013); Yang and Scherer (2019))都被考虑过用于表达更丰富的地图信息。语义信息和目标分割可以为动态目标的识别提供重要的场景先验信息 (Wang et al. (2007); G´alvez-L´opez et al. (2016))。最新的深度学习技术已经几乎可以实时检测和分割目标类别了(Girshick et al. (2018); He et al. (2017))。尽管基于视觉的目标检测和分割技术在最近有了发展，但视觉SLAM社区还没有充分利用这些信息(Nicholson et al. (2018))。（如果）要将这些信息合并到现有的几何SLAM算法中，场景中每个目标的三维模型数据集必须是可用的(Salas-Moreno et al. (2013);G´alvez-L´opez et al. (2016)) 或者前端必须显式地提供除了检测和分割之外的目标位姿信息 (Milan et al. (2016); Byravan and Fox (2017);Wohlhart and Lepetit (2015)) （这些要求）给这个问题增加了一层复杂性。对三维模型的精确性要求严重限制了它的潜在应用领域，而就我们所知，多目标跟踪和三维位姿估计仍然是一个具有挑战性的技术。这显然需要一种算法，可以利用当前深度学习算法强大的检测和分割能力，而不依赖于额外的位姿估计或目标模型先验。

在本文中，我们提出了VDO-SLAM，一种新颖的基于特征的双目/RGB-D动态SLAM系统，利用基于图像的语义信息来同时定位机器人，绘制静态和动态结构，以及跟踪场景中刚体的运动。总之，本论文的贡献有:

• 一种基于机器人位姿、静态和动态三维点以及目标运动的估计的统一框架的动态场景模型。

• 本算法可以对动态目标的位姿变化 SE(3) 进行精确估计并估算出场景中目标的（移动）速度，效果优于目前最先进的算法。

• 一种利用语义信息跟踪运动目标的鲁棒方法，能够处理由于语义目标分割失败而导致的间接遮挡问题。

• 一个在复杂且有代表性的现实场景下能够被演示证明的系统

具我们所知,这是第一个完整的能够实现运动分割，动态目标追踪，估算跟随着静态和动态结构相机的位姿，估算场景中的每一个刚体的完全SE(3)位姿变化，提取速度信息和可以在现实户外场景中被演示证明的动态SLAM系统。我们证明了我们的算法在真实数据集中的性能，并展示了系统解决刚性目标运动估计的能力。本系统对目标运动和相机位姿估计的准确性都比当前最新算法在城市驾驶场景中（的结果）高出一个数量级的性能。

本文的其余部分结构如下，在下面的第2节中，我们将讨论相关的工作。在第3节和第4节，我们会讲解所提出的算法和系统。我们在第5节介绍实验配置，以及结果和评估。我们将在第6部分进行总结并提出结论。

3. Related Work

4. Methodology

在本节中，我们将展示如何在基于点跟踪的自由模型中建模刚体的运动。我们提出了一种因子图来优化相机和目标运动的估计。

在我们系统的跟踪组件中，如图4所示，被选择的用于估计相机位姿和目标运动的代价函数(见3.2节)与3D-2D重投影误差相关，并在图像上定义。由于噪声在图像平面上可以得到更好的特征，这就为相机定位提供了更准确的结果(Nist´er et al. (2004))。此外，基于这一误差项，我们提出了一种新的公式来联合优化光流以及相机位姿和目标运动，以确保对点的鲁棒跟踪(见3.2.3节)。在建图模块中，我们使用了三维误差代价函数来保证三维结构和目标运动估计的最佳结果，如3.3节所述

4.1 Background and Notation

${}^0{\mathbf{X}}_k,{}^0{\mathbf{L}}_k\in \mathrm{SE}(3)$分别为k时刻、在全局参考系 $o$ 中的机器人/相机和目标的三维位姿，其中$k\in \Gamma$为时间步长的集合。

第 i 个三维点在k时刻的齐次坐标为 ${}^0{\mathbf{m}}_k^i$，其中${}^0{\mathbf{m}}^i={\left[m_x^i,m_y^i,m_z^i,1\right]}^{\top }\in {\mathbb{I}\mathbb{E}}^3$。

4.1.2 object和三维点运动

---

• 公式3
  k-1到k时刻的目标运动可以由齐次变换表示：$${}_{k-1}^{L_{k-1}}{\mathbf{H}}_k={}^0{\mathbf{L}}_{k-1}^{-1}{}^0{\mathbf{L}}_k$$
  其中，${}_{k-1}^{L_{k-1}}{\mathbf{H}}_k\in SE(3)$。该方程表示了k-1时刻的object位姿 经过 一个运动变换到了k时刻的位姿
  

---

在k时刻，对于object上的某个点来说，经过object的位姿，可将该点在坐标系 $o$下的坐标， 变化为对应的object坐标系下的坐标：
$${}_{}^{L_k}{\mathbf{m}}_k^i={}^0{\mathbf{L}}_k^{-1}{}_{}^0{\mathbf{m}}_k^i$$

---

• 公式4
  将上述公式在k时刻的位姿由公式3替换：

$${}^0{\mathbf{m}}_k^i={}^0{\mathbf{L}}_k{}^{L_k}{\mathbf{m}}_k^i={}^0{\mathbf{L}}_{k-1}{}_{k-1}^{L_{k-1}}{\mathbf{H}}_k{}^{L_k}{\mathbf{m}}_k^i$$

---

• 公式5
  对于刚性object来说，object坐标系下的某点的坐标 ${}_{}^{L_k}{\mathbf{m}}_k^i$ 是始终不变的，所以：$${}_{}^{L_k}{\mathbf{m}}_k^i={}^0{\mathbf{L}}_k^{-1}{}_{}^0{\mathbf{m}}_k^i={}^0{\mathbf{L}}_{k-1}^{-1}{}^0{\mathbf{m}}_{k-1}^i$$
  替换一下，得到 坐标系 $o$ 下，object的某点坐标在k-1时刻与k时刻的变换关系：
  $${}^0{\mathbf{m}}_k^i={}^0{\mathbf{L}}_{k-1}{}_{k-1}^{L_{k-1}}{\mathbf{H}}_k{}^0{\mathbf{L}}_{k-1}^{-1}{}^0{\mathbf{m}}_{k-1}^i$$

---

• 公式6
  定义公式5的变换矩阵为$${}_{k-1}^0{\mathbf{H}}_k:={}^0{\mathbf{L}}_{k-1}{}_{k-1}^{L_{k-1}}{\mathbf{H}}_k^0{\mathbf{L}}_{k-1}^{-1}$$
  由此得到公式6：
  $${}^0{\mathbf{m}}_k^i={}_{k-1}^0{\mathbf{H}}_k^0{\mathbf{m}}_{k-1}^i$$
  

方程(6)是我们的运动估计方法的核心，因为它以无模型（model-free）的方式通过驻留在目标上的点表示刚体位姿变化，而不需要在估计中包含目标的三维位姿作为随机变量。第3.2.2节详细介绍了如何根据上述方程估计刚体的位姿变化。方程中的 ${}_{k-1}^0{\mathbf{H}}_k\in SE(3)$ 表示全局参照系中目标（提取出的）点的运动。对于本论文的其余部分，为了便于阅读，我们将这个量称为目标的位姿变化或目标的运动。

4.2 相机位姿和object运动估计

4.2.1相机位姿估计

即利用静态点的最小重投影误差和LM求解相机位姿。

定义 $I_k$ 为与相机在k时刻捕捉到的图片左上角的参考系，就是k时刻的像素坐标系。

通过相机内参$K$将相机坐标系$X_k$下坐标变换为像素坐标系$I_k$下坐标 ${}^{I_k}{\mathbf{p}}_k^i=\left[u^i,v^i,1\right]\in {\mathbb{E}}^2$，即$${}^{I_k}{\mathbf{p}}_k^i=\pi \left({}^{X_k}{\mathbf{m}}_k^i\right)={\mathbf{K}}^{X_k}{\mathbf{m}}_k^i$$

给定 k-1 时刻，全局坐标系下观测到的 一组静态3D点$\left\{{}^0{\mathbf{m}}_{k-1}^i\mid i\in \mathcal{M},k\in \mathcal{T}\right\}$，以及图像 $I_k$ 的2D关联的集合$\left\{{}^{I_k}{\tilde{\mathbf{p}}}_k^i\mid i\in \mathcal{M},k\in \mathcal{T}\right\}$

---

相机位姿 ${}_{}^o{X}_k$ 可以通过最小化重投影误差来估计：
$${\mathbf{e}}_i\left({}^0{\mathbf{X}}_k\right)={}^{I_k}{\tilde{\mathbf{p}}}_k^i-\pi \left({}^0{\mathbf{X}}_k^{-10}{\mathbf{m}}_{k-1}^i\right)$$

即，将 k-1 时刻下，观测到object的i点的 世界坐标系坐标 经过 k时刻下的位姿，变换到k时刻下的相机坐标系坐标 ${}^0{\mathbf{X}}_k^{-10}{\mathbf{m}}_{k-1}^i$，通过内参矩阵获得k时刻下对应的像素坐标 $\pi \left({}^0{\mathbf{X}}_k^{-10}{\mathbf{m}}_{k-1}^i\right)$，将重投影得到的坐标与k时刻真正投影到像素坐标系下的坐标求差，即为误差。

李代数形式：

用huber函数代替最小二乘，便于快速迭代，同时减少了对异常值敏感。

利用LM算法求解即可。

4.2.1object位姿估计

注意，此节的点为动态点；

与相机位姿估计类似，构造基于重投影误差的代价函数可以求解目标运动。使用公式（6），目标的三维点通过重投影对应到图像Ik中二维点的误差项为:

即，先根据 公式6：${}^0{\mathbf{m}}_k^i={}_{k-1}^0{\mathbf{H}}_k^0{\mathbf{m}}_{k-1}^i$ 得到 k时刻的世界坐标系坐标，再经过上节估计的位姿，以及内参，获得重投影像素坐标，与真实坐标作差，作为误差。

同理，利用李代数优化

4.2.3 与光流联合估计

相机位姿估计和目标运动估计都依赖于良好的图像对应。由于遮挡、较大的相对运动和较大的相机-目标距离，对于运动目标上（提取出的）点的跟踪非常具有挑战性。为了保证对于点的跟踪的鲁棒性，本文提出的技术旨在结合运动估计和光流估计进行（全局）细分。

相机或目标运动都会产生光流，它可以由像素在图像帧上的位移向量表示：
$${}^{I_k}{\mathbf{\varphi }}^i={}^{I_k}{\tilde{\mathbf{p}}}_k^i-{}^{I_{k-1}}{\mathbf{p}}_{k-1}^i$$

对于相机的位姿估计，使用上式对相机位姿 ${}_{}^o{X}_k$ 的最小化重投影误差进行重新表述:$${\mathbf{e}}_i\left({}^o{\mathbf{X}}_k\right)={}^{I_k}{\tilde{\mathbf{p}}}_k^i-\pi \left({}^o{\mathbf{X}}_k^{-1}{}^o{m}_{k-1}^i\right)$$

$${\mathbf{e}}_i\left({}^o{\mathbf{X}}_k,{}^{I_k}\mathbf{\varphi }\right)={}^{I_{k-1}}{\mathbf{p}}_{k-1}^i+{}^{I_k}{\mathbf{\varphi }}^i-\pi \left({}^o{\mathbf{X}}_k^{-1}{}_{}^o{\mathbf{m}}_{k-1}^i\right)$$
应用SE(3)元素的李代数参数化，通过最小化代价函数得到最优解

类似地，4.2.1中的目标运动代价函数可以结合光流进行细化：

4.3图优化

该方法将动态SLAM作为一个图优化问题来细化相机位姿和目标运动，并建立包含静态和动态结构的全局一致地图。我们将动态SLAM问题建模为一个因子图，如图3所示。因子图公式具有很强的直观性，它的优点是允许有效的实现批次处理 (Dellaert and Kaess (2006); Agarwal et al. (2012))和增量求解器(Kaess et al. (2011); Polok et al. (2013); Ila et al.(2017))

四种测量/观测被整合到一个联合优化问题中：三维点测量，视觉里程计测量，动态目标上点的运动的观测和目标平滑运动的观测。

带有运动对象的目标感知SLAM的因子图表示。黑色方块表示相机在不同时间步的姿态，蓝色方块表示三个静态点，红色方块表示物体(虚线框)在不同时间步的相同动态点，绿色方块表示物体在时间步之间的姿态变化。为了便于可视化，这里只绘制一个动态点，但是在估计时，将使用检测到的动态对象上的所有点。先验一元因子用黑色圆圈表示，里程二元因子用橙色圆圈表示，点测量二元因子用白色圆圈表示，点运动三元因子用品红圆圈表示。平滑的运动二元因子显示为青色环

---

定义三维点的测量模型误差为:
$${\mathbf{e}}_{i,k}\left({}^0{\mathbf{X}}_k{,}^0{\mathbf{m}}_k^i\right){=}^0{\mathbf{X}}_k^{-1}{}^0{\mathbf{m}}_k^i-{\mathbf{z}}_k^i$$

$\mathbf{z}=\left\{{\mathbf{z}}_k^i\mid i\in \mathcal{M},k\in \mathcal{T}\right\}$是所有时刻步骤的所有三维点测量值的集合，三维点测量因子如图3中的白色圆圈所示。

---

定义视觉里程计模型误差为:

$${\mathbf{e}}_k\left({}^0{\mathbf{X}}_{k-1},{}^0{\mathbf{X}}_k\right)={\left({}^0{\mathbf{X}}_{k-1}^{-1}{}^0{\mathbf{X}}_k\right)}^{-1}{}_{k-1}^{X_{k-1}}{\mathbf{T}}_k$$
其中${}_{k-1}^{X_{k-1}}{\mathbf{T}}_k\in SE(3)$，里程计因子如图3中橙色圆圈所示。

---

动态物体上点的运动模型误差为：
$${\mathbf{e}}_{i,l,k}\left({}^0{\mathbf{m}}_k^i,{}_{k-1}^0{\mathbf{H}}_k^l{,}^0{\mathbf{m}}_{k-1}^i\right)={}^0{\mathbf{m}}_k^i-{}_{k-1}^0{\mathbf{H}}_k^l{}^0{\mathbf{m}}_{k-1}^i$$
被测刚体l 上所有点的运动特征都由式(6)给出的相同位姿变换描述，对应因子是一个三元因子，如图3中洋红色圆圈所示，我们称之为刚体上点的运动模型。

---

研究表明，在动态SLAM中，结合场景中目标运动的先验知识是非常有价值的(Wang et al. (2007);Henein et al.(2020)).由于相机帧率和“控制相对较大目标(车辆)运动的物理定律的影响，并且为了防止它们的运动突然改变，我们引入平滑运动因素，用来最小化连续目标运动的变化，其中的误差项可以定义为:

物体平滑的运动因子用来最小化物体在连续时间步长的运动变化，如图所示为青色圆圈

---

设$\theta ={\theta }_X\cup {\theta }_M\cup {\theta }_H$代表图中所有节点，利用 SE(3)对X和H使用李代数参数化(将(8)代入(16)和(17)，将(20)代入(18)和(19)，得出最小二乘代价的解为:

Σz是3d点测量噪声协方差矩阵，Σo是测程法噪声协方差矩阵，ΣΣg为运动噪声协方差矩阵，其中运动噪声协方差矩阵为三值目标运动面总数，Σs为平滑运动协方差矩阵，ns为平滑运动总数，(21)中的非线性最小二乘问题采用Levenberg-Marquardt方法求解

5. 系统

在本节中，我们提出了一种新的目标感知动态SLAM系统，该系统可以稳健地估计相机和目标的运动，以及环境的静态和动态结构。整个系统概述如图4所示。该系统由图像预处理、跟踪和建图三个主要部分组成。

系统的输入是双目图像或RGB-D图像。对于双目图像，首先采用 Yamaguchi et al. (2014)描述的双目深度估计方法提取深度信息，生成深度地图，所得数据被处理为RGB-D形式。

虽然本系统最初设计为RGB-D系统，但为了充分利用基于图像的语义信息，我们采用单图像深度估计来获取单目相机的深度信息。我们基于学习的单目系统是单目的，因为只使用RGB图像作为系统的输入，但是估计问题是使用RGB- d数据来表示的，其中深度是使用单图像深度估计获得的.

5.1预处理

这个模块需要满足两个挑战问题。一是将静态背景与（动态）目标实现鲁棒地分离，二是保证对于动态目标的长期跟踪。为了实现这一目标，我们利用了计算机视觉的最新技术，例如水平语义分割和密集光流估计，以确保高效的目标运动分割和鲁棒的目标跟踪要求。

5.1.1目标实例分割

使用实例级语义分割对场景中潜在的可移动位姿进行分割和识别。语义信息是区分静止和运动目标点的重要先验，例如建筑物和道路总是静态的，而汽车可以是静态的，也可以是动态的。

实例分割有助于将语义前景进一步划分为不同的实例mask，从而更容易跟踪每个单独的位姿。此外，分割mask提供了一个“精确”的目标边界，以确保对目标上的点的鲁棒跟踪。

5.1.2光流估算

利用密集光流可以最大化跟踪运动目标上点的数量。大多数运动目标只占图像的一小部分。因此，使用稀疏特征匹配不能保证鲁棒性和长期的特征跟踪。

我们的方法利用了密集光流，通过对语义mask内的所有点进行采样来显著增加（对于运动）目标（提取出的）点的数量。密集光流还可以通过分配给目标mask每个点的唯一标识符来持续地跟踪多个位姿。在语义分割失败的情况下，（DVO-SLAM）可以恢复目标mask;而使用稀疏特征匹配很难实现这种任务。

5.2 跟踪

跟踪组件包括两个模块；相机自身运动跟踪包括特征检测和相机位姿估计子模块，目标运动跟踪包括动态目标跟踪和目标运动估计子模块。

5.2.1特征检测

为了实现快速的相机位姿估计，我们检测一组稀疏的角点特征并利用光流对其进行跟踪。在每一帧中，只有拟合相机运动估计的内点才被保存到地图中，用于跟踪下一帧的对应（点）。如果跟踪到的内点的数量低于某一阈值，系统就会检测并添加新的特征。在静态背景下检测到的稀疏特征中，图像区域不包括被分割的位姿。

5.2.2相机位姿估计

对于所有检测到的3D-2D对应静态点，DVO-SLAM使用公式(13)可以计算得到相机位姿。

为了保证估计的鲁棒性，系统采用运动模型生成方法对数据进行初始化。具体说来，该方法基于重投影误差生成了两个模型，并对两个模型的内值进行了比较。一个模型是通过计算相机之前的运动生成的，而另一个模型是通过使用RANSAC的P3P ((Ke and Roumeliotis (2017))算法计算新的运动变换生成的。系统会选择产生较多内点的运动模型进行初始化操作。

5.2.3动态目标跟踪

运动目标的跟踪过程分为两个步骤。第一步将分割的目标分为静态目标和动态目标。第二步通过连续的帧来关联动态目标。

实例级目标分割允许我们从背景中分离目标。虽然算法有能力对所有的分割目标估计其运动，动态目标的有效识别可以有助于降低系统的计算成本。这是基于场景流估计实现的。具体来说，在获得相机位姿后，描述三维点在k-1到k帧之间运动的场景流向量可以被计算得到：

计算空间内某一点m在k、k-1帧时刻位置偏差，得到场景流（scene flow）如果该点的场景流变化大于预先设定的阈值（文中取0.12），则认定该点为动态点。在语义分割后的掩码范围内，计算每个物体场景流的动态点占比是否大于某一阈值（文中取30%），如果大于则判定该物体掩码对应的物体为动态物体。

与光流法不同，场景流(理想情况下仅由场景移动引起)可以直接决定某些结构是否有过移动。理想情况下，对应于所有静态三维点的场景流向量的大小应该为零。然而，在深度和匹配方面的噪声或错误会使真实场景中的情况复杂化。为了稳健地处理这个问题，我们会对每个目标上的所有样本点计算它们的场景流大小。如果场景中某些点的场景流数值大于预先设定的阈值，则认为该点是运动的。在本系统进行的所有实验中，这个阈值都被设置为0.12。如果“动态”点的比例高于某一水平(所有点数的30%)，则该目标会被识别为是运动的，否则该目标为静止的。如前文所述，（设置）阈值是用于识别一个位姿是否是动态的，为了（使结果）更加保守，系统可以灵活地将一个静态位姿建模为动态位姿，并每个时间步长对其估计出一个（大小为）零的运动。然而，如果进行相反的处理，则会降低系统的性能。

实例级别的语义分割仅仅能够提供单阶段（单张图片）的分割结果，但是没法实现追踪，因此需要引入稠密光流进行数据关联。使用光流为每个运动物体进行编号，对受到噪声，图像边界及遮蔽影响的运动物体，当其上一帧周边的点场景流运动标签为0时，重新对其编号。

5.2.4目标的运动估计

在实现动静态区分后，实现动态目标的运动估计也是和相机位姿估计类似，利用一些特征点来实现，并通过光流来实现帧间的跟踪以及构成correspondence的获取。

如上文所述，（运动）目标在场景中通常是小范围出现的，很难得到足够的稀疏特征来稳健地跟踪和估计目标的运动。我们对所有位姿mask内的第三个点进行了采样，并在图像帧之间对它们进行跟踪。与相机位姿估计相似，只有内点会被保存进地图中并用于下一帧的跟踪。当跟踪的目标点减少到一定水平以下时，会对目标进行采样并添加新的目标点。我们采用了4.2.2节中讨论的方法来生成初始目标运动模型。

5.3 地图

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

5.3.1局部批量优化

我们的系统会维护和更新一个局部地图。局部批量优化的目标是确保将精确的相机位姿估计提供给全局批量优化。相机位姿估计对目标运动估计的精度和算法的整体性能有很大的影响。局部地图是使用一个固定大小的包含最后一帧信息的滑动窗口构建的。局部地图会共享一些公共信息:这会造成不同窗口之间的重叠。我们选择只在窗口大小范围内局部优化相机位姿和静态结构，因为局部优化动态结构不会给优化带来任何好处，除非在窗口内假设存在硬约束(例如目标的恒定运动)。但是，如果需要的话，系统能够在局部地图中合并静态和动态结构。当一个局部地图以类似的方式构造时，因子图优化将执行来细化局部地图中的所有变量，然后将它们在全局地图中进行更新。

5.3.2全局批量优化

跟踪模块的输出和局部批量优化包含了相机位姿、目标运动和内点结构。它们被保存在一个全局地图中，这个全局地图是由之前的所有时间帧构建的，并且随着每一个新的时间帧的加入不断更新。在处理完所有输入帧后，系统会基于全局地图构造因子图。为了有效地获得时间约束，系统在因子图中只添加被跟踪超过3个实例的点。该图被表述为如3.3节中描述的优化问题。优化结果将作为整个系统的输出。

5.3.3从地图到跟踪:

维护地图可以为跟踪模块估算当前状态时提供历史信息，如图4所示，蓝色箭头从全局地图指向系统跟踪模块的多个组件。利用最后一帧的内点可以跟踪当前帧中的对应点并估计相机位姿和目标运动。最后的相机和目标运动也可能作为先验模型来初始化4.2.2和4.2.4节中所述的当前估计问题。此外，在语义位姿分割失败导致“间接遮挡”的情况下，通过利用之前分割的mask，（运动）目标（上提取出的）点可以帮助（系统实现）跨帧关联语义mask，以确保对目标的鲁棒跟踪。

6. 实验

我们从摄像机运动、目标运动和速度以及目标跟踪性能方面评价我们提出的方法。对室内的牛津多运动数据集(Judd and Gammell(2019))和室外场景的KITTI跟踪数据集(Geiger等人(2013))进行了评估。我们还将我们的结果与两种最先进的方法，MVO (Judd et al.(2018))和CubeSLAM (Yang and Scherer(2019))进行了比较，以证明VDO-SLAM的更好性能。

我们采用基于学习的实例级位姿分割Mask R-CNN (He et al.(2017))来生成位姿分割mask。该方法的模型在COCO 数据集上训练(Lin et al.(2014))，可以直接用于本工作中，不需要进行任何微调。对于密集光流，我们利用了最先进的方法PWC-Net (Sun et al.(2018))（进行测试）。该模型在FlyingChairs数据集上进行训练(Mayer et al. (2016))，然后在Sintel上进行微调(Butler et al.(2012)和在KITTI数据集上进行训练(Geiger et al.， 2012)。为了在我们系统的“单目”版本生成深度地图，我们采用了一种基于学习的单目深度估计方法MonoDepth2 (Godard et al.(2019)).。在排除本文测试数据的情况下，对该模型进行了深度特征分割(Eigen et al.(2014))训练。特征检测使用FAST完成(Rosten and Drummond（2006））( Rublee et al. (2011)).

https://www.cnblogs.com/CV-life/p/13644001.html

https://blog.csdn.net/electech6/article/details/108506856

https://zhuanlan.zhihu.com/p/163816831