使用非参数位姿图的物体 SLAM
Mu B, Liu S Y, Paull L, et al. Slam with objects using a nonparametric pose graph[C]//2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2016: 4602-4609.
作者：麻省理工学院航空航天控制实验室
 开源代码演示视频

其他文章：
ICRA 2019：Robust Object-Based SLAM for High-Speed Autonomous Navigation
ICRA 2019：Efficient Constellation-Based Map-Merging for Semantic SLAM
IJRR 2018：Reliable Graphs for SLAM
Resource-aware collaborative SLAM

SLAM with Objects using a Nonparametric Pose Graph

【C】穷理以致知，一文而四问

1. 针对什么问题？
- 同时考虑目标检测的数据关联和 SLAM，并如何采用非参数（物体数量、模型）的方式构造物体级的 SLAM
2. 采用什么方法？
- 在目标检测方面，利用包含更多城市、室内环境物体的 ImageNet 2014 数据集训练 R-CNN 模型进行目标检测；
- 在物体测量方面，用深度图像剔除背景点云，用点云的质心表示物体的质心；
- 将数据关联和 SLAM 问题建模成非参数的因子图 SLAM，通过输入里程计信息、物体测量最大化对数似然来优化相机位姿、物体数量、位置和类别，交替地执行数据关联的推断以及机器人和物体姿势的优化。
3. 达到什么效果？
本文非参数位姿图的方法与 FbF、OL、R-SLAM 进行了对比：
- 在仿真数据中，NP-Graph 方法获得了准确的物体数量、较小的物体位置方差和轨迹累计误差，利用了所有对象测量共同推断机器人姿势和数据关联；
- 在真实环境中，以物体的 liner 比例、物体数量、假阳物体数量、物体变动比例（检测错误）作为指标，NP-Graph 方法均达到了最好的效果，并且可以针对单个物体进行简单的可视化恢复。
4. 存在什么不足？
- 以点云质心作为物体位置的表达方式不准确；
- 以深度作为依据剔除背景点的方法太粗糙，比如图 11 左下角的键盘所在的桌面无法滤除，从而也间接影响到了物体质心的计算；
- 没有在公开数据集测试，模拟数据集结果太简单，真实数据集也没太多遮挡等干扰。

0. 摘要

在未知环境中进行建图和自身定位是许多机器人应用程序的基本功能；
- 这些任务通常涉及将物体标记为唯一特征或地标，这要求同时检测对象，然后分配唯一的标识符，当从不同的视角和不同的图像中查看时，可以保持这些标识符。
在相关文献中分别对数据关联和 SLAM 进行了独立且充分的研究；
- 但实际上这两个这两个问题是精密结合的，并且没有得到很好的解决；
- 如果没有准确的 SLAM 位姿估计，数据关联可能是混乱的，并且很容易变得难以处理；如果没有准确的数据关联，SLAM 算法的错误很容易发生。
本文提出了一种新的非参数姿态图，它在单一框架中对数据关联和 SLAM 进行建模；
- 进一步引入算法以在推断数据关联和执行 SLAM 之间交替。
实验结果表明，我们的方法具有同时关联物体检测和定位物体的新功能，导致数据关联和 SLAM 问题的性能明显优于仅考虑一个并忽略另一个的缺陷。

1. 简介

SLAM：在许多机器人应用中，例如救灾，行星探测和监控，机器人需要在没有准确的先前地图或全球位置参考（例如 GPS）的情况下自主探索未知空间；
- 机器人面临的一个基本挑战是仅使用从环境中提取的信息来有效的定位；
- 例如，识别对象实例并将它们与唯一标识符相关联的功能将使机器人能够构建环境地图并在其中进行定位；
- 构建全局地图和对机器人进行定位的问题被称为同时定位和建图（SLAM）。
传感器： 具有场景各种表现形式和不同传感器的 SLAM 已经在文献中得到了彻底的研究；
- 采用激光雷达或激光测距仪的占优网格地图可以追溯到 20 世纪 80 年代 ^[1-4]
  - 在基于占有的方法中，世界由自由空间和占用空间组成的 2D / 3D 网格表示；
  - 来自 LiDAR 或激光测距仪的新扫描的信息与先前的扫描进行比较和匹配，以增量式地构建这样的地图；
  - 这种简化的世界表示有助于高效计算，因此可以在具有单个 CPU 的相对较大的场景上达到实时性能。
  - 但是，两次扫描的成功匹配依赖于几何特征，例如角点，在缺乏这些特征的地方，例如长走廊，使用占用网格地图的 SLAM 往往会失败；
- 近年来，具有 3D 密集建图和 RGB-D 相机的 SLAM 变得越来越流行 ^[5-7]；
  - 这一系列工作能够利用来自深度相机的几何信息和来自 RGB 相机的颜色信息来重构环境的 3D 地图；
  - 将入射深度和彩色图像转换为体积或变形表面 ^{[5. Elasticfusion: Dense slam without a pose graph. 2015]}，然后与先前构建的体积或曲面相匹配，以递增地构建地图;
  - 3D 稠密地图提供具有数百万个体积或表面的环境的图像细节，但是它们严重依赖 GPU 启用的并行计算，并且不能很好地扩展。
因子图：因子图是 SLAM 问题的不同的表示 ^{[8, 11]}，不是使用诸如网格、体积或表面之类的小单位来表示空间，而是使用因子图来编码机器人的姿态和沿着轨迹所观察到的路标；
- 在因子图中，每个因子表示对两个连续机器人姿态之间或者姿态与路标之间的相对姿态的约束；
- 机器人的姿态和路标的位置被建模为随机变量，并通过最大化联合似然来优化他们；
- 可以设计机制使得仅当存在显着的姿势更新或有新的目标测量时才添加新因子，以简化表示；
- 因此，因子图 SLAM 比具有占用网格或 3D 稠密地图的 SLAM 表示方法具有更强的拓展性；
- 然而，因子图 SLAM 算法的收敛很大程度上依赖于路标的准确数据关联，即使是单个错误关联也可能导致算法发散 ^{[12, 13]}。
Object SLAM：本文对通过识别物体并利用他们的位姿（物体级 SLAM）的位置环境中的 SLAM 进行了重点关注；
- 因子图是自然的表示，因为物体可以很容易地表示为地标，并且由物体表示的地图是理想的，因为物体具有非常丰富的语义含义；
- 通过使用物体信息，机器人可以执行语义级别的任务和操作，例如在搜索森林中的人、抓取对象以及检测街道上的移动汽车；
- Object SLAM 要求机器人能够检测物体，生成测量值，并将这些测量值与唯一标识符相关联；
  - 在本文中，物体检测是指识别图像中某些预定义对象的出现的问题；
  - 物体测量是检测到的物体相对于机器人姿势的 3D 位置。
数据关联：数据关联是指将物体测量值与图像间的唯一标识符相关联的问题，目标检测问题一直是计算机视觉领域的一个重要课题。
- 深度学习方法在单图像目标测方面取得了显著效果 ^[14-18]，这些方法还具的能力：一旦检测器被训练以识别对象类，例如椅子，检测器即使在不同的形状，颜色和背景设置中也可以检测同一类的不同实例；
- 最近关于基于区域的卷积神经网络 ^[15,19]的一些工作在训练深度学习模型以检测单个图像中的多个对象实例方面取得了重大成功；
- 但是，目标检测仅给出在图像中存在某些预定义对象类的对象，但不提供图像之间的数据关联，如果在两个图像中检测到某个类的对象，则对象检测器不提供关于两个图像中的检测到的对象是否是同一对象的信息；
- 这对于 SLAM 是有问题的，尤其是当环境中存在相同目标类的多个实例时，仅使用这些模糊目标检测来实现 SLAM 的可靠性仍然是一个未解决的问题；
- 如图 1 所示，存在相同对象类的多个实例，例如椅子，机器人需要在不同视图的图像之间建立目标检测的数据关联；
- 注意，数据关联和 SLAM 本质上是耦合问题：良好的数据关联保证了 SLAM 算法的收敛，良好的 SLAM 解决方案提供了良好的数据关联初始化。
方法：本文提出了一种新的世界表示，即非参数姿态图，共同执行数据关联和 SLAM；
- 在所提出的模型中，因子图用于定位机器人和目标物体，DP（Dirichlet process，非参数模型先验）用于将检测与场景中的唯一目标标识符相关联；
- 在该算法中交替地执行数据关联的推断以及机器人和物体姿势的优化，该耦合框架达到了数据关联和 SLAM 的更好性能；
主要贡献：
- ① 创建一个将数据关联和 SLAM 耦合起来的非参数位姿图模型；
- ② 提出一种在非参数位姿图上联合推断数据关联和优化机器人位姿/目标位置的算法；
- ③ 开发一种通过深度学习目标检测从三维空间中的 RGB 和深度图像生成物体测量的方法；
- ④ 通过模拟和现实数据展示所提出的方法的性能。

1. 相关研究

物体和 SLAM 的数据关联通常作为解耦问题来解决
- 文献 [20] 表明，当 SLAM 解决方案已知时，不存在机器人姿态的不确定性，其姿态提供了物体位置的良好先验信息，并且可以实现比逐帧检测更好的回召；
- 文献 [21] 使用 SLAM 求解器构建房间的 3D 地图，然后修复地图并在房间中手动标记目标；
- 此外物体检测也可以改善位姿，文献 [22] 预先对门和椅子建模为路标，在导航阶段，在线检测这些预置的物体，并使用它们的位置信息来定位机器人。
然而，在这里考虑的场景中，物体和机器人姿势的数据关联都不是完全已知的，算法必须关联对象检测并同时执行 SLAM，解决物体检测和 SLAM 联合的算法可以分为前端方法和后端方法。

1.1 前端数据关联

在前端方法中，将新图像中检测到的物体与先前图像进行比较，如果找到新旧图像之间的匹配，则相应的物体与相同的唯一标识符相关联；
- 这些匹配通常是可靠的，因为两个连续图像之间的差异通常很小；
- 当机器人在经过长距离后回到先前访问过的地方时，必须执行高成本的全局优化以实现全局闭环；
- 前端程序的这些数据关联被认为是可靠和真实的，然后传递给 SLAM 求解器 ^[23，24]；
- SLAM ++ ^[23] 就是这样一种前端方法，创建椅子和桌子的 3D 扫描并用作模板，在测试期间观察到新的点云时，它们与预先构建的模板匹配，成功匹配的检测通常具有很高的可信度，然后在这些可靠的检测上运行 SLAM 求解器以优化对象位置和相机姿势；
- 在语义 SLAM 中，文献 [24] 创建了一个包含六个对象的库，使用 SURF 特征检测这些对象，然后运行 EKF 以同时定位机器人和地图中的物体。
在本文的工作中不是为物体创建精确的模板，而是使用深度学习来检测环境中的物体；
- 深度学习相较于基于模板的方式有更强的概括性，可以利用开源软件（数百万图像已经在线存在以创建模型），可以轻松拓展到数百个对象类而不是少数预制的模板，并且不需要场景中的物体与模板完全匹配；
- 然而，检测具有显着的误报率和部分遮挡率，因此对于产生可靠数据关联的前端算法而言是非常具有挑战性的。

1.2 后端鲁棒性 SLAM

鲁棒的 SLAM 是一系列的研究，明确使用后端方法来处理数据中的异常值；
- 在 SLAM 中，大多数物体测量已经正确地与唯一标识符相关联，当某些测量值不正确关联时，它将与同一标识符的其他对象测量值不一致；
- 相反，鲁棒 SLAM 最大化了一组在标识符和预测位置中彼此一致的测量，只有一致的测量值才会插入到 SLAM 求解器中，以恢复机器人姿势和地标位置。
本质上，鲁棒的 SLAM 依赖于以下假设：与异常值测量相比，具有唯一标识符关联的内部测量占大多数；
- 在此假设下，消除异常值之后仍然可以提供良好的 SLAM 结果；
- 但是，在物体级 SLAM 中，通常情况是存在同一对象类的多个实例，如果具有相同类的所有对象测量都与相同的标识符相关联，则不同的对象实例将始终给出不一致的测量，换言之，在物体 SLAM 中，异常值是普遍存在的；
- 如果只保留每个对象类的一组一致测量值，则该算法将消除大部分数据，并且无法识别同一类的任何重复实例。
本文提出的算法是一种后端方法，其中存在同一对象类的多个实例，对象测量与唯一标识符的数据关联被是未知的，并且必须在进行 SLAM 时建立，我们利用数据关联和 SLAM 之间的耦合，共同优化两者，并在两者上实现更好的性能。
（总结：也就是说前端数据关联是采用模板进行匹配，并认为这些测量信息是完全准确的，后端鲁棒 SLAM 是指在 SLAM 阶段对测量进行决策，确定是否数据关联，并不完全相信这些信息，还需要参与优化）

2. 利用深度学习进行物体测量

本节设置了通过深度学习生成物体测量的方法，并在末尾讨论了这种方法的局限性，强调了后端数据关联和 SLAM 算法的必要性。

2.1 基于深度学习的目标检测

目标检测是指识别某些类的对象的存在并在单个图像中找到它们的边界框的问题；
- 过去十年中的物体检测主要基于 SIFT 和 HOG 特征的使用；
- 尽管研究人员已经开发出了对单类物体检测（例如行人）表现出良好性能的算法，但是多类物体检测问题仍然很困难；
- 特别是，在 2012 年之前，最先进的方法（deformable part models）在 PASCAL VOC 2010 数据集 ^[15] 上实现了 33.4％的准确性，其中包含 20 个对象类别。
基于区域的卷积神经网络（R-CNN） ^[15] 是使用 CNN 进行物体检测的首批工作之一;
- 该算法使用选择性搜索 ^[27] 算法生成边界框提议，然后使用每个提议裁剪图像块，随后缩放图像块并通过 CNN 模型运行以进行对象检测；
- 这种方法在 PASCAL VOC 数据集上实现了 53.7％的准确率，R-CNN 非常慢（每个图像13秒），因为所有图像块都需要单独通过 CNN；
Faster R-CNN ^[19] 使用 VOC 数据集 ^[14] 进行训练，VOC 数据集中的大多数类别在城市或室内环境中比较少见，例如牛，马，绵羊，飞机和船只；
- 本文的工作在 ImageNet 2014 数据集 ^[17] 上训练了一个更快的 R-CNN 模型，其中包含与室内/城市环境更相关的类别，包括汽车，摩托车，自行车，交通信号灯，电视，椅子，花盆，杯子和键盘；
- 请注意，可以轻松修改此框架以从 ImageNet 数据集中解析出与特定应用程序相关的任何其他类子集。

2.2 物体测量

物体测量指的是相对于机器人姿势的路标的 3D 位置，为了生成这样的测量，除了物体检测之外，还需要相对于机器人的位置信息，在本文中，通过查询深度图像中的相应像素来完成的：
- ① 裁剪边界框在深度图像中对应于 RGB 图像；
- ② 过滤掉太远的背景像素；
- ③ 从 RGB 和深度图中生成点云；
- ④ 计算点云的质心作为物体的中心。
如图 2a 中可以清楚地看出，具有深度学习目标检测的物体 SLAM 具有两个主要挑战；
- 首先，存在对象类的多个实例，例如图 2a 中的“椅子”，如果没有正确的数据关联，很难区分不同的对象实例，标准位姿图 SLAM 算法只能优化具有精确数据关联的姿势，例如 g2o ^[28] ，isam ^[10] ，gtsam ^[29] ；
- 第二个挑战是高误报率，如图 2a 中检测到的椅子，深度学习算法现在报告对象，下一帧可能又没有出现，盲目地在标准 SLAM 算法中使用这些未经过滤的检测将导致“不存在”节点并导致闭环失败。
注意，在这种情况下，质心用作对象的中心，当从不同视图查看物体被部分遮挡时，质心将不是物体位置的一致度量；
- 根据我们的经验，误差可能是10-20厘米，但是我们将展示在办公室设置中，即使在明显的遮挡和视点噪声下，我们的算法仍然会收敛。

3. 位姿图背景

本节设置 SLAM 问题中使用的图模型的背景，下一节将把当前位姿图扩展为一个新的非参数位姿图，并引入一种算法来对其进行推理。

3.1 因子图

图模型是表示随机变量之间的条件依赖结构的概率模型；
- 图模型使用基于图的表示来编码高维空间上的完整分布；
- 常用的图形模型包括贝叶斯网络，马尔可夫随机场和因子图；
- 因子图是在 SLAM 问题中广泛使用的图模型，图 3 给出了因子图的一个例子，圆圈代表随机变量，方块代表因子；
- 定义 $X = X_{1}, X_{2}, \dots, X_{n}$ 为随机变量，将 $ψ_{a} (X_{{a}})$ 表示为集合 {a} 中随机变量的一个因子，联合概率可以表示为因子的乘积，如公式（1）
  - 其中 A 是所有因子的集合；
  - 每个因子 $ψ_{a} (X_{{a}})$ 将随机变量 X 的值映射为一个表示变量似然的严格正实数，它也表示了因子中变量之间的概率依赖性。
因子图是一种有效的表示，因为它捕获变量之间的稀疏性：当且仅当两个变量不属于同一因子时，给定所有其他变量，两个变量是相互独立的;
- 对数似然 $\log p (x)$ 可以用一系列的因子写成： $\log p (x) \propto \sum_{a \in A} ϕ_{a} (x_{{a}})$ ，其中 A 是所有因子的集合；
- 每个因子 $ψ_{a} (X_{{a}})$ 将随机变量的值映射为表示变量的对数似然的严格正实数；
- 使用图模型，存在快速算法来计算统计属性，例如边缘化、期望和最大似然 ^[30] 。

3.2 因子图 SLAM

由于其高效性，因子图已经在 SLAM 中得到了成功的应用和普及 ^[10，29] ，首要假设机器人能通过静态路标来定位自身。
假设 1：存在用于在环境中定位机器人的静态路标数据库，这些路标的数量和位置没有先验信息。
通过具有路标的假设，机器人在环境中移动时可以获得这些路标的测量值；
- 给定数据集，机器人的轨迹表示为离散的位姿序列，定义 T 为时间步长的总数，定义 $X_{0 : T} = {X_{0}, X_{1}, \dots, X_{T}}$ 为机器人从开始到结束的轨迹；
- 机器人的每个位姿由位置和方向组成，SE(2) 表示为 2D 位姿空间， SE(3) 表示为 3D 位姿空间；因此将 $X_{t} \in S E (2)$ 应用于 2D 空间， $X_{t} \in S E (3)$ 应用于 3D 空间；
- 在不使用 GPS 的环境中，这些位姿不能直接观察到，但是机器人可以通过 IMU 或车轮编码器测量两个连续姿态之间的增量变化，这称之为里程计；
- 定义 $x_{t - 1}$ 和 $x_{t}$ 之间的里程计测量为 $o_{t}$ ，在添加高斯噪声的标准假设下，t 时刻的里程计测量可以表示为公式（2）
  - 其中 $⊖$ 是在 SE(2) 或 SE(3) 空间中取两个时刻相对位姿的操作符， Q 是里程计的噪声协方差；
- 然后给出两个位姿之间里程计 $o_{t}$ 的似然为公式（3）
在导航过程中，机器人还会观察环境中的路标，假设环境中存在 M 个路标（这可能是未知的），路标的位置表示为 $L = {L_{1}, L_{2}, \dots, L_{N}}$ ，在2D 空间中 $L_{i} \in R^{2}$ ，在 3D 空间中 $L_{i} \in R^{3}$ ；
- 在 t 时刻，机器人获得 $K_{t}$ 个路标的测量值，表示为： $z_{t} = {z_{t}^{1}, z_{t}^{2}, \dots, z_{t}^{K_{t}}}$ ；
- 每个测量值与唯一的路标标识符相关联，关联被定义为 $y_{t} = {y_{t}^{1}, y_{t}^{2}, \dots, y_{t}^{K_{t}}}$ ，其中 $y_{t}^{i} \in {1, \dots, M}$ ，比如在 0 时刻，物体获得了两个测量 $z_{0} = {z_{0}^{1}, z_{0}^{2}}$ ，并且这两个测量来自于路标 5 和 7，所以 0 时刻的数据关联为 $y_{0} = {y_{0}^{1}, y_{0}^{2}} = {5, 7}$ ；
使用标准模型，物体测量 $z_{t}^{k}$ 被加性高斯噪声干扰，表示为公式（4）；给定机器人位姿、路标关联和路标位姿情况下，测量 $z_{t}^{k}$ 的似然为公式（5）
- 其中 R 是测量的噪声矩阵；
通过联立公式（2）和（4），里程计和路标测量的联合对数似然为公式（6），根据高斯噪声的概率分布公式，可知两个因子都是服从二次型的，如公式（7）；
位姿图 SLAM 问题通过最大化对数似然公式（8）来优化机器人位姿 $X_{0 : T}$ 和物体位置 $L$ ；
- 注意对数似然在 $X_{t}$ 和 $L_{t}$ 中是非线性的，因为 $⊖$ 在公式（2）（4）中是非线性运算。
SLAM 的因子图表示也称为位姿图，图 4 展示了这种位姿图，变量表示机器人或路标的位置，因子包括里程计和路标测量。

4. 非参数位姿图

本章通过将当前位姿图扩展为新的非参数位姿图来建立联合数据关联和 SLAM 问题，该姿势图将物体关联与机器人位姿紧密耦合，还引入了一种新算法来联合推断数据关联并使用这种新模型执行 SLAM。

4.1 具有多类别物体的因子图

在进入不完全数据关联的非参数因子图之前，首先需要注意的是，在 SLAM 中除了测量物体的三维位置之外，还需要观察物体的类别，并且所观察到的类别并不总是可靠的，因此首先建立物体类别的概率模型：
- 假设总共有 N 个物体类，对于物体 i，定义 u 表示对物体类别的观测；
- u 的似然使用分类分布来建模为公式（9）
注意 1 ≤ u ≤ N，但本文特别设计了 $π_{i} (0)$ 来表示误报的概率，这种设计有助于算法在实际实验中过滤掉不存在的物体检测。
为了获得封闭的更新形式，对类别 i 的似然 $π_{i}$ 应用 Dirichlet 先验，见公式（10）
- 当观察到类 j，即 u = j 时， $π_{i}$ 的后验分布为公式（11），其中 $e_{j}$ 表示第 j 个元素为 1 的单位向量
注意 $β_{i} (0)$ 表示物体 i 为假阳性的初始似然；
- 由于观测不能为 0，当物体 i 的观察越来越多时，后验 $β_{i} (0)$ 将单调递减，这与直觉相一致，即如果从某个对象获得重复观察，则它具有较低的假阳性。
将多类别概率设置与原始 SLAM 问题相结合：
- 每个物体测量将成对 ${z_{t}^{k}, u_{t}^{k}}$ ，其中连续变量 $z_{t}^{k}$ 表示 3D 位置测量，离散变量 $u_{t}^{k}$ 表示观察到的物体类；
- 回忆前面， $y_{t}^{k} = i \in {1, \dots, M}$ 表示在 t 时刻的第 k 次测量来自物体 i；
- $u_{t}^{k}$ 是来自后验分布 $π_{y_{t}^{k}}$ （公式 12）的样本：
里程计、路标测量、物体类别的联合对数似然变成公式（13）
新的优化问题变成了通过优化相机位姿、物体测量和物体类别来最大化对数似然，见公式（14）
与公式（8）相比，公式（14）中的观测数据还包括物体观察 $u_{0 : T}$ ，并且在要估计的变量还包括物体类别 $π$
- 根据公式（13），给定数据关联 $y_{0 : T}$ ，联合似然可以被分解为 $z_{0 : T}$ 和 $o_{0 : T}$ 的似然之和和 $u_{0 : T}$ 的似然；
- 因此，物体类别独立于机器人的位姿和物体的位置；
- 公式（14）的优化等同于解决问题（8）并独立地计算物体类别的后验。

4.2 非参数位姿图

现在研究数据关联 $y_{t}^{k}$ 未知且必须建立的情况。

基于深度学习的算法将每个物体标记为某一个类，但不区分同一类的不同实例，当存在同一对象类的多个实例时，例如房间中的多个椅子，数据关联变得具有挑战性；
- 本文使用后端框架来联合推断数据关联和物体位置，而不是依赖可靠的前端过程来关联对象；
- 注意，由于数据关联的模糊，物体的总数 M 提前未知，因此也需要建立；
- 非参数模型是一组使模型复杂性适应数据的工具，它具有嵌入机制，当观察到新数据时，模型参数可以增长；
- 特别地，Dirichlet Process (DP) 是这样的非参数随机过程，其模拟离散分布但具有灵活的参数大小，它可以作为具有无限维的 Dirichlet 分布的推广；
- 与 Dirichlet 分布相同的是分类分布的共轭先验，DP 可以被视为无限非参数离散分布的共轭先验 ^[31] ；
- 本文中使用 DP 作为数据关联 $y_{t}^{k}$ 的先验，假设在任何点上，总共检测到 M 个物体， $y_{t}^{k}$ 属于物体 i 的概率为公式（15）
  - 这个模型后背的思想是： $y_{t}^{k}$ 来自于某个现有物体 i ≤ M 的概率与物体 i 的测量次数成正比；来自于新物体的概率与 DP 先验的浓度参数 $α$ 成正比。
里程计 $o_{0 : T}$ 、物体测量 $z_{0 : T}$ 和给定数据关联 $y_{0 : T}$ 的物体类别 $u_{0 : T}$ 的联合对数似然可以写成以下形式，其形式与公式（13）相似，但在这个公式中，物体测量 $z_{0 : T}$ 的似然与物体类别 $u_{0 : T}$ 的似然通过数据关联 $y_{0 : T}$ 联系起来。
新的优化问题涵盖了里程计、物体测量和给定数据关联的物体类别的联合对数似然：
与公式（8）相比，新的优化问题公式（16）更具挑战性，因为数据关联是未知的，从而物体测量的对数概率不再具有简单形式，并且问题公式（16）变为混合整数非线性问题（不再像之前可以独立）；
- 其次，环境中的真实对象的数量 M 不一定是先验已知的，公式（16）还必须同时推断 M。

4.3 非参数 SLAM

本章最后一部分，对于时间 $t = 1, \dots, T$ 和时间 t 下观察到的物体 $k = 1, \dots, K_{t}$ 生成以下的问题模型
- 其中 $α, β, Q, R$ 是给定的参数；
- 机器人的位姿 $X_{0 : T}$ 、路标的位置 L、物体的类别分布 $π_{1 : M}$ 和物体数据关联 $y_{0 : T}$ 是需要估计的变量；
- 里程计 $o_{0 : T}$ 、物体测量 $z_{0 : T}, u_{0 : T}$ 是观测数据。
与规范的 DP 混合模型不同，观测数据 $o_{0 : T}, z_{0 : T}, u_{0 : T}$ 不是给定变量 $X_{0 : T}, L, π$ 的独立样本，而是通过因子图相关联的；
- 因此，推论涉及计算因子图上的最大似然，当要建立关联和变量时，标准方法在分配数据和优化变量之间交替；
- 在已知物体编号 M 的情况下，K-means 具有确定性数据关联，而期望最大化以概率方式关联数据 ^[30]；
- 当知道先验物体数量并且使用 DP 作为先验时，广泛使用马尔可夫链蒙特卡罗方法（例如，吉布斯采样）或变分推理算法 ^[30]；
- 然而，在这些算法中，需要始终计算和跟踪每个标签 $y_{t}^{k}$ 为任何潜在对象 L 的似然，算法需要多次遍历所有数据以收敛到稳态分布，而数据在我们的问题中的大规模和强烈依赖性使这些方法不合适。
在文献 [32] 中显示，在小方差假设下，吉布斯采样可以简化为 DPmeans；
- 如果似然在某个特定阈值内，则将 $y_{t}$ 指定为最大似然对象，而不是对后验分布进行采样；不在该阈值则将其分配给新对象；
- 直观地，在这种情况下，小方差意味着测距，物体测量和物体类别中的噪声相对较小，因此 $y_{t}$ 的后验分布是最高的。
假设 2：里程计，物体测量和物体类别的差异很小，因此数据关联的后验分布具有很小的方差和唯一的最大似然值。
DPmeans 算法在两个步骤之间交替：① 通过变量 $X_{0 : T}, L, π$ 最大化似然；② 并将数据关联 $y_{0 : T}$ 分配给它们的最大似然对象。算法 1 显示了该方法的总体流程，以下解释了算法的步骤：
步骤一：初始化（line 1）
- 在初始化中，所有 $y_{t}^{k}$ 都被设置为它自己的对象；
- 机器人位姿 $X_{0 : T}$ 和物体的位置 L 初始化他们的开环估计；
- 对象类的 Dirichlet 分布先验被设置为某个初始值 $β_{0}$ 。
步骤二：优化数据关联（line 3）
- 在执行主循环时，算法在优化数据关联 $y_{t}^{k}$ 和优化变量 $X_{0 : T}, L, π$ 之间交替进行；
- 在优化物体关联时，修正 $X_{0 : T}, L, π$ ，计算 $y_{t}^{k}$ 的后验作为 DP 先验（公式 15）和测量 $(u_{t}^{k}, z_{t}^{k})$ 似然（公式 2,21）的乘积，如公式（18）
- 然后将数据关联 $y_{t}^{k}$ 分配给最大似然对象，见公式（19）。
步骤三：位姿优化（line 10）
- 在优化位姿时，数据关联是固定的，可以用公式（20）来更新物体类的 Dirichlet 分布的后验参数
- 利用 Dirichlet 先验 $D i r (β_{i})$ ，每个物体类别的最大似然（ML）与参数 $β_{i}$ 成正比，如公式（21）所示；
- 然后通过标准 SLAM 求解器获得机器人位姿 $X_{0 : T}$ 和物体位置 L 的最大似然（参见公式（8））。
步骤四：剔除假阳性（line 18）
- 前面已经设置了对于物体 i 为假阳性的概率 $π_{i} (0)$ ；
- 在初始化的时候 $β_{i} (0)$ 被设置为某个正数；
- 当物体 i 的新的测量被观测并且累积， $β_{i}$ 得到更新，使得 $β_{i} (j), j > 0$ 变得比 $β_{i} (0)$ 大，结果 $π_{i} (0)$ 单调递减；
- 组后通过简单地在 $π_{i} (0)$ 上设置阈值 $ϵ$ 来剔除假阳检测。

5. 实验

5.1 模拟数据集

在模拟中，在 2D 平面中随机生成 15 个物体，它们被随机分配到 5 个不同的物体类别：红色菱形，蓝色圆形，绿色三角形，黄色星形和洋红色方块，手动设计机器人轨迹并多次穿过环境，图 7a 显示了生成的数据集的 ground truth；
- 在每个位姿 $X_{t}$ 处，机器人观察其视野内的相对位置 $o_{t}^{k}$ 和物体的类别 $u_{t}^{k}$ ；
- 高斯噪声被添加到里程计测量和物体测量中（见公式 2 和 4），表 1 列出了模拟数据集包含的参数。
实验结果：
- 图 6a 中显示了仅基于开环里程计的物体预测，这些预测的物体位置存在大量的方差和漂移，这模糊了环境中实际存在多少物体；
- 图 6b 是第一次迭代的结果，非参数位姿图聚类测量并使用它来校正机器人位姿，物体数量减少到 33；
- 图 6c 是第二次迭代的结果，进一步将物体的总数减少到 20；
- 图 6d 是第三次迭代的结果，算法收敛到真实的物体数量 15 个。
将提议的非参数位姿图方法（NP-Graph）的性能与三种方法比较：
- ① 逐帧检测（FbF）：每帧中的物体都被视为一个新的观测，即没有数据关联（图 6a）；
- ② 开环目标检测（OL）：使用机器人里程计在图像之间执行数据关联，但不使用数据关联结果来纠正机器人位姿（图 7c）；
- ③ 鲁棒性 SLAM（R-SLAM）：后端算法，可以找到最大的一致性测量集，并消除不一致的测量（图 7b）。
图 7 和表 2 比较了四种不同算法的 SLAM 性能，图 8 展示了机器人轨迹与 ground truth 相比的累积位置误差，而图 9 对 R-SLAM 和 OL 方法的各种参数进行了设置比较其物体数量和它们的定位误差；
- FbF 和 OL 纯粹依赖于里程计并且不能纠正机器人姿势，因此具有最大的误差;
- R-SLAM 使用物体测量的子集来达到机器人位姿的闭环，因此误差较小；
- 本文 NP-Graph 的方法利用了所有物体测量，因此在机器人姿态和物体位置上具有最小的误差；
- FbF 不进行任何数据关联，因此显著高估了对象的数量；
- OL 方法不优化机器人位姿，当机器人返回到原点时，里程计显著漂移，因此 OL 方法无法将物体与之前观察到的对象相关联，同时 OL 方法也过度估计了对象的总数；
- R-SLAM 仅为每个对象类保留一组一致的度量，因此它只能为每个对象类检测一个实例，并且显着低估了对象的总数；
- 另一方面，NP-Graph 利用所有对象测量并共同推断机器人姿势和数据关联，因此可以准确地推断出正确的物体数量。

5.2 办公室环境

为了测试真实场景中的性能，收集了办公环境的数据集，并使用深度学习来检测物体，如椅子，屏幕，杯子等。有关办公室数据集的统计信息如表 3 所示。
表 4 和图 10 比较了 FbF，R-SLAM，OL 和本文 NP-Graph 的性能，虽然物体位置的 ground truth 不适用于此数据集，但比较了有效物体的数量，inlier 测量数量和物体位置方差的性能；
- 当一个物体的误报概率 $π_{i} (0)$ 低于一个阈值（ 2％）时，该对象被定义为有效，否则它被标记为误报；
- 当测量与有效物体相关联时，测量结果表示为 inlier；
- 物体方差由物体的预测位置的不确定性与其相关测量值确定；
- 从表 4 可以看出，NP-Graph 具有最高的 inlier 测量比例，最接近真实的物体数量，以及物体位置上具有最小方差。
虽然没有机器人位姿的 ground truth，但我们也定性地比较了性能;
- 图 1 显示了环境的俯视图以及机器人轨迹；
- 图 10 比较了 4 种方法的结果。 FbF 和 OL 估计是开环方法并且过度估计物体的总数，R-SLAM 仅使用物体测量的子集，它只能为每个对象类识别一个实例，并且即使机器人位姿产生闭环也会有错误的估计；
- 另一方面，NP-Graph 能够使轨迹闭环并恢复角落的转弯；
- 虽然办公室数据集中没有计算物体定位误差的 ground truth，但值得注意的是，在最左下角的衣架上挂着毛衣，我们的算法能够恢复其距离而其他方法则失败。
图 11 展示了检测到的和良好关联的物体的一些示例，其包括椅子，屏幕，键盘，玩具车和悬挂在角落的毛衣；
- 这些图是从与相应物体相关联的单个边界框的点云中提取的；
- 请注意，这些点云仅用于说明目的，但不在算法中维护，该算法仅使用这些点云的质心作为对象测量。

6. 总结

物体 SLAM 具有挑战性，因为数据关联是模糊的，位置测量未知；
- 数据关联和SLAM本质上是耦合问题，这项工作提出了一种新的非参数位姿图方法，它紧密地耦合了这两个问题，并开发了一种算法在推断数据关联和执行 SLAM 之间交替进行；
- 仿真和真实数据集都表明我们的新方法具有同时进行数据关联和 SLAM 的能力，并且在将物体检测与唯一标识符和定位物体相关联时实现了更好的性能。

【R】参考文献

[11] Kaess M, Johannsson H, Roberts R, et al. iSAM2: Incremental smoothing and mapping using the Bayes tree[J]. The International Journal of Robotics Research, 2012, 31(2): 216-235.
因子图，iSAM2：使用贝叶斯树进行增量平滑和建图
[14] Everingham M, Van Gool L, Williams C K I, et al. The pascal visual object classes (voc) challenge[J]. International journal of computer vision, 2010, 88(2): 303-338. VOC 数据集
[15] Girshick R, Donahue J, Darrell T, et al. Rich feature hierarchies for accurate object detection and semantic segmentation[C]//Proceedings of the IEEE conference on computer vision and pattern recognition. 2014: 580-587. 区域的卷积神经网络（R-CNN）,使用 CNN 进行物体检测的首批工作之一
[19] Ren S, He K, Girshick R, et al. Faster r-cnn: Towards real-time object detection with region proposal networks[C]//Advances in neural information processing systems. 2015: 91-99. Faster r-cnn
[20] S Pillai and J. Leonard. Monocular slam supported object recognition. In Proceedings of Robotics: Science and Systems, Rome, Italy, July 2015.
支持物体识别的单目 SLAM，相机位姿为物体位置提供先验
[21] S. Song, L. Zhang, and J. Xiao. Robot in a room: Toward perfect object recognition in closed environments. CoRR, abs/1507.02703, 2015. 在室内环境中实现完美的物体识别
[22] Atanasov N, Zhu M, Daniilidis K, et al. Semantic Localization Via the Matrix Permanent[C]//Robotics: Science and Systems. 2014, 2. 预置物体作为路标约束 SLAM 的位姿
[29] Dellaert F. Factor graphs and GTSAM: A hands-on introduction[R]. Georgia Institute of Technology, 2012. 开源因子图优化框架 GTSAM 代码：http://borg.cc.gatech.edu/
[31] Ferguson T S. A Bayesian analysis of some nonparametric problems[J]. The annals of statistics, 1973: 209-230. 一类非参数问题的贝叶斯分析

【Q】问题

4.1 节中 $π_{i} (0)$ 是如何表示误报的概率的？
公式（10）的 Dirichlet 先验？还有随后的 Dirichlet Process (DP) ，categorical distribution；
公式（13）是不是少写了个 π？
里程计/SLAM 部分采用的什么框架方法呢？全文也没提到；
物体路标如何校正轨迹漂移也没提，把物体质心当做点来处理作为观测？
5.1 节的模拟数据是如何生成的？有什么工具吗？

【T】思考

文章要求解的两个问题建模成了联合似然公式（6）和公式（13）（联合里程计、物体测量和物体类别），通过优化变量最大化联合似然

wuyanminmax@gmail.com
2019.05.07