DynaSLAM2:紧耦合的多目标追踪和SLAM

摘要 - 场景刚度的假设在视觉SLAM算法中很常见。但是，它限制了它们在人口稠密的现实环境中的适用性。此外，大多数智力包括自动驾驶，多机器人协作和增强/虚拟现实，都需要对周围环境进行明确的运动信息，以帮助决策和场景理解。我们在本文中介绍了DynaSLAM II，一个用于立体和RGB-D配置的视觉SLAM系统，它紧耦合了多目标跟踪功能。
DynaSLAM II利用实例语义分割和ORB特性来跟踪动态对象。在一种新颖的束调整方案中，结合摄像机和移动主体的轨迹对静态场景和动态对象的结构进行优化。在一个固定的时间窗口内对物体的三维边界框进行估计和松散优化。我们证明，跟踪动态物体不仅为场景理解提供了丰富的线索，而且有利于相机跟踪。
验收合格后发布项目代码。
索引术语SLAM，语义，跟踪，动态对象

1.引言

视觉同时定位和映射（SLAM）是创建未知环境的地图并在此地图中估算机器人姿势的问题，仅从其板载摄像机的数据流中。大多数SLAM方法都假设一个静态场景，只能通过将它们标记为这种静态模型的离群值[1] - [3]来处理动态内容的小部分。尽管静态前提适用于某些机器人应用，但它限制了其在人群中用于自动驾驶，服务机器人或AR/VR的情况。近年来，动态目标的处理问题受到了广泛的关注。大部分文献通过检测观察场景内的移动区域并在SLAM问题中拒绝这些区域来解决这个问题[4][7]。一些作品通过将显示动态内容的图像转化成只有静态内容的逼真图像来处理本地化管道之外的图像流[8]，[9]。另一方面，机器人社区的一个小但不断增长的部分已经通过将移动物体的动力学纳入问题来解决这个问题[10][13]。前两组主要侧重于从静态场景中实现准确的自我运动估计，后一组的目标是双重的:他们不仅解决SLAM问题，还提供其他动态主体的位姿信息。
对于VR/AR或自主系统导航中新兴应用程序的前沿要求，了解周围的动态对象至关重要。尽管在准静态环境中排除次要动作是可以忍受的，但大多数场景在内，包括自动驾驶，多机器人协作和AR/VR都需要周围环境的明确运动信息，以帮助决策和场景理解。例如，在虚拟现实中，需要明确地跟踪动态对象，以允许虚拟对象与现实世界中的移动实例进行交互。在自动驾驶场景中，汽车不仅要定位自己，还必须可靠地感知其他车辆和行人，以避免碰撞。
绝大多数专门解决这一问题的文献都是通过使用传统的多目标跟踪方法来检测运动物体并将其与SLAM公式分开跟踪[14][18]。它们的准确性高度取决于相机位姿估计，这更容易引起在不保证可靠静态结构存在的复杂动态环境中的失败。这些系统通常是为特殊用例量身定制的，并且利用多个先验来约束空间解决方案:道路平面结构和驾驶场景中的平面物体运动，甚至使用物体3D模型。
在这一点上，我们借此机会介绍DynaSLAM II。DynaSLAM II是一个开源的立体和RGB-D SLAM系统，用于动态环境，同时估计相机的姿势，地图和场景移动物体的轨迹。我们提出了一种紧耦合优化场景结构、摄像机姿态和局部时间窗口内物体轨迹的束调整方案。对象的边界框也在解耦公式中进行了优化，该公式允许估计对象的尺寸和6自由度位姿，而无需针对任何特定用例。我们对KITTI数据集[19]进行了详尽的评估和比较，验证了我们的建议。我们的系统输出的一个例子可以在图1中看到.

2.相关工作

解决三维多目标跟踪的传统方法是将运动目标从SLAM公式中分离出来进行检测和跟踪[14][18]。其中Wang等人[14]推导出了运动目标跟踪SLAM的贝叶斯公式，为理解和解决这一问题提供了坚实的基础。Wangsiripi- tak等人[15]提出了用三维目标跟踪器并行实现SLAM: SLAM为跟踪器提供注册地图对象的信息，跟踪器允许在对象上标记特征。Rogers等人[16]将EM技术应用于基于图形的SLAM方法，并允许地标是动态的。最近，Barsan等人[17]提出了一种基于立体的城市环境密集建图算法，该算法可以同时重建静态背景和运动物体。Rosinol等人[18]的一项新工作将视觉惯性SLAM和密集网格跟踪协调起来，主要关注人类，在模拟中显示出令人印象深刻的结果。这些方法的主要缺点是它们的精度与相机位置的估计高度相关。也就是说，如果相机姿态估计失败，这在复杂的动态环境中很有可能发生多目标跟踪也会直接失败。
同时估计摄像机运动和多运动物体运动的思想起源于SLAM- MOT工作[20]。他们建立了一个数学框架来整合基于滤波的SLAM和运动目标跟踪，并证明它同时满足自动驾驶的导航和安全要求。随后，使用RGB-D相机的作品遵循这一思路，使用逐像素实例分割密集地重建静态室内场景以及运动物体，结果令人印象深刻[21][23]。由于获得高精度的分割蒙版对于密集方法至关重要，mask - fusion[22]和MID-Fusion[23]的工作通过假设人造物体大部分是凸的来改进它。
在基于特征的方法中，像我们的方法一样，很少有目标将来自静态和动态对象的信息合并到单个框架中以提高估计精度。Henein等人[24]是最早将动态物体跟踪问题与相机自我运动紧密结合起来的人之一。然而，他们只报告了合成数据的实验，显示出有限的实际结果。Li等人[10]使用端到端训练的CNN来估计汽车的3D姿态和尺寸，并与相机姿态一起进一步细化。使用数据驱动的方法通常在6 DoF物体姿态估计中提供出色的精度，但也失去了一般性，因此，它们只能跟踪汽车。用它们的方法跟踪通用对象需要大量的数据。CubeSLAM[11]的作者利用基于二维边界框和消失点的三维边界框建议生成方法，仅用单目摄像机就取得了令人印象深刻的结果。他们假设物体在一个硬编码的持续时间间隔内具有恒定的速度，并利用物体先验，如汽车尺寸，道路结构和平面非完整物体车轮运动模型。此外，它们仅跟踪可观察到三维边界框的对象，即只能看到长方体形状对象的两个或多个面。ClusterSLAM[25]提出了一个没有场景先验的SLAM后端来发现单个刚体并计算其在动态环境中的运动。由于它作为一个后端而不是一个完整的系统，它的性能在很大程度上依赖于地标跟踪和关联质量。同样的作者最近开发了完整的系统ClusterVO[12]，它用对象所属的概率对对象点进行建模，以处理分割不准确。在没有先验假设的情况下，在室内和室外场景下都能得到很好的跟踪结果，但对三维边界框的估计不准确。VDO-SLAM[13]是最近的一项工作，它使用密集光流来最大化运动物体上的跟踪点数量。他们使用相机、物体和点执行捆绑调整，虽然效果不错，但在实时运行时计算复杂。
鉴于这些进展，很明显，基于特征的SLAM社区正在寻找最佳的优化配方来结合相机，物体和结构点。在我们的提案中，我们使用紧密耦合的BA公式，并在相机，对象和点之间进行新的测量，从而特别注意其计算复杂性和参数的数量，而无需引入硬编码的先验。为此，我们将实例语义先验与稀疏图像特征相结合。这个公式允许相机，地图结构和动态对象的估计在一个低的计算成本是互利的。另一方面，目前的部分文献关注于动态对象的点云结构和随机对象参考的轨迹估计[12]，[13]，[25]，而另一部分文献则试图找到同类对象的共同参考以及更有信息量的占用量[10]，[11]。我们打算独立执行这两项任务，以便充分利用两者的优点，而不受其缺点的影响。

3.方法

Dynaslam II建立在流行的Orb-Slam2 [1]基础上。它将同步和校准的立体声/RGB-D图像作为输入，并为每个帧输出摄像机和动态对象，以及包含动态对象的空间/时间地图。对于每个输入帧，计算逐像素语义分割，提取ORB特征[26]并跨立体图像对进行匹配。我们首先将静态和动态特征与前一帧和地图中的特征联系起来，假设相机和观察到的物体都是匀速运动。然后根据动态特征对应匹配对象实例。静态匹配用于估计初始相机姿态，动态匹配产生目标的SE(3)变换。最后，相机和物体轨迹，以及物体边界框和3D点在滑动窗口上进行优化，并进行边缘化和平滑运动先验。以下小节将解释DynaSLAM II的不同贡献和构建模块。

A.符号

我们将使用以下符号：一个立体/RGBD相机i 在世界坐标系W和t时刻，位姿为$T_{cw}^i\in SE(3)$，相机i的观测1）静态3D地图点$x_w^l\in R^3$
，2）位姿为$T_{wo}^{k,i}\in SE(3)$的动态点，和i时刻的线速度角速度$v_i^k,w_i^k\in R^3$，都用物体坐标系表示，每个观测对象k包含动态对象点$x_o^{j,k}\in R^3$。

B.对象数据关联

对于每个输入帧，计算逐像素语义分割，提取ORB特征[26]并在立体图像对之间进行匹配。如果一个实例属于动态类(车辆、行人和动物)，并且包含大量新的附近关键点，则创建一个对象。然后将关键点分配给实例和相应的对象。我们首先将静态特征与前一帧和地图中的特征相关联，以初步估计相机姿势。接下来，动态特征以两种不同的方式与局部地图中的动态点相关联：(a)如果地图物体的速度已知，假设帧间匀速运动，通过重投影搜索匹配（b)如果对象的速度没有初始化或在(a)之后没有找到足够的匹配，我们将暴力匹配约束到那些属于连续帧中重叠最多的实例的特征。请注意，我们的框架可以处理遮挡，因为当前的关键点与地图对象匹配，而不是与先前的帧对象相匹配。还需要实例和对象之间的更高级别的关联。如果分配给新对象的大多数关键点与属于一个地图对象的点相匹配，则这两个对象被赋予相同的跟踪id。此外，为了使这种高级关联更加健壮，基于CNN实例2D边界框的IoU(交集/联合)执行并行的实例到实例匹配。轨迹的第一个对象的SE(3)位姿初始化为三维点的质心并进行身份旋转。为了根据轨迹预测更远物体的姿态，我们使用等速运动模型，并通过最小化重投影误差来改进目标姿态估计。
对于参考系W下的位姿为$T_{cw}^i\in SE(3)$的相机i，和一个齐次坐标${\overline{\mathbf{x}}}_{\mathrm{W}}^l\in {\mathbb{R}}^4$ 的3D地图点$l$，与之匹配的双目关键点${\mathbf{u}}_i^l=\left[u,v,u_R\right]\in {\mathbb{R}}^3$，多视点几何问题中重投影误差的常用公式为

其中${\pi }_i$是对校准的立体声/RGB-D摄像机的重新投影函数，将一个相机系下的3D齐次点投影到相机帧像素。与此公式不同，它对静态表示有效，我们建议将重投影误差重申为

其中$T_{wo}^{k,i}\in SE(3)$是相机i观测到物体k在世界坐标系下的位姿的逆，${\overline{\mathbf{x}}}_{\mathrm{o}}^{j,k}\in {\mathbb{R}}^4$ 代表点$j$在物体参考k在相机$i,u_i^j\in R^3$观测的3D齐次坐标。这个公式使我们能够共同优化相机和不同运动物体的姿势，以及它们的3D点的位置。

C.物体中心表示

考虑到额外的复杂性，主要是跟踪运动物体的任务在SLAM的基础上隐含的额外的参数数量，尽可能减少这些参数数量以保持实时性能是非常重要的。在通常的动态SLAM实现中，通过形成独立的点云将动态点建模为重复的3D点，这会导致大量的参数。给定一组$N_c$个相机，每个相机均观察到$N_o$个对象的$N_{op}$个3D点，跟踪动态目标所需要的参数为$N=6N_c+N_c\times N_o\times 3N_{op}$，与传统SLAM的$N=6N_c+N_o\times 3N_{op}$不同。对于长时间的操作和部署来说，如此多的参数变得令人望而却步。如果引入物体的概念，三维物体点就变得单一，可以引用它们的动态对象。因此，沿时间建模的是物体的位姿，所需参数的数量变为$N^{\prime }=6N_c+N_c\times 6N_o+N_o\times 3N_{op}$，图3为10个对象的参数压缩比定义为$N^{\prime }/N$。这种动态对象和点的建模大大节省了使用参数的数量。

D.带对象的BA

束调整(BA)已知提供相机姿态和稀疏几何重构的准确估计，给出一个强大的匹配网络和良好的初始猜测。我们假设，如果物体姿态也进行联合优化，BA可能会带来类似的好处(图4)。静态地图点3D位置${\overline{\mathbf{x}}}_w^l$和相机位姿$T_{cw}^i$通过最小化相对于匹配关键点$u_i^l$的重投影误差进行优化（公式1），同样，对于动态表示对象点${\overline{\mathbf{x}}}_o^{j,k}$，相机位姿$T_{cw}^i$和物体位姿$T_{wo}^{k,i}$可以通过最小化公式2中的重投影误差进行优化。

在我们的实现中，在地图中插入关键帧有两个不同的原因:(a)摄像机跟踪较弱(b)任何场景对象的跟踪都很弱。前者的原因与ORB- SLAM2相同。如果一个具有相对大量特征的对象在当前帧中被跟踪的点很少，则会发生后者。在这种情况下，插入一个关键帧并创建一个新对象和新的对象点。如果相机跟踪不弱，则此关键帧不会引入新的静态地图点，如果其余动态对象具有稳定跟踪，则不会为这些轨迹创建新对象。在优化方面，如果仅仅因为摄像机跟踪较弱而插入关键帧，则局部BA优化当前处理的关键帧、共视图中与之相连的所有关键帧以及这些关键帧所看到的所有地图点，和ORB-SLAM2的实现一样。对于动态数据，如果插入关键帧只是因为跟踪目标比较弱，局部BA优化该对象的位姿和速度，并且摄像机沿时间尾巴的2秒连同它的物体点。最后，如果由于摄像机和物体的跟踪都较弱而插入关键帧，则对摄像机姿态、地图结构、物体姿态、速度和点进行联合优化。
为了避免非物理上可行的目标动力学，在连续观测中假设一个恒定的速度来强迫一个平滑的轨迹。物体k在观测i处的线速度和角速度分别定义为$v_i^k\in R^3,w_i^k\in R^3$.我们定义如下误差项：

需要一个额外的误差项来将物体速度与物体姿态及其相应的三维点耦合起来。这一项可以在公式4中看到，其中$△T_{ok}^{i,i+1}$是对象k在连续观测i和i+1的时间间隔$△t_{i,i+1}$的位姿变换。
其中$△T_{ok}^{i,i+1}$，由物体k在时刻i的线速度和角速度定义，如公式5所示，其中$Exp:R^3\in SO(3)$是SO(3)的指数映射。

最后，下面是我们在可优化局部窗口C中的一组相机的BA问题，每个相机i观察一组地图点MPi和一个包含OPk个对象的每个对象k的对象集Oi，

其中ρ是鲁棒Huber成本函数，用于降低离群值对应的权重，Σ是协方差矩阵。重投影误差Σ的情况与相机I中关键点的尺度有关，相机$i$分别观测到点$l$和点$j$。对于另外两个误差项Σ与两个连续观测对象之间的时间间隔有关，即时间越长，恒速度假设的不确定性就越大。待优化的参数为$\theta =\left\{{\mathbf{T}}_{\mathrm{CW}}^i,{\mathbf{T}}_{\mathrm{WO}}^{k,i},{\mathbf{X}}_{\mathrm{W}}^l,{\mathbf{X}}_0^{j,k},{\mathbf{v}}_i^k,{\mathbf{w}}_i^k\right\}$。
图5显示了所描述问题的布尔黑森矩阵(H)。黑森矩阵可以由与因子图中每条边相关联的雅可比矩阵来构建。为了有一个非零的(i,j)块矩阵，在因子图的i和j节点之间必须有一条边。注意地图点和对象点的稀疏模式的差异。Hessian矩阵的大小由地图点Nmp和物体点的数量决定，在典型问题中，它们比相机和物体的数量大几个数量级。应用Schur补码技巧求解该系统的运行时复杂度为$O(N_c^3+N_c^2{N}_{mp}+N_c{N}_o{N}_{op})$，根据静态点和动态点的数量，第二项或第三项将主导成本。

E.边界框

目前文献中的一些方法会认为动态对象的跟踪是完整的，迄今为止的贡献。也就是说，对于场景中的每个动态物体，当它第一次被观察到，我们都有其地图点的质心轨迹的估计，以及点云表示。这些作品的例子是ClusterSLAM[25]和VDO-SLAM[13]。然而，我们认为为相同语义类的对象找到一个共同的空间参考，以及对它们的尺寸和空间占用的估计也是至关重要的。
另外，CubeSLAM[11]和Li等人[10]的工作的基础是发现对象边界框。只有当边界框被发现时，对象才会沿着帧被跟踪。也就是说，如果摄像机视角不允许估计对象边界框(局部视图)，则不进行对象跟踪。然而，这对Li等人来说不是问题，因为cnn本质上对物体的局部视图具有鲁棒性，而CubeSLAM很难从被遮挡物体的视图初始化边界框。
我们提出将动态目标的轨迹估计与边界框解耦。前者为系统跟踪提供了丰富的自我运动估计线索，两者的结合有助于理解周围环境的动态。数据关联和BA阶段的输出包含相机姿态、静态场景和动态物体的结构，以及每个物体点的6自由度轨迹。这一点是物体第一次被观察到的3D点的质心。即使质心随着时间的推移随着新的点观测而变化，被跟踪和优化的物体姿态也参考了第一个质心。为了充分了解运动环境，了解物体的尺寸和空间占用是非常重要的。独立解决这两个问题允许从独立于相机观察点出现的第一帧跟踪动态对象。
我们通过搜索大致适合大多数对象点的两个垂直平面来初始化对象边界框。我们假设，即使对象并不总是完美的长方体，但许多对象可以近似地适合3D边界框。在只找到一个平面的情况下，我们在与对象类相关的不可观测方向的粗略尺寸上添加一个先验。这个过程是在RANSAC方案中完成的:我们选择计算的3D边界框，它与CNN 2D边界框的图像投影具有最大的IoU。这个边界框对每个对象轨迹只计算一次。
为了细化边界框尺寸及其相对于目标跟踪参考的姿态，在时间窗口内执行基于图像的优化。该优化旨在最小化3D边界框图像投影与CNN 2D边界框预测之间的距离。考虑到这个问题在一个对象的少于三个视图中是不可观察的，只有当一个对象至少有三个观察关键帧时才会执行这个问题。此外，为了约束解决方案空间，以防一个对象的视图使这个问题不可观察(例如，从后面观察到一辆汽车)，包含了一个关于对象尺寸的软先验。由于这个先验与对象类紧密相关，我们认为添加这个软先验并不意味着失去一般性。最后，将初始边界框位姿设置为先验，使优化解保持接近

4.实验

5.结论

我们提出了一种具有相机、物体和三维地图点之间新颖测量功能的目标级SLAM系统。这使我们能够跟踪动态对象，并紧密优化自我和周围环境的轨迹，使两种估计都是互利的。我们将目标跟踪问题与边界框估计问题解耦，与其他工作不同的是，我们没有对物体的运动、姿态或模型做出任何假设。我们的实验表明，DynaSLAM II在实时性能上达到了最先进的精度，这使得我们的框架适合于大量的应用。
我们的系统基于特征的核心限制了它发现准确的3D边界框的能力，也限制了它追踪低纹理物体的能力。充分利用密集的视觉信息肯定会推动这些限制向前发展。我们还想探索更具挑战性的任务，即仅用单目相机进行多目标跟踪和SLAM。这是一个有趣的方向，因为动态对象跟踪可以提供关于地图比例的丰富线索