论文笔记:DynaSLAM II: Tightly-Coupled Multi-Object Tracking and SLAM

1. Abstract

DynaSLAM II利用实例语义分割和ORB特性来跟踪动态对象。使用一种新的BA方案，对静态场景的结构和动态对象，联合相机和移动对象的轨迹一起优化。对象的3D边界框也在固定的时间窗口内被估计和优化。证明了跟踪动态对象不仅为场景理解提供了丰富的线索，而且也有利于相机机跟踪。

2. Introduction

针对动态对象的传统做法：

1. 拒绝这些区域[4]–[7]
2. 将显示动态内容的图像转换成只有静态内容的真实图像，在其他地方处理图像流[8]–[10]
3. 将移动物体的动力学模型纳入来解决这个问题[11]–[14]。

前两个组主要致力于从静态场景中获得精确的自我运动估计，而最后一个组的目标是双重的:它们不仅解决了SLAM问题，还提供了关于其他动态对象的姿态和轨迹的信息。

4. 绝大多数专门解决这一问题的文献都是通过使用传统的多目标跟踪方法来检测运动目标，并将其与SLAM公式分开跟踪[15]–[19]。精度依赖于相机姿态估计，必须保证存在可靠静态结构的复杂动态环境中，否则相机姿态估计容易失败。

DynaSLAM II是一个用于动态场景的开源系统，可同时估计摄像机的姿态、地图和场景移动对象的轨迹，贡献如下:
a) 一种由高级的2D实例匹配算法——动态对象特征匹配方法。
b) 一种将相机、特征点和动态对象一起BA的新方案。
c) 边界框的解耦优化，以找出同一类对象之间的公共引用。
d) 我们的实验表明，相机运动估计和多目标跟踪是互利的。

3. Related Work

A. Loosely-Coupled Multi-Object Tracking and SLAM

1. 解决这一问题的基础：[15]推导了带有运动目标跟踪的SLAM的Bayes公式。
2. [16]提出了用3D对象跟踪器并行实现SLAM：SLAM为跟踪器提供注册地图对象的信息，跟踪器允许标记对象上的特征。
3. [17]将电磁技术应用于基于图形的SLAM方法，并允许地标是动态的。
4. [18]提出了一种基于立体的城市环境密集映射算法，该算法同时重建静态背景和运动物体。
5. [19]协调了视觉惯性SLAM和密集网格跟踪，主要关注人类，在模拟中显示了令人印象深刻的结果。

这些方法的主要缺点是它们的精度与相机姿态估计的精度高度相关。也就是说，如果相机姿态估计失败(这在复杂的动态环境中很有可能)，多目标跟踪也会直接失败。
同时估计相机运动和多个运动物体的运动的想法源于SLAM-MOT工作[21]。他们建立了一个数学框架来集成基于滤波的SLAM和运动目标跟踪，证明它满足自动驾驶中的导航和安全要求。后来，使用RGB-D相机的作品跟进了这一想法，使用像素级实例分割稠密重建静态室内场景和运动物体，显示出很好的结果[22]–[24]。由于稠密方法获得精确分割至关重要，因此[23]和[24]通过假设人造物体是凸的来完善它。

B. Tightly-Coupled Multi-Object Tracking and SLAM

1. 基于特征，将来自静态和动态对象的信息合并到单个框架中以提高估计精度。[25]将动态物体和相机自我运动紧密结合起来。然而，他们只报道了合成数据的实验，显示出有限的真实结果。
2. [11]使用以端到端方式训练的CNN来估计汽车的3D姿态和尺寸，这与相机姿态一起被进一步优化。优点：在6 DoF物体姿态估计中提供极好的精度，缺点：通用性差，需要大量的数据训练。
3. CubeSLAM[12]通过使用一个3D包围盒，仅用单目相机就显示了很好的结果，该3D包围盒是基于2D包围盒和正在消失的点生成的。他们假设物体具有恒定的速度，并利用物体的先验知识，如汽车尺寸、道路结构和平面非完整物体车轮运动模型。此外，它们仅跟踪其3D边界框可观的对象，即，仅看到长方体对象的两个或多个面。
4. ClusterSLAM[26]提出了一种无场景先验的SLAM后端，用于发现单个刚体并计算它们在动态环境中的运动。由于它充当后端而不是完整的系统，因此其性能在很大程度上依赖于路标点的跟踪和关联质量。同一作者最近开发了完整的系统ClusterVO [13]，该系统用对象所属的概率对对象点进行建模，以处理分割的不准确性。假设它们没有先验，优点：室内室外跟踪效果好，缺点：对3D包围盒的估计不准确。
5. VDO-SLAM [14]使用密集的光流来最大化移动物体上被跟踪的点的数量。用相机、物体和点实现了一个BA，给出了好的结果，但是计算复杂。

C. A Common Reference for Each Object Class

作者认为，为相同语义类别的对象找到一个共同的空间参考以及对它们的维度和空间大小的估计也是至关重要的。

1. CubeSLAM [12]和[11]工作的基础是发现对象3D包围盒。只有发现了边界框，才会沿着框架跟踪对象。也就是说，如果相机视角不允许估计对象边界框(部分视图)，则不进行对象跟踪。虽然这对于Liet等人来说不是问题[11]，因为CNN本质上对物体的部分视图是鲁棒的，但是CubeSLAM很难从被遮挡物体的视图中初始化包围盒。
   鉴于这些进展，很明显，基于特征的SLAM团队正在寻找最佳的优化公式来组合摄像机、物体和结构点。在我们的工作中，我们使用了一个紧耦合的BA，在相机、物体和点之间进行新的测量，特别注意其计算复杂性和涉及的参数数量，而不引入硬编码先验。为此，**我们将实例语义先验与稀疏图像特征相结合。**该公式允许以低计算成本对相机、地图结构和动态对象进行互利的估计。另一方面，一部分当前文献集中于动态对象的点云结构的估计和随机对象参考的轨迹的估计[13]，[14]，[26]，而另一部分文献寻求为相同类别的对象找到共同的参考以及信息更丰富的空间体积[11]，[12]。我们打算独立完成这两项任务，以便充分利用两者的优势，而不受它们的劣势影响。

4. Method

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

A. Notation(标记)

我们想指出，就其性质和对视觉SLAM的影响而言，有两种类型的动态对象:

1. 天生具有移动能力的物体(人、动物和车辆)组成。我们把这些物体称为先验动态可移动的。
2. 移动的物体，视觉SLAM传感器观察它们，不管它们的语义类别。我们称它们为移动物体。
   
   

关于几何场景，我们将使用以下符号:
相机在 i 时刻位于世界系下的位姿位Ti,CW；
相机在对象的坐标系下，第 i 时刻观测到的：

• 静态的3D地图点；
• 位姿为Tk,i W0，线速度Vk,i，角速度wk,i 的动态对象
• 动态对象k包含动态对象点xj,k o
• 红色三角形为动态对象包围框。

B. Object Data Association

对于每个输入帧，遵循以下程序:

1. 计算像素级语义分割，提取ORB特征[27]并在双目图像对之间进行匹配。我们假设动态特征是那些属于先验动态实例的特征，不管它们的运动如何。
2. 作者首先将静态特征与前一帧及地图中的特征点相关联，以初步估计相机姿态，按照ORB-SLAM实现。
3. 利用 KM 算法建立连续帧之间并行的实例对实例匹配，以 2D 检测框的交并比作为代价。
4. 接下来，动态特征以两种不同的方式与局部地图中的动态点相关联:
   a) 如果地图对象的速度是已知的，则假设帧间匀速运动，通过重新投影来搜索匹配。实例匹配结果用于发现异常值。
   b) 如果物体速度没有被初始化或者在(a)步骤时没有找到足够的匹配，则在连续帧中对那些属于大多数重叠实例的特征使用暴力匹配。
5. 实例和对象之间也需要更高级别的关联。如果大多数新实例关键点与属于同一个地图对象的点相匹配，则该实例被赋予相同的对象跟踪id。
6. 如果对应于先验动态类的实例包含超过七个被观察到的关键点，这些关键点的立体3D投影靠近相机(小于立体基线的55倍)，则会创建一个新的对象实例。然后将关键点分配给相应的对象。

请注意，我们的框架处理持续不到两秒钟的遮挡。如果需要，这个阈值可以扩展:如果物体速度保持不变或几乎不变，我们的框架会处理遮挡。由于此功能，如果被跟踪对象的速度在此窗口中发生变化，则扩展阈值可能不会总是导致更好的结果。
一条轨迹的第一个对象的SE(3)姿态用这些三维点的质心和identity旋转初始化。为了从轨迹取预测物体的姿态，我们使用恒速运动模型，并通过最小化匹配重投影误差来优化物体姿态估计。
在参考系W中，相机 i 的位姿Ti CW、3D地图点 l 的四维齐次坐标xl，关键点ul i= [u，v，uR]的重投影误差公式:

其中π是校正后的多目/RGBD相机的重投影函数，该相机将相机坐标中的3D齐次点投影到相机帧像素中。与此公式不同，它对静态表示有效，我们将重投影误差重新定义为

其中，Tk,i WO是相机 i 正在观察物体k时，物体在世界坐标系中的反位姿，xj,k O∈R4表示点 j 在其物体参考k中的3D齐次坐标，在摄像机i uj i∈ R3中进行观测。这个公式使我们能够联合优化摄像机和不同运动物体的姿态，以及它们的三维点的位置。

C. Object-Centric Representation（个人理解）

这一小节的内容，主要就是阐述了优化变量维度的减少，思想上有着一定的指导性。
文中给出：
给定一组相机Nc，即可以理解为滑窗的大小，No表示观测到的动态对象，Nop表示观测到的动态对象的3D点。
作者基于点的表示，则在优化时变量的维度大小为：
N=6Nc+Nc×No×3Nop
6表示相机的位姿6DOF；观测到No个对象，每个对象上附有Nop个3维点，这些点均被跟踪了Nc帧；

传统的SLAM的表示：N=6Nc+No×3Nop；

作者这里引入了一个很重要的概念，即基于对象的概念，这个地方也可以看出文章为什么叫做对象级的SLAM系统！
引入对象这一概念后，属于对象的3D点就不会每一帧都去加入优化，而只和对象个数有关（这里就会有很多的点会被固定住，优化维度大大降低），即对象数量不增加，在优化时的维度就不会随着帧数增加而增加。
N′=6Nc+Nc×6No+No×3Nop；
6表示相机的位姿6DOF；观测到No个对象，每个对象被跟踪了Nc帧，因为每个对象上附有的Nop个3维点被固定在对象上了，故只需关注对象就行，即关注对象的位姿6DOF，同时，被优化的对象的点就只需加入一次，即No×3Nop；

下面简单看看为什么维度减小了：上下同除

由于Nop相对于Nc是个大量，很直观就可以看出，引入对象的概念后，维度随Nc增大而减小
eg：假设滑窗维护10帧，Nc=10，跟踪了5个动态对象，No=5，每个对象附有观测点100个，则维度为：N′=1860，N=15060，维度相差8倍！！！
该思想具有一定的指导意义：比如更高级的前端，线，面，甚至基于对象等，可以参照该想法，减少优化的变量！

D. Bundle Adjustment With Objects

我们假设，如果物体姿态也加入联合优化，BA或许也会带来类似的好处(图4)。静态地图点3D位置xl W和相机位姿TiCW，通过最小化匹配的关键点ul i 的重投影误差来优化见(1)式。类似地，对于动态物体，对象点xj,k O，相机姿态Ti CW和对象姿态Tk,i WO可以通过最小化(2)中的重投影误差公式来优化。
在我们的实现中，由于两个不同的原因，关键帧可以插入到地图中:a)相机跟踪较弱；b)对任何场景对象的跟踪较弱。前者的原因与ORB-SLAM相同。然而，若某个有许多特征点的对象，在当前帧中却只有少数的点被跟踪上，就会发生后一种情况。随后会出现以下优化场景:

• 如果仅因为相机跟踪较弱而插入关键帧，则局部 BA 会优化 当前处理的关键帧、共视图中与其连接的所有关键帧以及这些关键帧看到的所有地图点，执行的是 ORB-SLAM的优化策略。
• 如果仅因为实例跟踪较弱而插入关键帧，则会创建具有新对象点的新对象。这个关键帧不会引入新的静态结构，如果其余的动态对象具有稳定的跟踪，则不会为这些跟踪创建新的对象。在这种情况下，局部BA优化的是 对象的姿态、速度和点，以及优化在接下来2秒的时间内的相机位姿。TODO
• 最后，如果因为相机和对象跟踪都很弱而插入关键帧，则相机姿态、地图结构、对象姿态、速度和路标点将被联合优化。

为了避免引入复杂的物体动力学模型，在连续观测中，假设物体运动速度恒定来强制平滑轨迹。观测I处物体k的线速度和角速度分别表示为vk i 和wk i。我们定义误差:

需要额外的误差项来将物体速度和姿态与其对应的三维点耦合起来。这个项可以在(4)中看到，其中δTi,i+1 Ok是物体k在连续观测i 和i+1之间的时间间隔δt i,i+1中经历的姿态变换。Note：补充，相邻两帧，附在某对象上的点的坐标的误差

δTi,i+1 Ok的定义：是由物体k在时刻i的线速度和加速度定义的，如(5)所示，其中Exp : R3→ SO(3)的变换。

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

其中ρ是鲁棒核函数，σ是协方差矩阵。在重投影误差的情况下，σ与关键点的尺度相关，关键点是在相机i 中分别观察到的点 l 和点 j。对于另外两个误差项，σ与物体两次连续观测之间的时间间隔有关，即时间越长，恒速假设的不确定性越大。需要优化的参数是θ = {Ti CW，Tk，i WO，xl W，xj，k O，vk i，wk i}。
图5显示了所述问题的布尔海塞矩阵(H)。海塞矩阵可以由因子图中与每条边相关的雅可比矩阵建立。为了得到非零(i,j)矩阵块，因子图中的节点 i 和 j 节点之间必须有一条边。注意地图点和对象点的稀疏模式的差异。Hessian矩阵的大小由地图点Nmp和对象点的数量决定，在典型问题中，它比相机和对象的数量大几个数量级。应用舒尔补码技巧并求解系统的运行时复杂度为O(N3 c+ N2 cNmp+ NcNoNop)，其中第二项或第三项将控制残差根据静态点和动态点的数量决定。
Note：图5的个人理解：
一大格代表30维，论文中的例子是：Hessian矩阵有5个关键帧(KFs)， 1个对象与10个对象点(OPs)和10个静态地图点(MPs)；
也就是说，Nc=5，No=1，Nop=10，位姿为6维的李代数se(3)，故维度为：5×6+5×(6+3+3)+3×10+3×10；Objects包含了对象的位姿，线速度和角速度，可以看到，5帧的滑窗，只添加了一次对象点。

E. Bounding Boxes

文章提出解耦动态对象轨迹的估计和边界框。前者为系统跟踪提供了丰富的自我运动估计线索，两者的结合有助于理解周围的动态环境。数据关联和BA阶段的输出包含摄像机姿态、静态场景和动态对象的结构，以及每个对象的质心的6DoF轨迹。尽管质心随着时间的推移随着新点的观察而变化，但被跟踪和优化的物体姿态被称为第一质心。了解物体的尺寸和空间大小对充分了解移动的环境很重要。独立解决这两个问题允许从动态对象出现的第一帧开始跟踪它们，而与相机-对象视点无关。
我们通过搜索适合大多数对象点的两个垂直平面来初始化对象边界框。我们假设许多人造物体可以近似适合一个三维边界框。在只找到一个平面的情况下，我们在与对象类相关的不可观测方向的粗略维度上添加一个先验。这个过程是在一个RANSAC方案中完成的：我们选择计算出的三维包围盒，它的图像投影与CNN 2D包围盒的IoU最大。这个边界框对每个对象轨迹而言，只计算一次。
为了优化边界框尺寸及其相对于对象跟踪参考系的姿态，在暂时的窗口内执行基于图像的优化。该优化寻求最小化3D包围盒图像投影和CNN 2D包围盒预测之间的距离。考虑到这个问题在一个物体少于三个视图的情况下是不可观测的，所以只有当一个物体至少有三个可观察到的关键帧时，这个问题才被执行。此外，为了在对象的视图使得该问题不可观(例如，从后面观察的汽车)的情况下约束解空间，因此加入了关于对象尺寸的软先验。由于这个先验与对象类紧密相关，我们认为增加这个软先验并不意味着丧失一般性。最后，初始边界框姿态被设置为先验，以便优化解保持接近。

5. CONCLUSIONS AND FUTURE WORK

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