0. 简介

我们常说的位姿图松弛指的就是基于闭环检测的边进行位姿图优化。而位姿图松弛已成为SLAM中不可或缺的补充，能够在满足逐对相对变换约束的目标下，实现传感器参考帧的高效全局配准。这些约束可以通过增量运动估计或全局地点识别来给出。尽管后一种情况可以实现闭环和漂移补偿，但在单目情况下需要注意，局部结构和位移的估计与实际情况可能不同，不仅在噪声方面，还可能存在尺度因子方面的差异。由于尺度传播误差的累积，这个尺度因子会随时间漂移，因此《Scale jump-aware pose graph relaxation for monocular SLAM with re-initializations》一文引入了尺度漂移感知的位姿图松弛。我们将这个想法扩展到后续传感器帧之间的相对尺度未知的情况，如果单目 SLAM 进入重新初始化并且无法识别连续局部地图之间的可靠重叠，则很容易发生这种情况。该方法是通过混合姿态图公式实现的，该公式将常规相似性一致性项与新的尺度盲约束相结合。我们将该技术应用于能够实现纯旋转位移的小型室内服务机器人的实际相关案例，这种情况很容易导致跟踪失败。我们证明，即使沿循环发生多次重新初始化，也可以恢复全局一致的轨迹，并对成功和失败案例进行了深入研究。

图1. 本文所讨论的场景示意图。一个配备有单目摄像头（例如机器人吸尘器）的智能体正在穿越一个房间。由于定期执行纯旋转位移，单目SLAM算法经常会失去跟踪并需要重新初始化。结果是一个分段尺度一致的轨迹。我们的贡献是一个混合位姿图优化框架，具有调和全局一致轨迹的能力

1. 主要贡献

本文的贡献如下：
• 我们证明了在重新初始化引起尺度跳变的情况下，简单地将相对尺度设置为1并应用尺度漂移感知松弛方法可能会导致严重的失真。
• 我们引入了一种新颖的混合位姿图优化框架，该框架允许在重新初始化边上使用无尺度因子，并展示了这个看似简单的改变如何在循环图中实现尺度协调和无失真估计，即使面临多次跟踪失败。
• 我们对可以容忍的失败边的数量和空间配置进行了全面讨论，并揭示了关键配置，其中无法获得全局一致的估计（仅限于单个全局尺度因子）。

2. 理论介绍

我们将从回顾传统的姿态图优化的表述开始，包括其解决方案以及使用相似变换进行尺度漂移适应。接下来，我们将介绍我们修改后的混合姿态图表述，明确考虑了未知尺度跳跃的存在。最后，我们将讨论退化情况，并提出一种方法来确定是否可以调和单一的全局一致尺度因子。

3. 尺度漂移感知姿态图优化的简要回顾

姿态图优化问题可以抽象如下。设\tilde{T}_{ij}为两个相邻传感器帧i和j之间的测量相对变换。它是一个欧几里得变换，可以用于线性映射从传感器帧j到i的齐次表示中的点，因此

在姿态图优化中，估计的变量由绝对传感器帧姿态T_i 给出，这些姿态被定义为线性映射传感器帧中的点到全局参考帧。

位姿图优化的目标是估计绝对传感器帧姿态T_i，使其与每个测量的相对姿态之间的总差异最小化。形式上，目标可以表示为：

最小化每个测量的相对姿态\tilde{T}_{i_kj_k}的连接的平方偏差之和，其中k = 1, . . . , M，并且两个对应的优化绝对姿态。后者由包含6个向量θ =[t^T ϕ^T]^T表示，其中包含了平移t和罗德里格斯向量ϕ。T(θ)使用Riemannian指数映射来获得相应的旋转矩阵，而t2v(T)则利用Riemannian对数映射再次回到最小表示。直观上可以清楚地看到，当绝对姿态与相对姿态测量一致时，它们将形成变换循环，相应的最小向量将为零。任何不一致都会导致非零向量，并因此对整体能量产生贡献。在实践中，我们通常会最小化鲁棒性、协方差加权的替代方案。

其中r_1(θ_{i_k}, θ_{j_k}) = t2v (T (θ_{i_k}))^{−1} T (θ_{j_k})− \tilde{θ}_{i_kj_k}是相对位姿一致性项，\tilde{θ}_{i_kj_k}是测量到的相对位姿的最小表示，\tilde{Ω}_{i_k,j_k}是相应的信息矩阵，ρ(·)是一个鲁棒的代价函数（例如Huber损失），它会减小异常值的影响。在全局估计的情况下，迭代更新最终使用稀疏Cholesky分解计算。

在单目情况下，初始相对位姿估计来自直接帧间相对位姿估计，它是尺度不变的。在随后的帧中，算法执行局部地图跟踪，从而实现尺度传播。在理想情况下，估计的相对位姿与实际值的差异在于：1）存在噪声，2）存在全局一致的尺度因子。我们可以简单地应用上述位姿图优化公式，找到受相同全局尺度参数影响的绝对传感器帧位姿。然而，就像算法的其他部分一样，尺度传播机制受到测量不确定性的影响，因此尺度会随时间漂移，并且变成一个局部变量。因此，对上述位姿图优化目标的简单应用将导致次优结果。为了考虑尺度因子的局部性质，Strasdat等人提出了基于相似变换的尺度感知位姿图优化方法[1]。绝对位姿被表示为

并且在位姿图优化器中使用的最小向量现在由7维向量θ = [t^T ϕ^T s]^T给出。估计的相对变换变为：

一些有趣的事实可以注意到：
• 假设的相对位姿是相似变换。
• 相邻传感器帧的尺度通常相似。
根据要求，在这种情况下，假设的相对位姿的尺度变为1（s_i ≈ s_j ⇒ e^{s_j−s_i} ≈ 1）。
• 在闭环情况下，尺度差异可能非常大。同样，这是正确反映的（si \neq s_j ⇒ e s_j−s_i \neq 1）。
• 优化的相对平移被适当地缩放，以便与可能的缩放相对平移测量进行比较。

4. 尺度跳变感知的混合位姿图优化

尽管将尺度作为优化变量有助于考虑尺度变化，但其潜力仍然受限于测量的相对尺度已知且受到噪声影响的情况。在常规跟踪过程中，相对尺度被简单地假设为1，因此该公式仅支持对随时间相对缓慢的尺度变化（即漂移）进行估计。在闭环情况下，相对尺度可以通过广义Procrustes对齐等方法计算得到。在这两种情况下，相对尺度被表示为高斯变量。

在实践中，我们可能会面临一些情况，其中相对尺度因子无法通过均值和标准差的高斯估计来反映。一个简单的例子是当算法丢失局部地图并需要重新初始化时。初始化后的尺度原则上是任意的，因此相对尺度变成了一个均匀分布的变量。为了考虑到可能存在未知相对姿态的边缘的情况，我们提出了一种替代的混合位姿图优化（HPGO）方法，该方法利用了修改后的残差项。

在相对尺度因子未知的边缘上。请注意，与引入替代噪声模型不同，该术语仅忽略相对尺度的误差，从而允许任意尺度变化发生。 HPGO的完整目标最终由以下给出：

在这里，
• 如果第k条边是沿着一个存在相对尺度的高斯估计的常规边，则dk = 1，
• 否则，d_k = 2。
请注意，可能存在相对尺度未知但其他参数仍然给定的情况。
让我们考虑一个能够执行纯旋转位移的平台的例子（例如机器人吸尘器）。很容易出现局部地图被推出当前视野的情况。虽然相对变换（即旋转）仍然可测量，但最终的重新初始化可能会导致局部尺度因子的跳变。有趣的是，如果发生闭环，仍然可以调和一个全局一致的图，使得每次算法重新初始化后的各个段保持一致的尺度。接下来，我们将分析可以恢复全局一致轨迹的情况。

5. 可行性研究

为了解释何时和为什么可以实现全局一致的尺度，让我们考虑一个几何图形的抽象。我们将位于未知相对尺度边的末端的节点标识为关键节点。我们在欧几里得空间中记录所有关键节点的真实位置，并在节点之间创建成对的连接，如果它们沿着实际轨迹直接连接（即不经过其他关键节点）。所得到的几何图形抽象是一组条形构造，每个条形代表整个轨迹的一部分，由于已知的相对尺度测量，在优化后保持一致的尺度。

现在可以如下回答上述问题：除了整个条形构造的平凡全局一致尺度之外，是否存在可以单独缩放的条形子集，而不需要改变条形之间的相对角度？条形之间的相对方向需要保持不变的要求源于事实，即轨迹段末端的平台方向必须与下一个段的开头方向保持一致。图2说明了一些基本情况，用于研究全局尺度调整的可行性。首先，直观上可以明确，只有当图中包含至少一个环时，才能获得全局一致的尺度。对于不属于环的关键节点，一些轨迹段将保持未确定的尺度。环形情况如下：

• 图2(a)：如果循环中只有一个关键节点，那么条形结构将会退化。沿着循环行进将始终返回到同一个关键节点，因此后者通过一条长度为零的条与自身相连。结果就是只有一个尺度因子，全局一致性总是可以实现的。

• 图2（b）：如果图形抽象中有两个节点，我们将有两个连接这两个节点的可能轨迹段，相应的条形图将是相同的。由于它们始终具有相等的比例尺，具有两个跟踪失败的循环将始终导致可调和的全局一致性。

• 图2（c）：对于三个关键节点，我们通常会得到一个三角形的条形构造。很明显，不变角度的要求意味着我们需要相似的三角形，并且所有的条形图再次必须具有各向同性的比例尺。如果三角形坍塌并且所有的条形图变得平行（参见图2（d）），则会出现退化情况。在后一种情况下，可以对三个段应用不同的比例尺而不影响角度一致性的要求。

• 图2（e）：对于具有四个关键节点的单个循环，在平面情况下无法再调和单一比例尺。一个简单的例子是条形图的矩形排列，它将使得平行条形图可以独立缩放。然而，对于任意四边形排列，情况也是类似的。

• 图2（f）：一旦图形添加了更多的连接，情况就会发生变化。与前一种情况相比，如果添加了一个直径连接，非各向同性的缩放将再次违反角度保持要求。可以提出一个一般规则如下：如果每个关键节点都是至少一个非退化三角形的一部分，则可以调和全局比例尺一致性。

• 图2（g）：需要注意的是，所有先前的情景只考虑了平面情况。在三维情况下，对于非平面排列的4个节点，总是可以实现全局尺度一致性。然而，如果至少有一个包含超过四个关键节点的回路，无论问题的维度如何，全局一致的尺度将无法调和。

图2. 可能出现追踪失败（即关键节点）的循环图的可视化。 (a)、(b) 和 © 分别显示了只有一个、两个或三个追踪失败发生的循环。 (d) 描述了三个关键节点的欧几里德位置对齐的特殊情况。 (e) 展示了一个具有4个关键节点的平面场景。 (f) 说明了如何通过添加额外的条来恢复全局尺度一致性。 (g) 表示了一个具有4个非平面关键节点的三维布局。除了 (d) 和 (e) 之外的所有配置都会导致全局尺度一致性。对于 (d) 和 (e)，可以进行非各向同性的缩放。详见正文。

从数学角度来看，全局尺度调和的可能性可以如下分析。假设图中有B条边，p_1，…，p_N是关键节点的绝对位置。设第k条边的起始节点和结束节点分别为{p_{s_k}，p_{e_k}}。如果不允许边之间的相对角度发生变化，这意味着每条边的绝对方向需要保持不变。设v_1，…，v_B是表示原始全局注册边的向量。为了满足角度约束，我们需要每条边的端点保持原始边向量的缩放版本，即

将所有B约束堆叠在一个大矩阵中，我们得到

其中I_{3×3N}（s_1，e_1）是N个矩阵的水平连接，这些矩阵都是3×3的零矩阵，除了第e_k个矩阵是I，第s_k个矩阵是−I。注意，最后一行是为了消除位置标定的自由度。现在可以通过检查A的秩来轻松分析至多一个全局尺度一致性的要求。如果A有一个零空间向量，可以实现全局一致的尺度。然而，如果A的秩缺陷大于1，则可能无法实现全局尺度一致性。注意，在实践中，旋转漂移将使得在实际位姿图优化完成之前无法确定全局定向的杆，因此整体图的一致性只能在事后进行分析。

参考链接

https://mp.weixin.qq.com/s/ZncGSlvbvlcBo_2I4T1skw