动态SLAM论文(8) — DynaSLAM II: Tightly-Coupled Multi-Object Tracking and SLAM
目录
1 Introduction
2 Related Work
3 Method
A. 符号表示
B. 物体数据关联
C. 以对象为中心的表示
D. 对象束调整
E. 边界框
4 Experiments
A. 视觉里程计
B. 多目标跟踪
C. 时间分析
5 结论和未来工作
摘要
索引术语 —— SLAM、语义、跟踪、动态物体
1 Introduction
SLAM技术涉及基于单个移动相机数据流，在任意未知场景中构建地图并推导机器人自身位置与方向的问题。大多数现有的SLAM方法通常假定场景处于静止状态，并且只能通过识别和标记异常值作为动态物体来处理少量动态内容[1]-[3]。尽管上述静默条件在特定类型的应用中得到满足，但它们在高度拥挤的人群聚集环境（如自动驾驶、服务机器人及增强现实/虚拟现实系统）中的适用性受到限制。
近年来 SLAM 动态物体处理成为研究焦点，在此领域内已有大量文献致力于探索有效解决方案。现有文献主要通过识别移动区域并将其从 SLAM 过程中排除以解决这一问题 [4]-[7]。此外，在视觉流处理方面的一些研究则致力于将动态内容分离出来生成静态图像 [8,9]。在机器人学领域内仅有一小部分学者尝试将物体运动特性纳入 SLAM 框架以实现这一目标 [10]-[13]。第一组专注于基于静态场景下的自运动估计；而第二组则不仅致力于完成 SLAM 任务本身还提供关于其他动态主体的姿态信息
掌握周围动态物体的行为对新兴应用的发展具有关键作用。非准静态环境中必须明确周围运动信息以确保系统的可靠性和准确性。例如，在虚拟现实中必须准确跟踪动态物体以实现与其真实世界的互动。而在自动驾驶中车辆不仅要精确识别自身位置还需可靠感知其他车辆与行人从而避免碰撞风险。
大多数文献都专注于解决这一问题。许多文献采用传统的多目标跟踪技术 [14-18]来从SLAM框架中识别并单独追踪移动物体。其精度很大程度上依赖于相机的姿态估计，在动态环境复杂的情况下, 相机的姿态估计往往更易出错, 因为稳定的静态结构并非总是可用。近年来, 在机器人学领域已开始探索动态物体追踪与视觉SLAM技术的发展, 这使得相关问题变得更加复杂。这些系统通常针对特定应用场景进行定制, 并运用多种先验信息来约束解决方案的空间: 道路平面结构以及驾驶场景中的平面运动物体等典型场景是其中的重点研究对象。
在此背景下，为了介绍这一技术，我们将重点阐述DynaSLAM II的核心功能。该系统是一个免费开源的空间定位与建模平台，在动态环境中支持基于RGB-D的数据处理。它能够同时完成相机姿态、环境布局以及动态物体运动轨迹的精确推算。为了提升系统的实时性能，在局部时空范围内提出了一个高效的优化方案。其中，在独立于物体特性的前提下，采用了解耦合的优化方法来推算物体尺寸及其六个自由度的姿态信息。通过系统性地进行数据集测试与分析（KITTI数据集 [19]），证明了该方法的有效性与优越性。
图1：KITTI跟踪数据集的定性结果。
2 Related Work
传统的3D多目标跟踪方法通常将移动物体的检测与SLAM公式[14]-[18]分开处理，并对追踪过程进行详细阐述。其中,Wang等人[14]提出了一种基于贝叶斯公式的SLAM框架,用于实现移动物体追踪,并为其理论基础奠定了可靠的基础.Wangsiripitak等人[15]则提出了基于并行计算实现的同时 SLAM 和 3D 物体跟踪的方法,其中 SLAM 为后续对象建模提供关键支持信息,而特征标记则有助于提升定位精度.Rogers等人[16]则通过引入 Expectation-Maximization（EM）算法改进了 SLAM 技术,实现了地标位置的有效更新.近期研究中,Barsan等人[17]提出了适用于城市环境中的立体视觉密集建图算法,能够在静态背景和平移物体间实现良好的平衡 Rosinol等人[18]则结合了视觉惯性 SLAM 和密集网格跟踪技术,专注于人像追踪领域,取得了令人瞩目的实验结果.然而,这些传统方法的一个显著缺陷在于其定位精度高度依赖于相机姿态估计结果.在复杂动态环境中出现姿态估计失误的概率较高时,会导致多目标跟踪完全失效
估算相机运动及多物体制动轨迹的概念源于SLAMMOT研究[20]。随后的研究主要聚焦于利用RGB-D相机技术进一步探索这一概念。他们构建了一个数学模型，在融合基于滤波的方法（如SLAM）与物体制动追踪中实现了统一优化，并验证了该方法在自动驾驶导航与安全方面的有效性。该研究通过像素级别的实例分割技术，在静态室内环境与动态物体之间实现了精细重建。实验结果令人瞩目且具有广泛的适用性；对于密集方法来说，获取高精度的分割遮罩非常重要；因此Mask-Fusion [22] 和 MID-Fusion [23] 通过假设人造物体主要是凸型来对其进行了改进。
与我们采用相同方法的基础上，在特征提取领域鲜少有其他研究能够将静态与动态对象的信息整合至单一框架以提升估计精度
鉴于这些进展，在SLAM领域中基于特征的方法正在寻求最优优化公式的途径以整合相机、物体及其结构点之间的关系。我们提出了一种紧耦合的包调整框架，在这一框架下利用相机、物体及点的新测量数据特别关注计算复杂度与参数数量之间的权衡关系，并避免硬编码先验知识的影响机制。为此我们融合了实例语义先验与稀疏图像特征以构建完整的感知模型这一方案能够以较低计算成本实现相机位姿估计、地图构建以及动态物体跟踪等目标同时该方法与现有文献形成对比现有研究主要集中在估计动态物体的点云结构及其运动轨迹方面[12][13][25]而另一些研究则聚焦于同类物体间的共同参考点以及占据体积信息更为丰富的场景[10][11]为此我们计划分别开展这两个任务研究并充分利用各自的优势以避免潜在的问题
3 Method
基于流行的ORB-SLAM2算法[1]的基础上进行了扩展改进。系统采用同步校正得到的立体视觉数据（包括RGB-D图像），能够实时生成每帧图像对应的相机姿态信息以及各时刻存在的动态物体的姿态描述，并构建出包含空间位置与时间信息的动力学物体分布图谱。对于每一帧输入图像，则会执行以下操作：首先利用深度分割技术得到像素级别的语义分割结果；接着从立体图像中提取ORBFeat特征并完成这对应关系；随后将这些特征与前一时刻的状态估计结果及地图中的潜在对象候选进行关联匹配工作假设所有被观察到的目标均处于匀速运动状态在此基础之上建立目标实例间的对应关系随后通过这一过程可分别获得初始 camera pose信息以及目标实例的空间位置与姿态参数最终系统会综合运用边缘化和平滑运动先验等方法对相机运动轨迹、各目标边界框位置及其三维点云分布进行滑动窗口优化求解
A. 符号表示
我们将使用以下符号表示：立体/RGB-D相机 i 在世界坐标系W中的姿态为
（见图2）。相机 i 观察到1）静态3D地图点
和 2）具有姿态
和线性角速度
的动态物体。每个观测到的物体 k 包含动态物体点
。
图2展示了构建动态系统模型的关键符号。相机i与i+1捕获了动态对象k的状态以及静态组件(. _ . )的信息。其姿态特征被确定为
和
的对象是连续观测中相同的移动物体( )。
B. 物体数据关联
对于每个传入的帧，计算像素级语义分割，并提取并匹配立体图像对中的ORB特征[26]。如果一个实例属于动态类别（车辆、行人和动物），并且包含大量的新近的关键点，那么就会创建一个物体。然后将关键点分配给实例和对应的物体。我们首先将静态特征与上一帧及地图中的特征进行关联，以进行初始的相机位姿估计。接下来，将动态特征与局部地图中的动态点进行两种不同的关联：(a)如果地图物体的速度已知，我们将根据投影假设搜索匹配的点，假设是帧间的恒定速度运动；(b)如果物体的速度没有初始化或者在(a)中没有找到足够的匹配，我们将将粗略匹配限制在与连续帧中最重叠的实例相关的特征上。请注意，我们的框架处理遮挡，因为当前关键点与地图物体匹配，而不是与前一帧的物体匹配。还需要更高层次的实例和物体关联。如果大多数分配给新对象的关键点与属于一个地图对象的点匹配，则将这两个对象赋予相同的轨迹ID。此外，为了使这种高层次关联更加鲁棒，还会进行一种并行的实例与实例之间的匹配，基于CNN实例的2D边界框的IoU（交并比）进行匹配。
对于跟踪的第一个物体，使用三维点的质心和单位旋转来初始化SE(3)位姿。为了预测轨迹中更远的物体的位姿，我们使用一个恒定速度运动模型，并通过最小化重投影误差来优化物体的姿态估计。
在多视角几何问题中，对于具有姿态为
和具有齐次坐标
的三维地图点 l，参考帧为W，具有立体键点对应
的相机 i，投影误差的常见公式为：
其中
该函数被称为投影函数，在相机坐标系中负责将三维空间中的点投射到成像平面的像素坐标上。相较于另一个公式而言，在此我们建议对投影误差进行重新表述。
其中
是相机 i 观察到的物体k在世界坐标中的逆姿态
表示在相机 i 中观测的点j在其对象k的参考框中的三维齐次坐标
该方法使得我们得以同时实现相机姿态控制和移动物体群体的姿态优化及三维空间中的位置信息处理。
C. 以对象为中心的表示
由于跟踪移动物体的任务比一般的SLAM任务更为复杂，并且主要原因在于涉及的参数数量远超常规SLAM的任务需求，在这样的情况下就显得尤为重要以缩减相关参数的数量从而维持实时性能的表现水平
个相机、
个动态对象和每个相机观测到
个3D点的情况下，跟踪动态对象所需的参数数量变为
，而传统的静态SLAM表示中为
在操作与部署时若持续时间过长或较短的情况下, 参数的数量会急剧上升; 当引入物体现有概念时, 这些点将获得唯一性标识, 并可通过相关动态体实现引用; 因此, 在动态体中所需参数数量会因时间变化而有所差异
。图3显示了定义为10个对象的参数压缩比
。这种对动态对象和点的建模大大减少了所使用的参数数量。
图3：在使用物体时所需的参数数量与独立跟踪物体中属于物体的点时
所需的参数数量之间的关系(没有物体)。
D. BA优化
BA优化可以通过提升点云匹配质量并借助高质量初始猜测来实现精确的相机姿态估计以及稀疏几何重构。我们推测如果同时优化物体的姿态，则BA也可能带来类似的改进效果（见图4）。在静态地图中，三维地点的位置信息对于空间布局具有重要意义。
和相机姿态
通过最小化与匹配关键点
对（等式1）的重投影误差进行优化处理。同样地，在动态场景中，则采用最小化与预设关键点之间的距离的方法。
（等式2）的重投影误差来优化对象点
、相机姿态
和对象姿态
的估计。
图4：具有动态物体的BA因子图表示。
在我们的实现中，一个关键帧可能有两个不同的插入原因：(a) 相机跟踪效果欠佳,(b) 场景中的某些物体跟踪效果也不理想。前者的情况与ORBSLAM2中的情况相同,但后者则是因为在当前帧中,具有明显特征的物体仅有少数点被成功追踪到。在这种情况下,系统会插入一个关键帧,并新增相应的物体及物体点。如果相机的跟踪效果良好,就不会在此处引入新的静态地图点;同时,如果其他动态物体会持续稳定地被追踪到,也不会生成新的物体描述。对于优化操作而言,如果仅仅因为相机Tracking不足而触发此操作,局部BA会对当前处理的关键帧进行优化,并同步优化与其相关联的所有关键帧及其可见的地图点;这种优化方式与ORBSLAM2中的实现一致。而对于动态数据部分,如果仅仅因为Tracking失败而触发此操作,则会优化相关物体现在的姿态、速度参数以及相机在两秒时间段内的状态信息和对应的物体点坐标;当两者Tracking均不佳时，则需对相机姿态、地图结构、物体现状、运动速度以及相关物体系坐标等进行全面联合优化
为了防止物理上不可行的情况出现于物体的动力学模型中，在连续观测过程中假设物体以恒定速度运动以生成平滑的轨迹。在观测时刻i时，物体k的线速度和角速度分别表示为
和
。我们定义以下误差项：
为了使物体速度与物体姿态及其对应的3D点实现耦合关系, 引入一个辅助误差项是必要的.
是物体 k 在连续观测 i 和 i+1 之间的时间间隔
中发生的姿态变换。
是从时间 i 处物体 k 的线速度和角速度(
和
)确定的，如公式5所示，其中
是SO\left(3\right)上的指数映射。 最后，在可优化的局部窗口C内提出了一个BA问题集合。其中每个相机i观测对应的一组地图点。
和一个包含每个物体 k 的物体点集
的物体集合
：
其中ρ被定义为鲁棒性增强的Huber代价函数，并且该代价函数通过减少离群对应对整体匹配效果的影响程度来提升整体匹配质量。该代价函数中的协方差矩阵Σ取决于相机i观测点l和j的关键点的比例；而这些额外误差项则受物体两次连续观测之间的时间间隔影响，在这种情况下时间间隔越长会导致对恒定速度假设下的不确定性度量更加敏感。我们需要最小化的一组参数包括这些因素的相关权值设置。
。
图5呈现了问题对应的布尔海森矩阵H...该矩阵可通过因子图中各边关联的雅可比矩阵组装而成...特别指出的是地图上的稀疏分布特征与物体上的稀疏模式存在显著差异...海森矩阵的整体规模主要取决于地图上存在的独立地图点数量
取决于物体点的数量，在典型的测试案例中，这种情况通常涉及的物体点数量远超相机和物体数量。通过施加Schur余数技巧来降低系统运行时间复杂度
，其中第二项或第三项将取决于静态和动态点的数量。
图5：由五个关键帧（KF）、一个拥有十个目标点（OP）和十个静态地图点（MP）构成的Hessian矩阵。
E. 边界框
现有文献中的几种方法所认为的是到目前为止对动态目标的跟踪已经相当完善。即，在场景中的每个动态目标上都估计了一个质心轨迹以及一个点云表示。这些工作的实例包括ClusterSLAM [25]和VDO-SLAM [13]。然而我们强调的是在同一语义类别对象中找到共同的空间参考及其尺寸与空间占用估计的重要性。相反基于物体边界框的基础研究如CubeSLAM [11]与Li等人工作 [10]则关注于在检测到边界框后进行物体追踪这一过程。值得注意的是如果相机视角受限无法估计物体边界框（部分视图）则不进行物体追踪这一做法对于Li等人而言不成问题因为CNN天然具备对物体部分视图的鲁棒性但CubeSLAM在初始化被遮挡物体的视图时仍存在困难
建议将动态物体的轨迹估计与边界框估计分开处理。前者为系统跟踪提供了丰富的线索来实现自运动估计，在实际应用中具有重要意义。两者的结合不仅有助于提升跟踪精度还能有效减少计算开销从而进一步提高系统的效率与稳定性。数据关联及BA阶段的结果包含了相机位姿、静态场景结构以及动态物体信息并将这些信息整合到统一的空间中实现了对复杂环境的有效建模与理解这一过程对于机器人感知与导航系统具有重要的参考价值。其中关键点是确定一个物体制动模型以描述目标在首次观察时的位置特征由于质心会随着时间推移而发生变化（尤其是在新增观测数据后）但被持续跟踪优化的对象姿态仍将以初始质心为基础进行计算以确保算法的有效性和稳定性
我们在定位大多数目标点大致符合两个垂直平面的过程中初始化一个物体边界框，在假设尽管单个物体会有一定程度变形的前提下认为许多实际场景仍能近似适应一个三维包围盒的情况下继续进行推断操作。若仅检测到单个平面，则补充与该物体类别相关的不可观测方向的大致尺寸，并利用先验知识进行约束优化；该过程采用RANSAC算法执行：在每一轮迭代中根据当前候选包围盒与真实目标之间的匹配程度动态调整其几何参数；最终所得结果满足各维度上的收敛条件后即可确定最终包围盒位置并将其用于后续状态更新操作；整个流程确保了计算效率的同时保证了较高的精度水平
旨在优化边界框尺寸和姿势以适应物体会动需求，在一段时间窗口期间执行图像空间优化以实现上述目标。该优化过程旨在最小化三维边界框图像投影与目标CNN二维预测之间的距离这一核心指标。鉴于可见性问题是不可见问题当且仅当一个物体至少具有三个独立观测关键帧时才可解决此难题因此选择在此条件下实施此方法。为了限制解空间范围防止视角导致不可见问题如从后方拍摄的车辆在此过程中融入了关于物体尺寸分布情况的软性先验知识。由于该软先验与物体类别高度相关联我们认为加入这一信息不会削弱方法的一般适用性。最后阶段在初始化阶段设定初始姿势作为预设值从而引导后续优化过程趋向合理结果。
4 Experiments
本节中阐述了DynaSLAM II实验验证的方法。该实验系统被划分为两大模块：首先是对相机运动估计因跟踪物体影响的研究模块（第IV-A小节），其次是对多目标跟踪性能分析模块（第IV-B小节）。
A. 视觉里程计
对于视觉里程计实验的研究工作，则采用了KITTI跟踪数据集（表I）与原始采集数据集（表II），其中引用文献[19]作为基础资料来源。这些场景是由车辆从不同角度拍摄形成的灰度序列与RGB立体影像组合而成，并包含了移动物体如汽车与行人的运动图像及相应的GPS定位信息。
两个表格对我们的系统与ORB-SLAM2以及我们之前的研究工作DynaSLAM [4]进行了全面比较。ORB-SLAM2是构建DynaSLAM II的基础性SLAM框架，并未专门针对动态物体进行处理。而DynaSLAM则在此基础上增强了对动态物体检测及类别特征识别的能力，并未放弃对静态特征的利用。然而，在某些特定场景下两者的性能表现存在差异：当场景中的动态物体具有代表性且持续运动时（如车辆移动），DynaSLAM表现出色；而当这些物体静止不动（如停靠状态）时，则会出现较大的定位误差。这种现象主要源于静态物体特征对于姿态估计的重要性，在固定位置区域中虽未被充分利用。此外，在经过多方面的评估测试中均表现优异的版本（即DynaSLAM II）能够根据动态条件的变化自动调节其跟踪策略：一方面，在动态物体运动状态下能够准确估计速度并提供丰富的姿态线索；另一方面，则能有效追踪静止状态下的关键点并估算接近零的速度特性
表格 I 和 II 展示了我们的自我运动结果与其他在联合 SLAM 框架中追踪动态物体的最新系统结果进行比较。其中 ClusterSLAM [25] 作为后端使用的 SLAM 系统,严重依赖于相机姿态的初始估计。另外,ClusterVO [12] 和 VDO-SLAM [13] 是与我们类似的多目标跟踪能力 SLAM 系统,其中前者能够处理立体视觉和 RGB-D 数据,而后者仅能处理 RGB-D 数据。报告中的误差数值采用不同的度量指标给出,以便我们可以直接使用作者提供的值。在所有序列中,DynaSLAM II 在所有序列中的平移相对误差 (RPEt) 均低于 VDO-SLAM.然而,在大多数情况下,VDO-SLAM 能够实现较低的旋转姿态误差.由于远距离点提供了最丰富的旋转估计线索,我们认为这种精度差异不取决于物体追踪性能,而是取决于底层相机姿态估计算法和传感器套件.对于 ClusterVO 的性能评估而言,其在大多数序列中的准确率与我们的方法相当.
B. 多目标跟踪
一旦证明了在估计自身运动时追踪动态目标的有效性后, 我们又采用了KITTI追踪数据集[19号]作为验证多目标追踪效果的标准. 通过耗时且精细的手工标注过程, 我们获得了用于描述动态物体轨迹及其三维边界框的数据.
为了引导读者关注图1, 我们将对其进行展示, 观察我们在该数据集上获得的定性结果。研究发现, 左边那两辆标有'紫色'标签的汽车拥有较为精确的边界框识别效果, 但由于视野有限, 其余部分未能呈现。值得注意的是, 这一场景更具挑战性, 因为另两辆前部车辆虽与摄像头距离较远, 却仍能实现精准跟踪。
过去十年间
表 III 展示了 KITTI 跟踪数据集中所有目标检测的评估结果，并基于 KITTI 3D 目标检测基准进行了计算（表 III）。Chen 等人 [28] 的卷积神经网络基于单视角网络准确性本身表现突出，并且结合多视角细化方法取得了出色的结果；但以泛化性能为代价。Li 等人 [10] 的研究进一步验证了这一结论。相比之下, Barsan 等人 [17] 和 Huang 等人 [12] 在边界框检测方面的准确率非常高,但他们对被截断或遮挡的目标表现得不够稳健.我们的实验表明,在仅接受少量精度损失的前提下,我们能够有效地处理被截断或遮挡的目标.然而,通常只能捕获较少数量的有效边界框.这与我们系统的特征性质所决定的独特性相矛盾:与 Barsan 等人的工作 [17] 不同,他们首先基于立体匹配计算稠密对应关系
为了评估我们对物体轨迹的估计效果，在表格IV中我们选择了KITTI跟踪数据集中最长持续性的12个序列。这些检测结果未受遮挡影响，并且未被截断，并且其高度至少为40像素。随后我们利用这些序列的真实物体标识符对其进行标记分类。针对每一个真实轨迹我们计算与其具有最大重叠度（重叠度至少为25%）的边界框作为匹配对象。在此过程中我们采用以下指标对我们的跟踪性能进行评估：ATE（平均平移误差）、RPE（相对位置误差）以及MOTP（多目标跟踪精确度）。其中MOTP的具体计算方法是将3D边界框在当前帧上的投影与该帧图像中的IoU作为重叠度衡量标准。而对于BV（鸟瞰图精度）和3D精度指标则分别采用鸟瞰图中边界的IoU以及三维空间中边界的IoU作为衡量依据。通过这一评估体系我们可以初步了解我们的框架在实际应用中的表现如何。观察结果表明大多数轨迹都能被成功跟踪记录下来这表明我们的系统具有较高的鲁棒性。然而我们也注意到存在一些特殊情况例如当物体距离摄像头较远时由于立体匹配效果不足导致特征提取不够充分从而影响了Tracking的效果表现得不够理想。此外对比测试发现汽车类目标物的Tracking误差相较于行人类目标物更为可控这是因为它们呈现出较为刚性稳定的形状特征（如序列0017所示）。然而行人Tracking的效果却明显不如汽车类目标物这与其非刚性形变特性密切相关。基于上述观察我们认为我们的算法设计在面对复杂场景时仍需进一步优化以提高Tracking精度和预测能力
C. 时间分析
为了评估我们的提案性能, 表V列出了不同构建模块的平均计算时间. 通过分析实验数据发现, DynaSLAM II 的运行效率与其被追踪物体的数量密切相关. 在KITTI数据集中的一段特定序列中, 当仅需同时跟踪两个物体时, 该系统能够达到12帧每秒（fps）以上的帧率. 然而, 在另一组实验中发现, 当同一场景下最多可同时处理20个物体时, 其运行速度略有下降, 但仍维持在约10fps的水平. 需要注意的是, 在计算总运行时间时, 我们并未计入语义分割网络的时间消耗, 因为这取决于GPU性能以及所采用模型复杂度. 类似YOLACT [29]等算法能够实现实时运行并生成高质量的实例分割结果.
最后，在表V中汇总了KITTI数据集中协同工作完成SLAM与多物体跟踪系统的平均运行时间数据。仅此一家的DynaSLAM II 专为实现实时性能优越的任务而设计。
5 结论和未来工作
我们开发了一种新型测量函数的对象级SLAM系统，在相机端、物体现象以及三维地图点间实现了测距功能。这种设计不仅有助于实现动态物体的实时跟踪能力，并通过优化自身运动轨迹与环境变化轨迹之间的关系来提升两方估计精度。我们成功地将物体跟踪问题从边界框估计中分离出来；而其他研究则未采取此方法；在本研究中未对被测物体的运动学参数、姿态描述或三维模型建立等前提条件作出限制性假设；通过大量实验测试表明：DynaSLAM II在实时运行中的精度表现超越了现有技术；这一创新方法可应用于多个实际应用场景
我们系统依赖特征的核心受到了制约，在精确识别三维边界框方面的能力以及对低纹理物体追踪方面均存在不足。显然会推动这些限制得到突破的情况下, 我们期望探索更为复杂的任务, 即仅仅依靠单目摄像头实现多目标追踪与SLAM. 这确实是一个很有吸引力的方向, 因为动态物体追踪能够提供丰富的地图尺度线索.