GSLAMOT 一种基于跟踪符和 Query 图的实时定位、映射和多个目标跟踪系统 ！

本文针对移动物体在陌生环境中交互的问题，提出了一种基于跟踪符（Tracklet）图和 Query 图（Query Graph）的框架，即GSLAMOT，用于同时定位、映射和跟踪移动物体的3D姿态。

GSLAMOT利用相机和激光雷达多模态信息作为输入，将动态场景的表示划分为表示静态环境的语义图、表示自主代理的轨迹以及用于跟踪和预测检测到的移动物体3D姿态的在线维护的跟踪符图（Tracklet Graph，TG）。

每个帧构建一个 Query 图（Query Graph，QG）以 Query 和更新TG。为准确关联物体，提出了一种多准则星图关联（Multi-criteria Star Graph Association，MSGA）方法在QG中的检测与TG中的预测跟踪符之间寻找匹配的物体。

然后，提出了一种基于物体的图优化（Object-centric Graph Optimization，OGO）方法，同时优化TG、语义图和代理轨迹。它将检测到的物体三角化为地图以丰富地图的语义信息。

作者并处理了三个紧密耦合的任务的并行处理效率问题。作者在KITTI、Waymo和模拟交通拥堵的数据集上进行了实验，这些数据集突出了具有挑战性的场景。

实验结果显示，与最先进的方法相比，GSLAMOT可以在拥挤环境中实现精确的目标跟踪，同时在大挑战场景下进行SLAM，展现出更优秀的性能。

代码和数据集可在https://gslamot.github.io。

1. Introduction

同时利用多模态信息进行自定位、映射（SLAM）和多目标3D姿态跟踪（3D MOT）是实现动态场景感知的必要条件，如自动驾驶、无人驾驶飞机（UAV）和机器人与机器人的协作。

多个因素共同作用使得这个问题非常具有挑战性。首先，SLAM和3D MOT需要同时进行并相互依赖。SLAM依赖于目标检测来消除动态目标的干扰，以便正确跟踪代理的姿态和静态映射，而3D MOT则依赖于代理的准确姿势来计算移动目标的3D姿势。其次，代理和动态目标的并发运动为定位和目标跟踪带来了挑战；第三，可能的目标检测算法错误和传感器噪声也会导致局部物体的定位不准确；最后，遮挡和高速度运动等因素也会 introduce 困难，导致目标匹配错误，从而定位和跟踪错误。

现有的3D多目标跟踪（3D MOT） 通常假设自运动已知且无噪声或传感器固定在世界坐标系中。然而，在实际应用中，通常代理和周围物体都在运动，需要同时进行SLAM和3D MOT。此外，由于运动，代理可能在不同的帧中以不一致的方式观测到同一个目标，导致边框（如图3所示）的不准确和跨帧目标匹配的困难。面向目标的SLAM（OOSLAM）是另一个密切相关的研究领域，但OOSLAM主要关注估计自运动轨迹。它没有明确讨论优化详细的物体3D轨迹。但是许多应用确实需要实时跟踪3D物体的姿势（Wang等人，2019；Wang等人，2019；Wang等人，2019）。

为了应对以上挑战，本文提出了一种基于图匹配和图优化的系统GSLAMOT，用于同时执行视觉 SLAM 和 3D MOT，该系统以立体图像和 LiDAR 点云序列作为输入。据作者所知，这是首个在具有拥挤和高动态目标的场景中，同时输出 ego 轨迹和物体轨迹，且准确无误的工作。特别是，GSLAMOT 通过一个结合表示环境的语义图、ego-agent 轨迹和在线维护的 Tracklet Graph（表示 3D MOT 轨迹）来表示动态场景。跟踪器 TG 跟踪多个物体的 3D 位置，并在时间 t 预测物体的 3D 位置。每个帧基于目标检测构建一个 Query Graph (QG)，用于 Query 预测的物体位置。为了进行精确的物体关联，作者提出了一种新颖的多标准星图关联 (Multi-criteria Star Graph Association, MSGA) 方法，用于在动态、拥塞和噪声环境中进行鲁棒关联，以处理匹配挑战。MSGA 评估 TG 和 QG 之间的局部一致性、空间一致性和形状一致性，从而显著提高与仅使用空间特征进行多目标跟踪相比的效果。

找到关联后，作者提出了一种以物体为中心的图优化（Object-centric Graph Optimization, OGO）方法，同时优化 TG、地图和代理人轨迹。

作者将优化分为两个部分：

（1）实时物体为中心优化窗口（Object-Centric Optimization Window, OCOW）和（2）长期物体-自我融合窗口（Object-Ego Fusion Window, OEFW）。在 OCOW 中，采用两阶段优化策略。然后，在 OEFW 中，以紧密耦合的方式将优化的 ego 运动、环境点 tracklets 和地图 fuse。实验证明，OGO 在收敛速度（图4）和精度上优于centric 方法。

在系统实现中，为了使局部定位、3D MOT 和语义映射可以同时高效运行，作者采用多线程并行计算，在跨线程的同时执行视觉前端、映射、检测、特征提取和跟踪等模块。本文的主要贡献如下：

作者提出了GSLAMOT，该算法使用多模态信息（包括立体图像和激光点云）作为输入，利用图匹配和图优化同时进行SLAM（同时定位与映射）和3D MOT（三维运动估计）甚至在具有挑战性的场景中。

1. 作者提出了一种多准则星图关联(MSGA)，用于匹配QG和预测的TG。
2. 作者还提出了一种以目标为中心的图优化(OGO)，用于估计跟踪lets(小物体)的位姿。同时，作者提出了一种自-我融合窗口(OEFW)，用于在长期滑动窗口中联合优化目标姿态和自-我运动姿态。
3. 作者在实现GSLAMOT系统时利用并行线程来满足并发需求。实验表明，与其他开放式视觉 SLAM（同时定位与映射）系统相比，GSLAMOT在实时性能上有更好的表现。
4. 作者还利用Carla模拟器（Wang等人，2019）创建了一个交通堵塞数据集，用于目标级场景理解。该数据集涵盖了一系列地图，并具有动态和静态目标的不同数量，为这一领域的研发提供了有价值的资源。

2. Related Work

与此最相关的论文已经被审查，它们被分成了两个领域：面向目标的SLAM（OOSLAM）和3D多重目标跟踪（3D MOT）。

Object-oriented SLAM

目标导向的SLAM（OOSLAM）是物体级场景理解研究的重要分支，旨在提取、建模和跟踪环境中的静态和动态目标（Brockman等人，2019年）。早期的OOSLAM系统通常维护一个目标数据库。这些系统使用RGB-D相机作为输入，并采用ICP损失与位姿图优化相结合来解决相机和目标位姿问题。然而，基于模型的方法需要提前准备物体模型，且无法展示出对未知场景的鲁棒的可扩展性。近期OOSLAM的进步越来越集中在改进目标特征提取和建模。在 QuadricSLAM 中，目标被建模为椭球体或四次几何体，从而实现形状和尺度的更有效的优化。CubeSLAM（Wang等人，2019年）将检测边界框与图像边缘对齐，从而增强目标 Proposal 。DSP-SLAM（Wang等人，2019年）利用符号距离函数（SDF）在检测后重建目标。学习的形状嵌入作为重建过程中的先验，并在联调过程中共同优化目标的形状。在MOTSLAM（Wang等人，2019年），张等人对2D边界框、3D边界框和语义分割分别应用三个神经网络。然而，多网络方法可以从不同尺度准确提取目标的详细信息，但需要大量计算资源。DynaSLAM II 采用实例语义分割并跟踪ORB特征在动态目标上。类似地，VDO-SLAM（Wang等人，2019年）利用实例语义分割，并利用稠密光流最大化在移动目标上的追踪点数量。

然而，大部分OOSLAM系统只将物体作为观察的标志，缺乏完整的多目标跟踪过程，并且无法输出周围物体的轨迹。对于目标检测和相机位姿跟踪中的噪声，目标跟踪在SLAM设置中仍然是一个具有挑战性的问题，尤其是在物体拥挤的环境中。

3D Multiple Object Tracking

2D多目标跟踪（MOT），它涉及在图像平面上追踪物体的边界框，已经在广泛研究中展开。然而，3D多目标跟踪仍然是一项具有挑战性的任务，因为其涉及空间运动以及物体的3D外观。

3D MOT框架主要分为基于检测和基于学习的算法。前者主要利用卡尔曼滤波估计物体，并依赖特定指标（如交点与并集，马氏距离[16]或广义交点与并集[50]）进行关联。

对于基于学习的算法，许多工作将3D MOT建模为GNN，通过预测边缘来表示物体的关联信息。本文更相关的是基于几何信息的方法。极线MOT[33]只将3D框作为输入引入GNN以学习物体的几何特征。BOTT[85]依赖于自注意力来表示全局上下文信息并将框进行关联。

现有的3D MOT方法一般要么在自运动框架内工作，要么在已知自运动假设下运行。然而，在未知环境中或在存在自运动噪声的情况下，SLAM和3D MOT特别需要。因此，本文的GSLAMOT提出了一种实用的方法。

3. Approach Overview

图1显示了所提出的GSLAMOT框架。作者假设智能体在一个陌生的环境中进行自我定位、环境映射和多个物体3D姿态跟踪。智能体装备有立体相机，捕获RGB图像对，和3D激光雷达。立体图像由视觉里程计（VO）系统处理，激光雷达点云用于3D目标检测[40,36]。

对于第t帧，传感器输入表示为 。3D检测器提供目标检测结果 。VO前端输出当前自车动姿势 。检测结果 通过 转换为世界坐标系，然后构建 Query 图 。

(2)

在本帧处理中，根据运动模型预测第t个有效跟踪短语的时间，表示为 。然后构建基于 的跟踪短语图 。然后，作者使用多准则星图关联（MSGA）在 和 之间进行匹配，以获取匹配关系 。

(5)

然后，作者使用基于目标的中心图优化（OGO）优化自车的运动 ，跟踪短语 和地图点 。OGO在一个窗口大小 w 内运行，其中包括目标为中心优化窗口（OCOW）和目标自车融合窗口（OEFW）。优化的结果用于更新跟踪短语图。

作者使用Pointpillars（Shi等人，2017）进行基于激光雷达的3D目标检测，并使用ORB-SLAM（Shi等人，2017）作为视觉里程计前端。他们的结果，即 和 是后续步骤的输入。

4. Multi-criteria Star Graph Association

首先，作者介绍了TG和QG的构建方法，以及多标准星图关联（MSGA）。

4.1 数据集描述

本实验使用了多种数据集,包括标准数据集和实际场景中的数据集。下面将简要介绍数据集的情况。

4.2 实验设置

实验中使用的设置如下:

• 数据集大小:10000
• 数据预处理:数据清洗和归一化
• 模型参数:随机初始化
• 迭代次数:50
• 正则化项:L2正则化

4.2.1. Neighborhood Consistency

在第 个检测的局部星图中，边 连接顶点 和 中的 ，可以通过 到邻居 之间的相对位姿变换 表示。如果 且 相符，那么 和 的局部邻居边应高度相符。例如，如果 对应于 ，那么 应该与 一致。因此，作者通过评估边 和 的局部一致性来衡量它们的一致性：

其中 是边 的变换， 是边 的逆变换。 表示Frobenius范数。

然而，在这个阶段，作者尚未完成目标关联，也就是说，对于 Query 图 和追踪图形 的叶节点对应关系（即方程（9）中的 和 ）未知。这呈现了一个典型的鸡-蛋问题。作者提出了一种 贪心邻居策略 来解决这个问题。对于 Query 局部星图 和追踪局部星图 。作者首先假设它们的中间顶点代表同一个目标，然后如果 中的边 （方程9）对 中的边 （方程9）的高斯一致性最高（方程9），则将 和 视为对应的边。

如果最高一致性边对 和