使用物体补充的 BA 来恢复单目 SLAM 的稳定尺度
Frost D, Prisacariu V, Murray D. Recovering stable scale in monocular SLAM using object-supplemented bundle adjustment[J]. IEEE Transactions on Robotics, 2018, 34(3): 736-747.
作者：Duncan Frost：牛津大学2017年博士毕业，好像是 PTAM 那个组的  谷歌学术
期刊：IEEE Transactions on Robotics  JCR 类别：ROBOTICS  排序：2/26   JCR分区：Q1   IF：4.684
文章：作者 2016 年 Object-aware bundle adjustment for correcting monocular scale drift

Recovering stable scale in monocular SLAM using object-supplemented bundle adjustment

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

• 1. 针对什么问题？
  • 如何在不使用恒定高度假设的情况下解决远距离、大规模的单目尺度漂移;
  • 如何同时保证地图的全局尺度一致性又减小累计的漂移。
• 2. 采用什么方法？
  • 之前错误的尝试还是仅仅使用的点之间的约束，只不过这些点在一个尺度范围内；本文新提出的方法使用物体作为路标进行约束，由于只用于校正尺度漂移，没必要构造复杂的物体模型和 6（甚至 9 ）自由度的表示，仅使用物体质心 + 半径（先验）的方式约束，将直接的测量（边界框的中心 + 边界框大小）与三维世界中的物体投影得到的（质心投影和物体宽高投影）作差，最小化。
• 3. 达到什么效果？
  • 相较没有物体尺度约束的情况大大降低了尺度漂移，保证了地图的一致性；
  • 没有高度假设约束的情况下平均产生 20％ 的平移误差和 0.014°/m 的旋转误差；
  • 只要可以获得常规的物体观测，该方法就能够在整个轨迹上保持一致的尺度估计，在缺乏物体观察发生尺度漂移时，当重新引入物体时，漂移会迅速减少；
  • 本方案避免对物体方向的恢复，物体的 BA 结构与常规场景点的结构联合，导致计算复杂度没有增加并且计算成本仅适度增加。
• 4. 存在什么不足？
  • 对比先进的方法没有优势，虽然说是没有使用高度假设；
  • 物体表示上，首先二维的表示不准确，可以考虑三维建模；其次就算使用二维，还是用的半径 1.2 m 表示尺寸，也有漏洞，Cube SLAM 里的 2D 方案使用约束为： w = 3.9 m；l = 1.6 m；h = 1.5 m；
  • 还是需要有先验知识（哪怕是粗略的），应用场景比较受限，换个场景就需要新的尺寸约束，特别是多物体更要有不同的约束；
  • BA 优化函数虽然是利用了点测量和物体测量来优化相机位姿，但是没有反过来促进目标检测（优化物体的位姿和尺寸），这是一个可以创新的点。

0. 摘要

• 问题：在不了解图像之间的绝对基线的情况下，来自单目 SLAM 的地图尺度会随着时间的推移产生巨大的漂移；
• 方法：本文除了点测量之外还考虑利用物体检测来解决尺度模糊和漂移的问题；
  • 通过对物体的大小进行预测，可以将尺度估计集成到 BA 约束中；
  • 当物体的观测可用时，在局部 BA 中 与相机位姿一起确定地图的尺度；
• 优势：本文方法没有严格限制，比如恒定的相机高度或平面道路假设；
• 实验：KITTI 数据集显示可以减少总长度为40 km 的远程室外序列的尺度漂移
  • 由于知道物体的绝对尺度，因此可获得所有序列的度量精度（metric accuracy）；
  • 还对来自手持相机的视频片段进行定性评估。

---

1. 简介

• 累计误差的产生：传感器测量对传感器位姿和场景结构的联合依赖性都是未知的，因此，当传感器远离起始位置时，SLAM 算法将会产生累积误差；
  • 当使用来自 LiDAR，RGBD 或立体相机的方向和距离测量时，这对继续探测的能力几乎没有影响；
  • 当仅使用单个摄像机的方向测量时，误差与深度/速度缩放模糊度的漂移相混合，并且可能完全使数据收集失效；
    • 例如，基于关键帧的方法通常基于它们与最近邻的接近度来选择关键帧；
    • 如果速度估计太大，关键帧将创建得太快，如果太小，则根本不会创建关键帧；
  • 虽然所有的方法都能够在闭环时减少和重新分配累积的误差 ^[4-8]，但是单目方法中的漂移太严重，以至于在地图中搜索之前观察到的地标是不可行的，尽管在使用几何和外观线索 ^[9] 方面取得了进展。
• 物体尺度矫正漂移：但是为什么人类只用一只眼的时候行走的时候也不会产生尺度漂移？一个关键的假设是，现实中物体大小短时间内在空间尺度上不会变化；
  • 本文利用路标点和合适的物体表示，修改的 BA 可以通过检测和跟踪已知大小分布的物体实例并将它们包括在地图内，来有效地减小尺度漂移；
  • 由于对尺度的持续估计在长距离中至关重要，本文倾向于在局部调整中经常性地矫正漂移，而不是在全局优化中一次性的矫正漂移。

---

2. 相关工作

• 近期对 SLAM 中的现有技术以及特别是单目 SLAM 的描述分别在文献 [11,12] 中有描述，这里本文只关注先前减小尺度不确定性的工作；
  • 方法主要分为两类：使用来自附加传感器的信息，或者像本文一样对相机的环境进行假设。
• 第一类：附加传感器信息
  • 添加第二个相机（即双目）^[3,13]，如果已知相机的基线，则立即消除尺度；
  • IMU，提供加速度和方向；
    • 帧被聚集成时间窗口，每个窗口保持通过对加速度计的输出进行双重积分而获得的估计轨迹；
    • 文献 [15] 将 IMU 融入到 PTAM 中；
    • 文献 [17] 利用 EKF 组成视觉、IMU 和压力计；
    • 文献 [18] 使用 IMU 数据，完成位姿估计用于行人导航。
• 第二类：对相机的环境进行假设
  • 使用视觉里程计的车辆导航中的一个常见假设是摄像机处于地面以上的固定且已知的高度^[19-22]；
    • 通过检测地平面，校正地图的局部比例以确保相机高度保持在其已知值；
    • 必须精确地确定平面：例如文献 [23] 通过跟踪可能在其上搁置的物体来检测它；如后面的实验所示，当这个假设成立时，这些受约束的方法表现优异，但不能以其他方式运作。
  • 假设场景中包含离散的重复结构，文献 [24] 使用从图像测量的特征之间的距离来记录场景结构的描述符，当再次出现类似的特征图案时，可以校正尺度漂移，由于没有使用实际尺寸的先验知识，因此不能恢复绝对尺度；
• 位姿图与 BA：文献 [25] 描述了一种能够识别尺度漂移的 SLAM 方法，虽然位姿图优化 ^[26,27] 具有比完整 BA 低得多的计算复杂度，但它无法校正闭环上的尺度漂移；
  • 通过在关键帧对之间施加相似性而不是刚体约束来弥补这一差距，当检测到闭环时，通过比较重叠结构来测量漂移；此信息被输入闭环约束的比例因子，该约束因子在优化后传播到闭环的其余部分；
  • 虽然该方法显示出对刚体姿势图优化的改进，但该方法是否优于完整 BA 的问题仍未得到解答。更重要的是，仅在闭环时校正比例，使其不适合开环摄像机轨迹。
• EKF 与 BA：文献 [28] 将尺度不变特征集 ^[29] 识别的平面物体合并到使用 monoSLAM 估计的地图中 ^[30,31] ,实时检测物体，估计并分解平面和相机之间的单应性，将物体的位置由 monoSLAM 的 EKF 结合；
  • 由于每个对象实例的大小已知，该方法可以解决深度/速度缩放模糊度的问题；
  • 但尽管能够从对象测量设置全局尺度，但在 EKF 中校正尺度漂移被证明是不方便的，随后在地图中结合的物体测量值被发现与周围地标的比例越来越不一致，导致跟踪失败或拒绝物体检测；
  • 在后来的工作 [32] 中，他们使用了 BA 优化而不是 EKF，但是对象仅用作增强而不是用于缩放校正；
  • 文献 [33] 也使用了 EKF 并用完整的三维物体增强了它们的地图，但他们也考虑了特定的物体而不是某一类对象。
• 利用物体约束改进地图一致性：
  • 文献 [34] 联合估算了 BA 约束中相机，点和矩形物体位置，他们的目标是提高批量处理 SFM 的精度；
  • 文献 [35] 有类似的目标，但采用增量 SLAM 框架（尽管在小环境中并使用 RGBD 图像），并建议与对象的特征数据库进行低级匹配，而不是使用对象检测器；
  • 以上两项工作都没有关注如何纠正长期 SLAM 轨迹的尺度；
  • 文献 [36] 建议将点云建模的物体添加到地图中，其中对象点之间的已知距离用于向 BA 中添加额外的几何约束并在地图中强制执行缩放；
    • 由于该方法使用特定对象实例的库，因此其对更一般场景的适用性是不确定；
    • 虽然地图的全局尺度在精确度上是准确的，但也没有显示长期无漂移操作的结果；
    • 在下一节中，本文追求在路标点之间使用成对约束的想法，但发现它们对于避免实时系统中的尺度漂移是无效的。
  • 文献 [37] 使用单目方法生成密集的地图，后来使用嵌入低维潜在空间的三维形状先验进行细化，由于形状的尺度是已知的，因此可以设置地图的尺度；
    • 然而，与 [36] 一样，该方法仅在一个小的局部地图而不是长距离数据中进行测试，并且不清楚物体是否能够校正漂移而不仅仅是设置全局尺度。
• 本文方法：借鉴物体补充地图的方法，优先对传感器位置施加某种形式的约束或使用额外的传感硬件；
  • 为了避免依赖于特定的物体实例，将使用对象类（object classes），因为它们的普遍性和实际大小的低差异性，同时它们的尺寸分布事先确定，以提高长期准确性；
  • 还提出使用与在线场景兼容的最小对象表示和使用 BA 约束的相机定位。

---

3. Landmark-to-Landmark(L2L) 约束：一种失败的表述

• 思想： 物体的大小可以调节尺度，但并不能解释如何最好地将大小信息合并到 BA 中；
  • 在文献 [36] 之后，我们首先测试了物体提供一个度量精确的坐标系统的建议，在该坐标系统上可以计算出地标之间的预期距离。
• ① 路标点重投影误差： 考虑在一组位姿为 \(\mathcal {T} = \lbrace {\texttt{T}}_{k}\rbrace,k = 1,\ldots,K\) 的相机关键帧中观察到世界坐标系中的一组路标 \(\mathcal {X} = \lbrace {\mathbf {X}}_{iw} \rbrace,i=1,\ldots,I\)；
  • Vanilla BA 旨在寻找具有最小化总重投影误差的路标位置和相机位姿：图像测量和相机第 k 帧中路标 i 的预测投影之间残差的加权 2 范数 \(\tilde{{\mathbf {x}}}_{ik} = ({\mathbf {x}}_{ik}-\hat{{\mathbf {x}}}({\mathbf {X}}_{iw},{\texttt{T}}_{k}))\) ;
  • 假设误差通常以零均值和协方差 \({\texttt{X}}_{ik}\) 分布，则 BA 提供相机位姿 \(\mathcal {X}\) 和路标位姿 \(\mathcal {T}\) 的最大似然估计：
• ② 路标间的距离误差： 使用 L2L 约束要求路标 \(i\) 与 \(i^{\prime }\) 之间的距离 \(d_{ii^{\prime }}\) 从一些正态分布的先前已知平均值 \(\mu _{ii^{\prime }}\) 和方差 \(\sigma ^2_{ii^{\prime }}\) 得出；
  • 对所有这些可用的约束进行求和得到代价函数： \[ E_{\mathrm{constraint}} = \sum _{i,i^{\prime } \in \mathcal {X}} \frac{1}{\sigma ^2_{ii^{\prime }}}(d_{ii^{\prime }} - \mu _{ii^{\prime }})^2 \quad (2) \]
• ③ 联合进行 BA： 将公式(1) (2)的反投影误差和路标距离误差进行 BA 调整： \[ {\color{Blue} \lbrace \mathcal {X}, \mathcal {T}\rbrace ^\ast = {\arg\,\min}_{\lbrace \mathcal {X}, \mathcal {T}\rbrace } \left[ E_{\mathrm{reproj}} + \lambda E_{\mathrm{constraint}}\, \right] \quad(3)} \]
• 举例：从图像获取 L2L 约束需要在关键帧中检测已知对象类的实例，比如图 1(a) 中人的实例；
  • 基于部分的目标检测器（例如文献 [38]）在检测周围提供检边界框，并根据目标宽度和高度的预先计算分布，假设平面性（假设目标中路标点在一个竖直平面上），通过二维投影获得目标内路标之间的距离约束；
  • 例如，在图 1(a) 中，人类肩部和脚部之间的合理距离允许两个地标之间的距离计算为1.56m。
• 实验结果： 该方法首先在已知真值（比如人的高度）的模拟中进行测试，在轨迹的末端（其中每个关键帧可获得一个约束），执行无约束和 L2L 约束的 BA；
  • 图1（b）和（c）分别比较了每个关键帧的相机平移产生的开环轨迹和闭环轨迹的误差，这些约束显然减少了每个关键帧错误，使其在整个轨迹上保持大致恒定。
  • 总结： 意思就是将已知真值的物体路标作为一个尺度约束，物体上的路标点（同一帧）之间是有一个合适的距离的，比如最大不超过人的高度，将路标之间的距离按照公式（2）产生距离误差；物体之外的路标点没有尺度约束，随着时间的推移尺度会发生漂移（特别是在没有闭环的情况下），而物体内的路标点，不管在什么时刻，其路标点之间的距离均值还是稳定的，尺度也就没产生漂移。“物体提供一个度量精确的坐标系统”的意思就是相当于把物体当做一把大的尺子（假设都投影到尺子平面上，咦？要是是前景呢？？），不管第几帧，它上面的路标点始终逃不脱它的手掌。
• 问题： 尽管存在这样的约束，但 L2L 约束法则在改进的 PTAM 测试中存在两个问题：
  • 首先，获得约束所需的平面假设过于严格；
  • 其次，更重要的是，在处理过程中一段时间内会出现的限制，以及在尺度漂移之后，无法恢复尺度；被约束受限的路标将快速移动以跟随它们，但是其位置的相应大的变化经常导致 BA 忽略了将它们作为异常值剔除而不是重新缩放地图。
• 结论： 这些问题与文献 [28] 提到的类似，当将路标约束强加到 EKF SLAM 中时，两种结果都表明：
  • 低水平的突然干预对指导优化的概率基础的破坏性太大，无论是通过卡尔曼滤波器递归还是通过 BA 的批量处理模式，两者都建议避免将关于对象大小的有些粗略的信息直接转移到已经在地图中具有精细尺度的路标点上。

---

4. 利用物体路标的 BA

• 第二节回顾了 SLAM 中引入的一些参数化的物体对象，包括从文献 [39] 的边界框表示到 [35] 和 [37] 中的三维表面模型；
  • 虽然更复杂的模型允许准确的分割，并且可以在本地提高重建质量，但是它们施加了相当大的计算开销；
  • 即使是矩形边界框也需要保持完整的6自由度姿势，而 3D 表面模型需要正确地定位 2D 轮廓分割；
  • 虽然这样的重建效果在视觉上很震撼，但如果唯一的目标是尺度校正，则不需要额外的复杂度，相反，在这里本文提出将物体和点表示为具有不同“范围”的通用地标。

4.1 物体表示

• 世界系：场景和图像中物体和点路标的组合表示如图 2 所示，两者都由最小的包围球体表示，并且具有单个附加尺寸参数，即球体的半径，将其称为范围 \(\epsilon\)；因此，物体路标 \(j\) 在世界系中表示为（物体路标的质心+范围）： \[ {\color{Blue} {\mathbf {Q}}_{jw} = {\left[\begin{array}{c}{\mathbf {X}}_{jw}\\ \epsilon _j\end{array}\right]} \quad(4)} \]
• 相机系：由于物体路标被认为是球形，因此坐标系之间的转换比较简单：范围是不变的，物体在世界系的表示转换到相机系中：
• 图像平面： 在相机第 k 帧下，物体路标投影到图像平面中用四维向量表示 \(\hat{{\mathbf {q}}}_{jk} = [u,v,w,h]_{jk}^\top\)：

4.2 物体测量与数据关联

• 使用在创建时应用于每个新关键帧的检测器来对物体进行定位，其测量值从检测周围的边界框中获得，并以与物体重投影 \(\hat{{\mathbf {q}}}_{jk} = [u,v,w,h]_{jk}^\top\) 相同的方式对路标进行参数化；
• 为解决关键帧之间的数据关联，使用文献 [40] 和 [41] 中描述的方法。

4.3 Object Bundle Adjustment

• 物体 BA： 如果物体测量服从协方差分布 \({{\texttt Q}}_{jk}\)，一个旨在找到最有可能的物体集合 \(\mathcal {Q}= \lbrace {\mathbf {Q}}_{1w}\ldots, {\mathbf {Q}}_{Kw} \rbrace\) 、路标点 \(\mathcal {X}\) 和关键帧 \(\mathcal {T}\) 的 BA 约束可以写成：
• 还可以对上式进行简化，首先假设特定目标类的范围（半径），并将类的实例的半径固定为此值，由于只有物体在三维世界中的位置（质心）需要进行细化，且点路标的半径为 0，所以可以将路标点的位置 \(\mathcal {X}\) 看做是 \(\mathcal {Q}\) 的子集。
• 世界系中半径分布： 点路标和物体路标的噪声协方差是相同的，实验表明可以将物体半径服从正态分布 \(\mathcal {N}(\epsilon,\sigma _\epsilon ^2)\);
• 图像平面高宽反投影分布： 根据公式 (7) ，物体半径的投影也大致服从正态分布，其中 \({\mathbf {f}} = 2 \langle Z^{-1}_{jk}\rangle {\left[\begin{array}{c}f_u\\ f_v \end{array}\right]} \quad(10)\) （世界系中的半径是一个参数，投影的宽高有两个参数） \[ {\color{Blue} {\left[\begin{array}{c}\hat{w} \\ \hat{h} \end{array}\right]} \sim \mathcal {N} \left(\epsilon {\mathbf {f}}, \sigma _\epsilon ^2 {\mathbf {f}} {\mathbf {f}}^\top \right) \quad(9)} \]
• 图像平面高宽测量分布： 测量过程中服从 \({\sim }\mathcal {N}({\mathbf {0}}, {{\Sigma }}_{\texttt{box}})\)，测得的宽度和高度分布为： \[ {\color{Blue} {\left[\begin{array}{c}w \\ {h} \end{array}\right]} \sim \mathcal {N} \left(\epsilon {\mathbf {f}}, [ \sigma _\epsilon ^2 {\mathbf {f}} {\mathbf {f}}^\top + {\Sigma}_{\texttt{box}} ] \right) \quad(11)} \]
• 物体测量协方差： 假设独立零均值，边界框中心的噪声方差为 \(\sigma _{x,y}^2\)，物体和点路标的测量协方差可以写成公式（12）
  • 其中，对于点路标，右下角的 2 * 2 矩阵（半径的分布）默认为 0，后续会考虑数值。 \[ {\color{Blue} {{\texttt Q}}_{jk} = {\left[\begin{array}{cc}{\left[\begin{array}{cc}\sigma _x^2 & 0 \\ 0 & \sigma _y^2 \end{array}\right]} & {{\texttt 0}}^{2 \times 2} \\ {{\texttt 0}}^{2 \times 2} & \sigma _\epsilon ^2{\mathbf {f}}{\mathbf {f}}^\top + {\Sigma}_{\texttt{box}} \end{array}\right]} \quad(12)} \]
• 从而公式（8）的 BA 约束可以简化成： \[ {\color{Blue}\lbrace \mathcal {Q}, \mathcal {T}\rbrace ^\ast = {\arg\,\min}_{\lbrace \mathcal {Q}, \mathcal {T}\rbrace } \sum _{j \in \mathcal {Q}} \sum _{k \in \mathcal {T}} \tilde{{\mathbf {q}}}_{jk}^\top {{\texttt Q}}_{jk}^{-1}\tilde{{\mathbf {q}}}_{jk} \quad(13)} \]
• 这种简化允许使用与一般 BA 相同的 Hessian 结构，虽然在测量残差中存在额外的参数处理操作，但主要的计算成本来自正规方程的求解，即仅使用路标进行 BA。

---

5. 实现

• 点和对象地标的组合表示可以应用于任何稀疏的基于关键帧的 SLAM 系统，本文讨论了与长距离长时间操作至关重要的实施细节。

5.1 局部 BA

• 将全局 BA 应用于视频序列中的每个帧，在计算上很快就会不可行，相反，如图 3 所示，本文优化当前估计的相机位置周围的局部区域，达到与视频速率操作一致的长时间实时稳定的计算负荷；
  • 当前中心相机帧 \({{\texttt T}}_{\text {cam}}\) 周围定义了 n（实验中取 n = 10） 个最近关键帧的局部集合 \(\mathcal {T}_{\text {local}}\) ；
  • 需要细化的结构包括在当前需要优化的相机和周围的 local 序列相机中可见的点路标和物体路标 \(\mathcal {Q}_{\text {local}}\) ；
  • 为约束地图其他部分的局部 BA ，来自其他关键帧的的所有观察结果 \(\mathcal {Q}_{\text {local}}\) 都包含在优化中；
  • 部分稳定的关键帧形成一组 \(\mathcal {T}_{\text {fixed}}\) 的位姿在优化期间保持固定；
  • 局部优化表示为：
• 由于局部 BA 仅在当前中心相机帧周围的局部区域中运行（仅优化 local 序列），因此无法将校正从 local 窗口传播到地图的其余部分；
  • 如果在一段时间内由于没有观察到物体而产生尺度漂移，只有当再次看到物体时，才校正包含 local 序列中 n 个关键帧的位姿，并更新这部分的地图；

5.2 降噪

• 误差源：实际物体测量受到许多误差源的影响：主要是物体检测中的误检测；图像中的大小和位置不准确；正确检测之间的不正确关联；最重要的是，在帧之间独立于传感器的运动。
• 处理方案一：首先，与点路标不同，点路标对于摄像机跟踪至关重要，并且必须尽快定位，因此不需要立即使用物体测量；相反，本文在 BA 中使用它们之前，要求物体需要有一定数量的观测次数，几个优势：
  • ① 增加的测量确保仅使用具有精确定位的物体；
  • ② 只要被测量的次数低于某个阈值，则任何来自物体检测算法的误检都会被忽略；
  • ③ 除非它们位于摄像机前方并以相同的速度移动，否则移动的物体由于少于所需的测量次数而被忽略。
    • 注意，物体以与摄像机相同的速度和方向移动相当于相机固定，然而，当有足够的路标测量不同意这种推断，并且相机确实在移动时，该物体将被稳健的估计器视为异常值。
• 处理方案二：其次，为了减少不良物体测量或不良数据关联，物体和点路标测量被包裹在 Tukey biweight 目标函数 ^[42] 中，将公式（13）改写成 \[ {\color{Blue} \lbrace \mathcal {Q}, \mathcal {T}\rbrace ^\ast = {\arg\,\min}_{\lbrace \mathcal {Q}, \mathcal {T}\rbrace } \sum _{j \in \mathcal {Q}} \sum _{k \in \mathcal {T}} \mathrm{Obj}\left({|\tilde{{\mathbf {q}}}_{jk}|},{{{\texttt Q}}_{jk}},\sigma _T\right) \quad (15)} \]
• 对上式考虑三种情况：
  • ① 针对点路标，\(\sigma _T\) 等于估计的点误差分布的标准差；
  • ② 针对 \(\tilde{{\mathbf {q}}}_{jk}\) 中的边界框位置(质心位置)\((x,y)_{jk}\) ，\(\sigma _T\) 被设置来适应物体检测固有的较低精度和尺度漂移还未被解决的不确定性；
    • 实验表明，在尺度稳定后，误差分布的标准差至少比标准误差高一个数量级（见图 12 a）；
  • ③ 针对边界框的大小 \((w,h)_{jk}\) ，实验发现最好关闭鲁棒的加权；
    • 如果最近有较好地观察到物体，则测量值大致表现良好（见图 12 b）；
    • 否则，尺度漂移可能会导致物体在周围地图中显得不成比例地大或小；
    • 对于漂移校正，调整不必剔除这些测量。

5.3 结合跟踪与建图

• 在系统框架上，修改了 PTAM 的基本版本来使用本方案，如图 4 所示；
• 跟踪线程：在初始化之后，相机位姿跟踪线程一直运行，使用简单的运动预测模型来辅助 FAST 角点与路标点投影的匹配，并迭代地使用重加权的最小二乘算法来优化位姿；
  • 与 PTAM 一样，该方案使用 8 * 8 像素模板匹配特征，这些模板首先与搜索半径中其他像素块进行粗略的匹配，然后针对亚像素精度进行迭代细化。
• 关键帧创建：除了测量与其他关键帧的距离之外，我们还监视当前相机估计估计值与最近关键帧之间的熵比 ^[2] ，与距离度量不同，熵度量具有对尺度漂移的不变性；如果这两个任一测量标准大于阈值，则将当前帧选择为关键帧。
• 添加路标到关键帧：由于只有一部分路标用于跟踪，所以建图器首先搜索其他路标（未被用于跟踪的路标）的测量结果并将其添加到关键帧；
  • 使用 PTAM 的图像块匹配（patch matching ）算法找到与最近的相邻关键帧的一组极线匹配，同时新的 3D 地图点被三角化；
  • 然后将来自目标检测的边界框形式的所有物体测量添加到关键帧；
  • 如果之前没有看到某个对象，则可以立即从关键帧中的单个测量中对其进行定位（因为其投影是可逆的），但是，在测量次数超过第 5.2 节中讨论的阈值之前，不允许进行 BA 优化。
• 关键帧进行 BA 优化并添加到建图线程：如果周围关键帧中有足够的物体测量值，则对 local 点路标和物体路标进行 BA 约束；如果没有找到物体，或者当前可见物体的测量值不足，则仅对局部点路标执行 BA。
  • 使用包含 10 帧可用于优化的关键帧的窗口，优化大约需要150毫秒才能收敛，不会导致跟踪中断；
  • 然后将新的关键帧添加到建图器的队列中，该队列将关键帧添加到独立的建图线程中；
  • 最后，使用摄像机的当前位姿更新运动模型。
• 总结流程：
  • 首先通过 FAST 角点提取、图像块匹配和简单的运动更新模型进行跟踪；
  • 然后根据与最近邻关键帧的距离和熵关系确定关键帧；
  • 再将关键帧观察到的路标（用于跟踪的除外）添加到关键帧中（观测次数少的就算添加了也不进行 BA 优化），同时完成数据关联（是更新测量还是实例化一个新的物体）；
  • 最后对关键帧周围 local 序列进行局部 BA 优化，并将路标和相机位姿添加到地图中。

6. 实验结果

• 该系统性能已经在 KITTI 街道场景数据集千米长室外序列，以及来自手持式手机摄像头的 100 米长室外序列上进行了评估。
  • 图 5 中示其中一个序列的整体表现，图 5（a）展示了目标检测，图 5（b）显示了具有路标点和物体检测的关键帧，图 5（c）显示重建的地图；
  • 从图 5（c）以及后面的图 14（c）中可以明显看出，物体标志的数量相较点路标的数量只是一小部分，典型比例了为0.1％；
  • 整体结构的改进源于物体稳定尺度，而不是他们增加了大量的测量。
• 本文方法能够处理多个任意对象类，或者说任何已知范围分布的对象类，但是在 KITTI 数据集上一般就使用汽车类；
  • 使用文献 [40,41] 的检测跟踪算法获得关键帧中的物体检测和相应的数据关联标签，提出适合在街道环境中检测车辆的方法；
  • 汽车的范围设定在 = 1.2 m，这是欧洲流行的平均水平。
• 本文开展以下实验：
  • ① 将补充物体 BA 的性能与没有物体测量且没有缩放信息的性能进行比较；
  • ② 当部分序列中的物体很少时，评估所提出方法的性能；
  • ③ 检查物体观测数量与相机速度误差之间的关系；
  • ④ 通过使用来自 KITTI 数据集的 ground truth 相机姿态数据来验证尺寸分布和噪声模型；
  • ⑤ 在线工具用于比较本文方法与其他在 KITTI 数据上表现最佳的单目里程计方法的性能；
  • ⑥ 展示了用手持摄像机以不受约束的方式移动拍摄的视频序列的结果。

6.1 有无物体约束性能对比

• 首先使用仅有点路标的 BA 优化，然后使用物体补充 BA。
• 轨迹对比：图 6 显示了使用（a）仅点路标的 BA 和（b）本文提出的点和物体 BA 优化获得的结果轨迹的比较，两者都应用于 KITTI ＃0 序列。
  • 虽然点路标的 BA 在轨迹的过程中累积了大量的尺度漂移，但是物体补充版本能够维持一个与地面实况基本相同的地图 - 并没有使用闭环来优化；
• 速度对比：图 6（c）中可以更清楚地看到误差的减少，图 6（c）显示了两种方法相机的速度与 ground truth 的比较，计算关键帧 k 处的相机速度为反向差分：
  • 其中 \({\mathbf {c}} = {{\texttt R}}^\top {\mathbf {t}}\) 是该关键帧的相机中心在世界坐标系中的位置；
  • 估计的与真实的相机速度之间的差异随着时间的推移而增长，而物体补充版本保持接近真实速度。 \[ {\color{Blue}s_k = | {\mathbf {c}}_k{-}{\mathbf {c}}_{k{-}1} | \quad(16)} \]
• 尽管在真实的相机轨迹中存在环路，但本文有意不闭合，以便产生更长的开环轨迹，图 6（d）展示了在进一步的 KITTI 序列上运行 物体 BA 的轨迹结果。
• 表 1 比较了 KITTI 数据集中前 11 个序列的有和没有物体 BA 的轨迹的均方根误差 ERMS，它带有ground truth，ERMS 是单目序列的平移误差的尺度不变量度：
• 由于ground truth 是可用的，表 1 显示了 s = 1 时的值，但是估计的轨迹已经标准化了，所以它的起始尺度等于 ground truth 的起始尺度；
  • 对于除两个序列外的所有序列，使用对象会显著降低平移错误，序列 1 和序列 4 仍然有大量的错误：如图 7 所示，这些序列中缺少静止的对象。

6.2 尺度漂移之后的恢复

• 第 3 节中方法的主要失败在于，在发生实质性的漂移之后引入尺度校正（例如，由于缺乏物体测量）是破坏性的，导致良好的测量结果被拒绝；
  • 图 8 表明现在情况并非如此，它显示了通过忽略前 2000 帧的所有物体测量而获得的估计轨迹和相机速度；
  • 在再次使用物体之后，几乎立即将比例修正为接近真实值的值。

6.3 物体观测对速度速度估计的影响

• 虽然第 6.1 节显示使用物体补充的 BA 优化明显优于仅使用路标点的 BA ，在这里我们提供了物体如何影响尺度估计的更局部化的视角。
• 对于轨迹中的每一个关键帧，速度上的 RMS 误差是与在 10 个关键帧的邻域内出现的目标观测的平均数一起计算的；
  • 平均值是在空间上计算的，因为观测通常还会影响周围的关键帧而不仅仅是当前的单个关键帧；
  • 虽然没有观察结果，但是没有对象观察但被具有物体观测的关键帧包围的单个关键帧仍然可能具有低速错误。
• 图 9 显示了速度的最大误差与 KITTI 0-10 序列的物体平均观察数量的关系图，但由于缺少静止物体而排除了序列＃1和＃4，增加观测数确实会降低误差的上限。

6.4 验证物体半径的分布

• 本文的目标不是在运行时了解检测到的物体的大小或结构，而是使用预先已知大小的数据来稳定尺度；
  • 在这项工作中，使用制造商的数据来找到流行的车辆的平均半径 \(\bar{\epsilon }\)，但其他物体的尺寸数据可以在建筑手册等来源中找到（例如，[45]），人类研究文献（如[46]）等；
  • 但是，由于 KITTI 数据集为每帧提供已知的摄像机姿势，可以以多种方式验证提议的汽车半径分布。
• 总结：
  • 实验一：将物体半径作为一个变量，验证在取不同值时对性能（估计的速度误差）的影响，最后得到半径取 1.2 m 时最优；
  • 实验二：利用 ground-truth 的关键帧真实位姿（上一步是估计值）利用公式（19）最小化得到所有帧中最优的物体尺寸，发现尺寸的分布均值依然是 1.2 m，方差 0.2 ㎡，符合正态分布；
  • 实验三：验证检测框的误差，发现检测框中心的误差符合正态分布，宽度和高不符合正态分布。

6.4.1 使用半径作为离散参数

• 首先，对半径 \(\bar{\epsilon }\) 进行经验搜索，将其作为要优化的参数，通过评估系统在包含汽车的 KITTI 数据集所有序列上的性能，得出性能最优时对应的半径参数；
  • 作为依据的性能指标是估计的每帧速度 RMS 的平均误差公式（18）
  • 其中 \(s_i\) 来自公式（16）的速度真值 ground-truth， \(\hat{s}_i(\epsilon)\) 是关于半径的函数估计的速度， \(N\) 是所有关键帧的数量。 \[ {\color{Blue} E_{\text {speed}}(\epsilon) = \left[ \frac{1}{N}\sum ^N_{i = 1} \left(s_i - \hat{s}_i(\epsilon) \right)^2\right]^{1/2} \quad(18)} \]
• 如图 10 显示了对象半径值的误差，具有最低误差的值出现在 \(\epsilon =\text{1.2}\) 米处，与制造商数据的平均值一致。

6.4.2 恢复半径分布

• 第二次验证直接估计 \(\epsilon\) 的分布，再次利用 KITTI 的 ground-truth，给定一组具有真值的包含物体观测的关键帧 \(\mathcal {T}_{\text{GT}}\) ，通过最小化来估计所讨论的特定类的一组对象 \(\mathcal {Q}^\ast\)： \[ \mathcal {Q}^\ast = {\arg\,\min}_\mathcal {Q}\sum _{j \in \mathcal {Q}} \sum _{k \in \mathcal {T}_{GT}} \tilde{{\mathbf {q}}}_{jk} ^\top \tilde{{\mathbf {q}}}_{jk} \quad(19) \]
• 这是公式（8）物体观测项的简化，没有协方差矩阵，每个物体的半径 \(\epsilon\) 不再是整个类的常量，而是每个物体实例位置旁的一个自由参数变量估计值；
  • 图 11 显示了在 KITTI ＃0 至 ＃10 序列中检测到的汽车的结果分布，分布平均值为 1.2 米，方差为 0.2 平方米，与早先的估算一致，而且，正态分布的假设似乎是合理的。

6.4.3 检测框尺寸的错误

• 物体相关误差的分布（公式 12）是通过累积在每个图像中检测到的边界框的位置、大小之间的差异的直方图以及在收敛之后通过 BA 给出的位置和大小来生成的，并且将后者视为正确的。
• 位置 \((x,y)\) 的误差直方图如图 12（a）所示，它们具有零均值，独立且接近正态分布，因此遵守（12）的假设；标准差 \(\sigma _x\) 和 \(\sigma _y\) 分别估计为 6.6 像素和 4.1 像素；
  • 边界框大小 \((w,h)\) 的误差直方图如图 12（b）所示，宽度和高度误差的平均值分别为 10.4 和 -11.6 ;
  • 误差协方差是： \[ \sigma _\epsilon ^2{\mathbf {f}}{\mathbf {f}}^\top + {\Sigma}_{\texttt{box}} = {\left[\begin{array}{cc}190.0& -123.4 \\ -123.4& 128.2 \end{array}\right]} \quad(20) \]
• 由于在 KITTI 数据集中，相机的 x 轴和汽车的主轴主要位于水平面上；
  • 对单一半径的物体模型进行拟合，往往会缩小宽度并推高高度，如图 13（a）所示；
  • 图 12（b）中的不对称分布在某种程度上打破了正态分布的假设，因为越靠近物体检测受到更大的透视畸变，同时检测到的数量更少；
  • 利用相机的光学、速度和目标检测等现实假设合成的分布如图 13（b）所示，再现了在图 12（b）中实际观测到的分布。

6.5 在线评估

• 本文方法使用 KITTI 的在线评估工具^[44]及其保留的测试序列上进行了评估；
  • 但是那里的所有参考方法都采用一种形式或其他形式的平面约束，使得本文的无约束方法的比较不公平；
  • 所有受约束的的单目 SLAM 系统的平均性能约 9％ 的平移误差和 0.020°/m 旋转误差，而性能最佳的约束方法实现 2.4％ 的平移误差和 0.006°/m 的旋转误差^[22,23]；
  • 本文无约束方法平均产生 20％ 的平移误差和 0.014°/m 的旋转误差，这些值至少接近并且旋转误差优于受约束方法的平均值。
• 尽管本文方法在绝对意义上难以与有约束方法竞争，但仍有进一步减少误差的余地；
  • 首先，KITTI 序列中的大型帧间运动过度扩展了 PTAM 的特征和相机跟踪线程，该系统是专为小型 AR 工作空间开发的；基于 KLT 的简单里程计系统 ^[47] 能够在 PTAM 丢失多个地图点的序列上的每对帧之间找到许多匹配，表 1 中的序列 ＃1 是一个这样的例子；
  • 其次，许多 KITTI 的测试序列不包含足够的静态汽车，因此难以进行漂移校正；一个极端的例子是测试序列 ＃14 ，它被捕获在公园里：有灌木，但没有汽车，后面在结论中讨论了多个类的使用。

6.6 手持相机（高度存在变化）序列测试

• 由于与 KITTI 在线评估工具提供的现有技术水平的比较不是对当前方法的公平测试，因为最佳表现方案都限制摄像机在道路上方具有固定高度。
• 为了证明我们的方法在没有这种高度约束的情况下运行的能力，使用手持式手机相机（具有固定焦距的三星 Galaxy S6 ，使用标准方法校准）在室外捕获序列；
  • 由于自制序列没有 ground truth ，所以包含一个小闭环，并且在闭环的开始和结束之间的定性距离上评估性能，汽车再次被用作物体级路标，并且将 \(\bar{\epsilon } = \text{1.2}\)；
• 从序列内的不同相机高度拍摄的两帧如图 14（a）所示,分别使用仅有点路标和物体补充的 BA ，估计的结果轨迹如图 14（b）中所示，其覆盖在拍摄序列的区域的卫星图像上；
  • 物体补充的 BA 方案尺度漂移显着减少，建图区域的详细点云如图 14（c）所示，再次发现，物体路标与背景特征点的比率非常小。

7. 总结与讨论

7.1 总结

• 本文提出了一种利用 BA 优化将物体的尺度信息整合到单目视觉 SLAM 中的新方法；
  • 然而其他方法依赖于进一步硬件传感器或关于地平面上方固定相机高度的假设，本方法仅假设相机能够观测到已知的物体，并且这些物体可以通过单一的半径进行合理的视觉描述；
  • 本方案通过避免物体方向的恢复，物体的 BA 结构与常规场景点的结构联合，导致计算复杂度没有增加并且计算成本仅适度增加。
• 结果已表明，只要可以获得常规的物体观测，该方法就能够在整个轨迹上保持一致的尺度估计；
  • 所需的物体路标观测数量是点路标数量的非常小的一部分，在我们的实验中，典型的比例是 0.1％；
  • 已经证明，在缺乏物体观察发生尺度漂移时，当重新引入物体时，漂移会迅速减少。

7.2 讨论

• ① 首先是使用多个物体类
  • 虽然在这项工作中使用了一个对象类，但使用多个对象类很简单：一个并行运行合适的物体检测器和跟踪器，有一点需要注意， BA 与物体类别无关；
  • 同时，知道各个类别相应的尺寸分布很重要，因此可以对来自几个类别的尺寸信息赋予适当差异的权重；
  • 尺寸信息来源：最有可能从独立数据中找到对象尺寸信息，但在本文中，也演示了当 ground truth 的相机位姿信息可用时，如何使用视觉信息来验证这些尺寸值。
• ② 其次是 SLAM 流程的细节
  • 目前，该方法不能在局部 BA 窗口之外的地图的部分中校正比例,例如，在第 6.2 节中，BA 无法校正没有物体观测可用部分的轨迹；
  • 文献 [48] 已经表明，全局 BA 是不必要的，因为仅在当前相机估计值附近需要精确的结构，相反，它们应用联合双窗口优化，在本地调整窗口中细化结构和关键帧，并对地图的其余部分进行姿势图优化；
  • 可以探究一下，使用相似度转换 [25] 来探索局部 BA 的缩放信息是否可以沿着位姿图正确地传播，此外，这将简单地使闭环闭合。
• ③ 第三个问题涉及物体建模
  • 结果表明，使用单一半径的模型不会妨碍整体尺度的恢复，这在一定程度上是基于物体的视图集成的良性结果（例如，细长对象的细的一段被相对较少地观测到；另外在一定程度上是视觉兴趣聚集的结果；
  • 最简单、也是最有效的建模增强是使用特定的部件检测器，并将不同的范围半径分配给不同的零件：汽车后部、汽车侧面等等；
  • 使用更复杂的姿态感知模型并非没有困难，例如可以使用非可视测量建立椭球体模型，也可以使用最近 [49] 和 [50] 中使用的带有边框的运动结构来构建椭球体模型；然后，需要估计物体的 6 自由度姿态，而不是仅仅估计它的 3 自由度位置，这一要求不会像点那样直接位于同一 BA 框架中；此外，如在 [50] 中看到的，在相机经过时，从几个少量视图恢复位姿并不总是可靠的。

【R】 参考文献

• [2] Kerl C, Sturm J, Cremers D. Dense visual SLAM for RGB-D cameras[C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2013: 2100-2106.
  本文关键帧选择策略之一：当前帧与关键帧之间的熵比作为依据的参考来源
• [23] Song S, Chandraker M. Robust scale estimation in real-time monocular SFM for autonomous driving[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. CVPR 2014: 1566-1573.
  自动驾驶中单目 SFM 尺度估计； 恒定高度假设，平面估计
• [24] Botterill T, Mills S, Green R. Correcting scale drift by object recognition in single-camera SLAM[J]. IEEE transactions on cybernetics, 2013, 43(6): 1767-1780.
  通过物体识别校正单目 SLAM 尺度漂移；物体之间的距离
• [25] Strasdat H, Montiel J, Davison A J. Scale drift-aware large scale monocular SLAM[J]. Robotics: Science and Systems VI, 2010, 2(3): 7.
  ERMS：单目序列的平移误差的尺度不变量度
• [35] Fioraio N, Di Stefano L. Joint detection, tracking and mapping by semantic bundle adjustment[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2013: 1538-1545.
  物体三维模型表达，联合检测、跟踪与建图的语义BA
• [36] Gálvez-López D, Salas M, Tardós J D, et al. Real-time monocular object slam[J]. Robotics and Autonomous Systems, 2016, 75: 435-449.
  需要有先验模型的单目目标级 SLAM； BA 约束；点云放置在地图中
• [37] Dame A, Prisacariu V A, Ren C Y, et al. Dense reconstruction using 3D object shape priors[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2013: 1288-1295.
  2013 利用 3D 物体先验的稠密建图
• [40] Zhang H, Geiger A, Urtasun R. Understanding high-level semantics by modeling traffic patterns[C]//Proceedings of the IEEE international conference on computer vision. 2013: 3056-3063.
  本文数据关联参考的方法
• [41] Geiger A, Lauer M, Wojek C, et al. 3d traffic scene understanding from movable platforms[J]. IEEE transactions on pattern analysis and machine intelligence, 2014, 36(5): 1012-1025.
  本文数据关联参考的方法；关键帧中物体检测和数据关联的方法
• [44] Geiger A, Lenz P, Stiller C, et al. Vision meets robotics: The KITTI dataset[J]. The International Journal of Robotics Research, 2013, 32(11): 1231-1237.
  KITTI 数据集；在线评估
• [48] Strasdat H, Davison A J, Montiel J M M, et al. Double window optimisation for constant time visual SLAM[C]//2011 International Conference on Computer Vision. IEEE, 2011: 2352-2359.
  双窗口优化；认为全局 BA 没有必要
• [49] Crocco M, Rubino C, Del Bue A. Structure from motion with objects[C]//Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. 2016: 4141-4149.
  椭球体物体建模
• [50] Rubino C, Crocco M, Del Bue A. 3d object localisation from multi-view image detections[J]. IEEE transactions on pattern analysis and machine intelligence, 2018, 40(6): 1281-1294.
  椭球体物体建模

【Q】 问题

• 公式（3）中的权重如何确定？公式（8）没有权重吗？
• 公式（15）的 Tukey biweight 目标函数不太理解，三种情况下对于 \(\sigma _T\) 的定义也不太理解；
• 5.3 节中将物体加入到关键帧部分不太理解；
• 5.3 节中第一个关键帧如何确定？
• 局部 BA 是针对关键帧还是普通帧：关键帧？
• 汽车范围设置为 1.2 m，但 cube slam 中使用的是 w = 3.9 m；l = 1.6 m；h = 1.5 m；
• 速度上的 RMS 误差是与在 10 个关键帧的邻域内出现的目标观测的平均数一起计算，这两个量怎么计算？平均值什么作用？
• 20％ 的平移误差大于 9% 的平均误差，表现并不好，最后的相机序列测试为什么没对比其他方案呢，可能效果也不错。

【T】 思考

• 1. 关于局部 BA：没有观察到物体的帧则无法优化，比如 local 序列中，第 3，4 帧没有观察到物体，其余帧观察到了，则其余帧都进行了优化，第 3,4 帧没有变化，但这两真肯定也有漂移，可以用他们周围的第 1,2 帧和 5,6 帧的误差均值作为他们的误差。
• 2. 本文的公式并不复杂，基本上就是建立在一个优化函数上，实验部分也不复杂相当于就是有无物体的对比，但却能分成 6 个实验来进行，而且还很有逻辑有说服力，写作方面可以学习。但是本文参考和对比（基本没有对比）的文献都比较老，基本都是 14 年之前的
• 3. 在效果上其实不是很好，包括在线对比也不占优势，作者解释是说没有基于相机高度的假设不公平（那你也可以加上高度假设啊），但是 Cube SLAM 也没有基于相机高度假设，误差只有 1.78% ，或许是 PTAM 本身的缺陷，在小场景比较实用， Cube SLAM 是基于 ORB SLAM，本身就有优势。
• 4. 使用 ground truth 视觉信息来验证这些尺寸信息，噪声分布等可以参考。

2019.02.27
wuyanminmax@gmail.com