← 返回博客首页

SLAM 中的回环鲁棒性

回环检测为 SLAM 提供了校正长期漂移的机会，也会把错误数据关联带入后端。这篇文章从一个错误回环进入系统后的完整生命周期出发，梳理约束构造、集合筛选、权重更新、图结构调整和历史状态管理几条线索。

本文讨论的是 SLAM 系统中的回环鲁棒性。位姿图是描述后端相对位姿约束的一种常用形式，因子图、滑动窗口或其他平滑后端也会遇到同样的错误数据关联问题。为了把各类算法写在同一个数学框架中，下面使用位姿图记号说明残差和优化过程，但文章的对象仍然是 SLAM 的回环处理问题。

1. SLAM 中的回环鲁棒性

1.1 SLAM 的基本形式

SLAM 同时估计机器人轨迹和环境结构。回环检测提供的是跨越较长时间间隔的观测，它通常以关键帧之间的相对位姿因子进入后端。后端可以用Pose Graph 表示这部分状态：每个位姿对应一个节点，每条相对位姿测量对应一条边。

• 相邻帧之间的边通常来自轮速计、IMU、激光里程计或视觉里程计；
• 非相邻帧之间的边通常来自回环检测；
• 优化变量是所有节点的位姿；
• 优化目标是让所有测量尽可能得到满足。

位姿 $X_i$ 和 $X_j$ 之间的相对位姿测量记为 $Z_{ij}$，相应的误差可以写成

$$e_{ij}(X) = \operatorname{Log} \left( Z_{ij}^{-1}X_i^{-1}X_j \right).$$

其中 $\operatorname{Log}$ 将 $SE(2)$ 或 $SE(3)$ 上的位姿误差映射到李代数。给定信息矩阵 $\Omega_{ij}$，经典图优化目标为

$$\min_X \sum_{(i,j)\in\mathcal{E}} e_{ij}(X)^T \Omega_{ij} e_{ij}(X).$$

这个目标函数让所有边的加权误差平方和尽可能小。优化器每次线性化这些误差并更新位姿，更新后的位姿又会成为下一轮回环残差和权重计算的输入。

为了看到后端实际做了什么，可以把一次非线性迭代写成四步。先在当前估计 $X^{(t)}$ 处计算每条边的残差和雅可比

$$e_{ij}(X^{(t)}\boxplus\delta\xi) \approx e_{ij}(X^{(t)})+J_{ij}\delta\xi .$$

然后用平方根信息矩阵白化残差，累加正规方程

$$H=\sum_e J_e^T\Omega_eJ_e,\qquad b=\sum_e J_e^T\Omega_ee_e,$$

求解 $H\delta\xi=-b$，最后通过李群上的 $\boxplus$ 更新位姿。错误回环的问题正出现在这里：普通最小二乘会把每条边都放进同一个 $H$ 和 $b$，并不会区分“测量噪声较大”和“数据关联完全错误”。鲁棒算法要做的，就是在构造残差、信息矩阵或有效边集合时加入这种区分。

1.2 里程计边和回环边的作用不同

里程计边主要描述短时间运动。相邻帧之间的运动变化通常连续，错误关联的概率相对较低。回环边来自外观相似性、特征匹配或场景识别，容易受到感知混淆影响。

例如，机器人经过两条外观相似的走廊。视觉前端可能将当前位置误认为历史上的另一条走廊，并生成错误回环

$$X_j \approx X_i Z_{ij}.$$

如果这条约束被当成高置信度测量，后端会尝试同时满足它和大量里程计边，进而造成以下影响

1. 轨迹被错误拉向另一个位置；
2. 地图出现大范围形变；
3. 优化器进入错误的局部极小值；
4. 后续回环基于错误地图继续生成，错误影响逐步扩大。

问题的根源在于普通最小二乘目标默认每条边都属于同一个可信测量模型。它能够优化位姿，却缺少判断数据关联是否错误的机制。

1.3 鲁棒回环处理的大致任务

前端生成的回环候选并不都可靠。要判断候选是否错误，并在错误约束已经进入图之后限制它的影响，SLAM 需要一套鲁棒的回环处理算法。沿着一条回环从产生、进入后端到长期维护的生命周期，可以把主要任务归纳为五项

1. 在候选回环进入图之前，判断它与其他候选共同成立的条件；
2. 在回环进入图之后，让优化器降低异常约束的影响；
3. 用合适的误差模型表达正常测量和异常测量；
4. 根据后续证据重新审查已经接受的回环；
5. 为暂时拒绝的回环保留恢复机会。

这五项任务分别对应候选集合、单条约束、误差模型、历史决策和恢复机制。下面按这个顺序介绍代表性论文，并在每一节说明算法的输入、代价函数、迭代位置和输出状态。

2. 几篇代表性论文的思路

下面选取几篇有代表性的论文，按照它们主要关注的方向依次介绍。

1. Towards a Robust Back-End for Pose Graph SLAM 讨论后端参与回环有效性判断，以及图拓扑的调整；
2. Switchable Constraints for Robust Pose Graph SLAM 讨论用显式 switch variable 表示回环的软剔除；本篇文章简称SC（Switchable Constraints）。
3. Inference on Networks of Mixtures for Robust Robot Mapping 讨论用混合概率模型表达正常测量和异常测量；本篇文章简称Max-Mixture。
4. Robust Map Optimization using Dynamic Covariance Scaling 讨论根据残差计算约束权重；本篇文章简称DCS（Dynamic Covariance Scaling）
5. Robust Loop Closing Over Time for Pose Graph SLAM 讨论系统根据新证据重新审查过去接受的回环；本篇文章的算法简称RRR（ Realizing, Reversing, Recovering）。
6. Pairwise Consistent Measurement Set Maximization for Robust Multi-Robot Map Merging 讨论候选测量之间的一致性。本篇文章简称PCM（Pairwise Consistent Measurement）

把这几篇论文放在一起看，可以看到四个逐渐展开的方向

• 处理对象由位姿扩展到约束可信度；
• 处理范围由单条边扩展到候选集合；
• 处理时间由单次优化扩展到长期运行；
• 处理结果由连续降权扩展到接受、拒绝和恢复等状态管理。

3. 后端参与回环判断

论文 Towards a Robust Back-End for Pose Graph SLAM
作者 Niko Sünderhauf、Peter Protzel
会议 ICRA 2012
论文链接 PDF · DOI

这篇论文是理解后续 Switchable Constraints 的起点。它提出的核心变化是后端可以参与判断一条回环是否继续留在图中。

3.1 固定图拓扑的局限

传统 SLAM 后端通常把系统分成两个部分

• 前端负责传感器处理、数据关联和回环检测；
• 后端接收一张已经构建好的图，并在固定拓扑上优化位姿。

这种分工带来一个严重后果。前端一旦把错误回环放入图中，后端就会把它当成普通约束。标准优化器看到的只有残差和信息矩阵，它无法知道这条边来自错误的场景匹配。

错误回环进入图结构后，标准后端只能继续把它当成普通约束。鲁棒后端还需要判断哪些边应该继续存在。

3.2 后端参与约束选择

正确回环会帮助不同部分的轨迹对齐，错误回环会迫使地图同时满足相互冲突的几何关系。

假设图中有一条回环边 $e_k$。后端可以尝试两种处理方式

• 保留 $e_k$，让它参与位姿优化；
• 降低或移除 $e_k$，让其他约束主导当前估计。

如果保留这条边需要让大量可靠的里程计边产生很大误差，后端就应该倾向于降低这条回环的作用。

因此，论文把“图拓扑”也纳入了后端需要考虑的内容。图优化的任务从“在一张固定的图上求最优位姿”，扩展为“同时寻找合理位姿和合理约束集合”。

3.3 把约束选择写进目标函数

为了说明这个变化，可以给每条可能错误的回环增加一个离散变量 $a_e\in\{0,1\}$。$a_e=1$ 表示边参与当前图优化，$a_e=0$ 表示暂时关闭它。一个抽象的联合目标为

$$\begin{aligned} \min_{X,a}\quad &\sum_{e\in\mathcal E_{\mathrm{odom}}} \rho\left(e_e(X)^T\Omega_e e_e(X)\right)\\ &+ \sum_{e\in\mathcal E_{\mathrm{loop}}} a_e\,\rho\left(e_e(X)^T\Omega_e e_e(X)\right)\\ &+ \lambda\sum_{e\in\mathcal E_{\mathrm{loop}}}(1-a_e). \end{aligned}$$

前两项衡量保留约束后的几何误差，最后一项表示关闭一条回环需要付出的代价。$\lambda$ 越大，系统越倾向于保留回环；$\lambda$ 越小，系统越容易牺牲单条回环来保护里程计骨架。这个式子是对“后端同时选择位姿和约束集合”的抽象表达，后续的 SC 把离散选择放松成连续的 switch，DCS 则直接用残差计算边权。

如果直接枚举所有 $a_e$，组合数量会随回环数指数增长。因此工程实现通常采用交替更新或候选剔除：固定当前有效边优化 $X$，再根据残差和冲突关系提出少量剔除候选，重新优化后比较全局代价。

3.4 具体做法

论文提出了一种允许后端改变图拓扑的鲁棒建模方式。工程实现包含以下环节

1. 保留原始里程计边和回环边；
2. 为可能错误的回环增加表示其可信度的变量或机制；
3. 让位姿和回环状态共同进入优化；
4. 根据优化结果判断某些回环是否应该被关闭；
5. 必要时从有效图中移除这些边并重新优化。

下面的伪代码给出这种“优化—检查—重优化”骨架。

输入：里程计边 E_odom，回环候选 E_loop，初始位姿 X
active ← E_odom ∪ E_loop

重复直到收敛或达到最大轮数：
    X ← OptimizePoseGraph(active, X)
    对每条 e ∈ active ∩ E_loop：
        χ²[e] ← MahalanobisResidual(e, X)
        conflict[e] ← ConsistencyConflict(e, active, X)
    candidates ← 选择 χ² 或 conflict 最大的少量回环
    对每条 e ∈ candidates：
        trial ← active 删除 e
        X_trial ← OptimizePoseGraph(trial, X)
        如果 Objective(trial, X_trial) + removal_penalty
           < Objective(active, X)：
            active ← trial
            state[e] ← rejected
        否则：
            state[e] ← monitored

输出：位姿 X、有效边 active，以及所有回环的状态记录

后续 IROS 2012 的 Switchable Constraints 论文给出了更加清晰的显式变量形式。这篇 ICRA 论文的主要价值在于明确了问题定义和后端职责

• 后端开始参与数据关联纠错；
• 图拓扑可以由后端参与调整；
• 错误回环可以在优化过程中被发现；
• 约束选择和位姿估计开始形成联合问题。

3.5 错误回环的处理位置

错误回环在优化过程中被处理。

后端先把候选回环纳入一个可调整的图模型，再根据整体约束的一致程度改变图结构。它没有把全部责任推给前端，也没有等待地图完全失败后才处理。

3.6 小结

这篇论文带来的核心认识是，回环约束的有效性可以成为后端估计的一部分。回环状态需要成为可观测、可记录的对象，错误回环的关闭也应当保留在系统状态和历史信息中。

每条回环都可以设计成一个带状态的对象，状态信息应当比单个布尔变量更丰富

candidate -> accepted -> downweighted -> rejected

支持恢复时，还可以增加

rejected -> reactivated

4. 用 switch 变量软关闭回环

论文 Switchable Constraints for Robust Pose Graph SLAM
作者 Niko Sünderhauf、Peter Protzel
会议 IROS 2012
论文链接 PDF · DOI

这篇论文把上一节的后端拓扑思想具体化为一个可以直接实现的优化模型。

4.1 回环约束的软关闭

标准最小二乘对离群约束比较敏感。只要错误回环的权重较大，它就会参与拉动位姿估计。直接删除回环需要一个可靠的提前判断，固定阈值又很难覆盖不同数据集、不同初始化和不同噪声水平。

Switchable Constraints(SC) 让优化器在估计位姿的同时估计每条回环的可信程度。这个可信程度需要满足三个要求

1. 正确回环保持较大影响；
2. 错误回环可以逐渐失去影响；
3. 系统不能因为某次局部残差较大就立即删除所有回环。

4.2 核心思路和直觉

SC 为每条回环增加一个 switch variable $s_{ij}$，作为这条回环的“软开关”

• $s_{ij}$ 接近 1 时，回环处于打开状态；
• $s_{ij}$ 接近 0 时，回环处于关闭状态；
• 取中间值时，回环仍存在，但影响已经减弱。

优化器同时调整两类变量

• 位姿 $X$，用于调整轨迹；
• switch $s$，用于调整约束可信度。

如果一条回环和大量可靠的里程计边发生冲突，系统会在两种代价之间进行权衡

1. 大幅改变轨迹，让许多里程计边产生误差；
2. 降低这条回环的 switch，让它承担自己的矛盾。

当关闭回环的代价小于改变整条轨迹的代价时，优化器会让 $s$ 下降。

4.3 具体目标函数

将回环边的误差写成 $e_{ij}(X)$，SC 的目标函数可以写成

$$\begin{aligned} \min_{X,s}\quad &\sum_{e\in\mathcal{E}_{\text{odom}}} \|e_e(X)\|_{\Omega_e}^2 \\ &+ \sum_{e\in\mathcal{E}_{\text{loop}}} s_e^2\|e_e(X)\|_{\Omega_e}^2 \\ &+ \sum_{e\in\mathcal{E}_{\text{loop}}} \lambda(1-s_e)^2. \end{aligned}$$

三部分分别表示

1. 里程计约束；
2. 经过 switch 缩放的回环约束；
3. switch 的先验项。

先验项的作用是让系统默认回环有效。若没有这一项，优化器可能轻易把所有回环关闭，从而得到一个只依赖里程计的解。

参数 $\lambda$ 控制关闭回环的代价

• $\lambda$ 较大时，系统更愿意保留回环；
• $\lambda$ 较小时，系统更容易关闭回环。

这里的公式用于表达核心机制。具体实现中，需要按照优化库的变量定义、残差定义和信息矩阵约定决定 $s$ 的取值域以及缩放方式。

把两类残差合并后，一条回环对优化器提供的是

$$r_e(X,s_e)= \begin{bmatrix} s_e e_e(X)\\ \sqrt{\lambda}(1-s_e) \end{bmatrix}.$$

在当前线性化点，位姿增量和 switch 增量对应的雅可比分别为

$$\frac{\partial r_e}{\partial\delta\xi} = \begin{bmatrix}s_eJ_e\\0\end{bmatrix}, \qquad \frac{\partial r_e}{\partial s_e} = \begin{bmatrix}e_e(X)\\-\sqrt{\lambda}\end{bmatrix}.$$

因此，当回环残差很大时，switch 的梯度会推动 $s_e$ 下降；当残差较小时，先验项把 $s_e$ 拉回 1。原始建模通常把 switch 作为带先验的标量优化，工程实现如果需要严格限制 $0\leq s_e\leq1$，可以优化无界变量 $u_e$，并令 $s_e=\operatorname{sigmoid}(u_e)$。这会改变变量参数化，但不改变“残差项和先验项共同决定可信度”的机制。

4.4 优化流程

一条可实现的 SC 流程如下

第一步 构建普通位姿图

加入所有位姿节点、里程计边和经过前端验证的回环边。

第二步 为回环增加 switch 节点

每一条回环对应一个标量 switch。通常将初始值设为 1，表达“前端暂时认为这条回环可信”。

第三步 构建带 switch 的回环残差

回环边的残差由位姿误差和 switch 共同决定

$$r_{ij}(X,s_{ij})=s_{ij}e_{ij}(X).$$

同时加入 switch 先验残差

$$r_s(s_{ij})=\sqrt{\lambda}(1-s_{ij}).$$

第四步 使用 Gauss-Newton 或 Levenberg-Marquardt 优化

每一轮优化同时更新以下变量

$$\Delta= \begin{bmatrix} \Delta \xi_1 & \cdots & \Delta \xi_n & \Delta s_1 & \cdots & \Delta s_m \end{bmatrix}^{T}.$$

优化器会根据所有边的线性化结果决定修改位姿，还是修改 switch。

第五步 读取 switch 并进行后处理

优化完成后，需要读取每条回环的 switch 值，并根据应用需求设置状态

• $s_e > \tau_{\text{accept}}$ 时保留；
• $s_e < \tau_{\text{reject}}$ 时关闭或放入待剔除集合；
• 处于中间区域时保留并继续观察。

阈值应当结合数据集和优化行为进行实验确定。直接使用单个固定阈值，容易把短暂的大残差误判为永久错误。

把上述步骤写成伪代码，可以清楚看到 switch 是在每次线性化时和位姿一起更新的。下面的 J_pose 只对位姿变量求导，switch 列则由回环误差直接给出。

输入：位姿节点 X，里程计边 E_odom，回环边 E_loop
对每条 e ∈ E_loop：s[e] ← 1

重复直到增量足够小或达到最大迭代次数：
    H, b ← 0, 0
    对每条 e ∈ E_odom：
        r, J_pose ← LinearizeOdomResidual(e, X)
        Accumulate(H, b, r, J_pose)
    对每条 e ∈ E_loop：
        r_pose, J_pose ← LinearizeLoopResidual(e, X)
        r ← [s[e] · r_pose, sqrt(λ) · (1 - s[e])]
        J ← [s[e] · J_pose, r_pose; 0, -sqrt(λ)]
        Accumulate(H, b, r, J)
    Δ ← SolveDampedNormalEquation(H, b)
    X, s ← Retract(X, s, Δ)

输出：优化后的 X、每条回环的 s，以及由 s 产生的状态

实际代码需要注意变量排序、李群增量和 switch 的边界处理；如果使用 Ceres、g2o 或 GTSAM，核心工作是实现带 switch 的 residual block/factor，并确保先验残差没有被错误地乘上 $s_e$。

4.5 错误回环的处理位置

错误回环在优化内部处理。

它仍然存在于因子图中，但优化器可以通过降低 switch 让它逐渐失去影响。是否最终从图中删除，可以由后处理策略决定。

这里需要区分两种操作

• “降权”是连续优化过程；
• “删除”是离散图结构操作。

SC 主要负责前者，离散剔除策略负责后者。

4.6 小结

优点

1. 形式直观，容易观察每条回环的状态；
2. 错误约束可以软关闭；
3. 适合和现有图优化框架结合；
4. switch 值可以作为后端诊断信号。

限制

1. 每条回环增加一个优化变量；
2. $\lambda$ 需要调节；
3. 初始位姿很差时，正确回环也可能暂时得到较低 switch；
4. 多条错误回环互相支持时，局部优化可能难以分辨它们；
5. switch 的低值并不自动等价于“永久错误”。

方法认识

SC 将回环的可信度作为连续变量引入图优化。它能够表达软关闭过程，也能为每条回环提供直观的诊断量。低 switch 不自动等价于永久错误，初始位姿较差或多条错误回环互相支持时，switch 的解释仍然需要结合轨迹和其他约束。

5. Inference on Networks of Mixtures for Robust Robot Mapping

论文 Inference on Networks of Mixtures for Robust Robot Mapping
作者 Edwin Olson、Pratik Agarwal
会议版本 RSS 2012
期刊版本 IJRR 2013
论文链接 RSS 页面 · 期刊 DOI

这篇论文从概率模型角度研究错误回环。SC 为约束增加软开关，Max-Mixture（本篇文章） 则把测量属于多个误差模型的可能性直接写进概率分布。

5.1 单峰误差模型的局限

回环错误通常来自数据关联。传统方法希望通过更强的视觉匹配、几何验证或回环确认，把错误率压到很低。长期运行、多机器人和感知混淆场景中，完全消除错误回环非常困难。

普通高斯模型把测量误差看成单峰分布

$$p(z|X)=\mathcal{N}(e(X);0,\Sigma).$$

这个模型适合描述小范围、连续的测量误差，难以表达“测量可能完全正确，也可能来自一个很远的错误位置”这种多峰情况。

5.2 核心思路和直觉

Max-Mixture 使用多个高斯分量表达不同假设

$$p(z_{ij}|X) = \sum_{k=1}^{K} \pi_k \mathcal{N} \left( e_{ij}(X); \mu_k, \Sigma_k \right).$$

可以用两个分量理解

• 正常分量的均值接近 0，协方差较小；
• 异常分量的均值可以接近 0，协方差较大，允许测量拥有较大误差。

当一条回环与当前地图相符时，正常分量具有更高概率。当前回环残差很大时，宽协方差的异常分量可以承担这条测量，避免它强行拉动位姿。

这个思路保留了测量的不确定性。系统不需要在前端立即给出完全确定的正确/错误标签，后端可以在多个假设之间进行判断。

5.3 混合模型的优化过程

完整的混合模型通常会带来较复杂的概率推断。论文采用网络化混合模型，并使用 Max-Mixture 形式获得适合图优化的计算过程。

工程实现包含以下步骤

第一步 为每条测量定义多个成分

例如一条回环可以设置两个成分

component 0: 正常回环，协方差较小
component 1: 异常回环，协方差较大

每个成分包含以下信息

• 权重 $\pi_k$；
• 均值 $\mu_k$；
• 协方差 $\Sigma_k$；
• 对应的负对数似然。

第二步 根据当前位姿计算各成分的代价

在分量均值取零的常见设置下，对每个成分计算代价

$$C_k(X) = -\log(\pi_k) + \frac{1}{2} e(X)^T \Sigma_k^{-1} e(X) + \frac{1}{2}\log|\Sigma_k|.$$

第三步 选择当前代价较小的成分

Max-Mixture 用最大概率成分近似整个混合概率

$$\log\left( \sum_k \pi_k p_k(z|X) \right) \approx \max_k \log\left( \pi_k p_k(z|X) \right).$$

在优化过程中，当前最有解释力的成分参与残差计算。位姿改变后，最优成分也可能发生变化。

在代价域中，等价的实现是对每个分量计算

$$C_k(X) = \frac{1}{2} (e(X)-\mu_k)^T\Sigma_k^{-1}(e(X)-\mu_k) -\log\pi_k +\frac{1}{2}\log|\Sigma_k|,$$

并取

$$k^\star=\arg\min_k C_k(X).$$

选定 $k^\star$ 后，当前迭代使用的白化残差可以写成

$$r_{k^\star}(X) = \Sigma_{k^\star}^{-1/2} \bigl(e(X)-\mu_{k^\star}\bigr).$$

下一次迭代重新计算 $C_k$，所以分量选择会随着位姿更新而改变。这里的“选择”是对数和的最大项近似，不需要给每条边再增加一个连续 switch 变量；代价是分量切换会使目标函数分段光滑，对初值和阻尼策略比较敏感。

第四步 继续使用图优化更新位姿

每条边的残差形式取决于当前被选择的混合分量。这样可以在普通图优化框架中处理多假设误差模型。

整个 Max-Mixture 迭代可以写成下面的形式

输入：位姿 X，含 K 个高斯分量的回环边 E_loop
重复直到收敛：
    对每条 e ∈ E_loop：
        对 k = 1 ... K：
            C[k] ← ComponentCost(e, k, X)
        component[e] ← argmin_k C[k]
        r[e], J[e] ← ResidualAndJacobian(e, component[e], X)
    用所有里程计残差和 r[e] 组装正规方程
    Δ ← SolveDampedNormalEquation(H, b)
    X ← Retract(X, Δ)
输出：X，以及每条边最后选择的 component

5.4 错误回环的处理位置

错误回环在优化内部的测量模型选择过程中得到处理。

它的处理结果通常体现为以下几种形式

• 选择宽协方差的异常分量；
• 降低这条测量对位姿的约束强度；
• 让概率模型吸收大误差。

Max-Mixture 重点输出“当前更适合哪一个误差分量”，不会天然产生一个明确的回环删除状态。若项目需要永久剔除，还需要额外的边状态管理。

5.5 Max-Mixture 与 SC 的关系

SC 和 Max-Mixture 都让后端参与回环可信度判断，但内部变量的含义不同

• SC 的变量表示一条边的作用强度；
• Max-Mixture 的分量表示一条测量属于哪种误差模型。

SC 更接近“边是否打开”的状态建模，Max-Mixture 更接近“测量来自哪种概率来源”的模型建模。

5.6 小结

Max-Mixture 将回环的可信度表达为多个误差模型之间的选择，尤其适合描述多峰误差和正常测量、异常测量并存的情况

1. 回环误差明显具有多峰结构；
2. 需要表达正常测量和异常测量的概率差异；
3. 希望保留异常测量的软解释，暂缓将它标记为 rejected；
4. 需要研究鲁棒误差模型和 DCS、SC 之间的关系。

异常分量的协方差决定了模型吸收大误差的能力。协方差过小，异常分量难以吸收错误；协方差过大，异常分量又可能削弱正常测量对位姿的约束。

6. Robust Map Optimization using Dynamic Covariance Scaling

论文 Robust Map Optimization using Dynamic Covariance Scaling
作者 Pratik Agarwal、Gian Diego Tipaldi、Luciano Spinello、Cyrill Stachniss、Wolfram Burgard
会议 ICRA 2013
论文链接 PDF · DOI

本篇文章（以下简称DCS，Dynamic Covariance Scaling） 关注 SC 的工程代价。每条回环增加一个优化变量，图规模和计算量都会增加。DCS 直接根据当前残差计算约束权重。

6.1 显式可信度变量的计算代价

SC 的每条回环都增加一个 switch variable。对于少量回环，这个代价通常可以接受。大规模地图、多 session 或多机器人合并场景中，回环数可能非常多，额外变量会带来以下开销

• 状态维度增长；
• 线性化和求解成本增加；
• 变量初始化与边连接关系更加复杂；
• 后处理需要同时解释位姿和 switch。

论文给出了一种更轻量的鲁棒方法。它根据约束残差直接计算动态权重，使异常边逐步失去影响。

6.2 核心思路和直觉

对一条约束，先计算马氏距离

$$\chi_{ij}^2 = e_{ij}^{T} \Omega_{ij} e_{ij}.$$

如果 $\chi_{ij}^2$ 较小，说明当前位姿与测量相符，保留完整信息矩阵。如果 $\chi_{ij}^2$ 较大，说明这条边当前存在较强冲突，降低它的约束强度。

DCS 用一个依赖 $\chi^2$ 的缩放因子实现这个过程。下面采用“缩放白化残差”的常见实现约定

$$s_{ij} = \begin{cases} 1, & \chi_{ij}^2\leq\Phi,\\[4pt] \dfrac{2\Phi}{\Phi+\chi_{ij}^2}, & \chi_{ij}^2>\Phi. \end{cases}$$

其中 $\Phi$ 控制衰减范围。

当 $\chi_{ij}^2$ 很小时

$$s_{ij}\approx 1.$$

当 $\chi_{ij}^2$ 很大时

$$s_{ij}\rightarrow 0.$$

因此，DCS 实现了“残差越大，约束影响越小”的效果。这里的 $\Phi$ 是以当前信息矩阵为尺度的阈值，不能直接把不同噪声模型下的原始残差平方拿来比较。

6.3 动态权重的计算过程

DCS 的关键是把缩放因子应用到约束的代价或信息矩阵。工程中常见两种等价实现。

方式一 缩放信息矩阵

$$\Omega'_{ij} = s_{ij}^{2}\Omega_{ij}.$$

然后使用新的信息矩阵计算该边的代价和雅可比。

方式二 缩放白化残差

如果使用平方根信息矩阵 $\Lambda_{ij}$，可以写成

$$r'_{ij} = s_{ij}\Lambda_{ij}e_{ij}, \qquad \Lambda_{ij}^{T}\Lambda_{ij}=\Omega_{ij}.$$

两种写法在具体实现中需要保持一致：如果代码把 $w_{ij}=s_{ij}^{2}$ 称为 weight，那么信息矩阵写成 $\Omega'_{ij}=w_{ij}\Omega_{ij}$，白化残差写成 $r'_{ij}=\sqrt{w_{ij}}\Lambda_{ij}e_{ij}$。代码中应明确缩放对象

• 原始 residual；
• 白化 residual；
• 信息矩阵；
• 还是最终 cost。

一个简化的 DCS 迭代过程如下

1. 根据当前位姿计算回环误差 $e_{ij}$；
2. 计算 $\chi_{ij}^2$；
3. 根据 $\chi_{ij}^2$ 计算 $s_{ij}$；
4. 使用 $s_{ij}$ 更新该回环的有效信息；
5. 进行一次 Gauss-Newton 或 LM 更新；
6. 进入下一轮，重新计算所有回环权重。

DCS 的权重通常是迭代过程中动态变化的。它不需要把 $s_{ij}$ 作为额外顶点加入状态向量。

对应的 IRLS 式伪代码如下

输入：位姿 X，里程计边 E_odom，回环边 E_loop，阈值 Φ
重复直到位姿增量收敛：
    H, b ← 0, 0
    对每条 e ∈ E_odom：
        r, J ← Linearize(e, X)
        Accumulate(H, b, r, J)
    对每条 e ∈ E_loop：
        e_raw, J ← Linearize(e, X)
        χ² ← e_rawᵀ Ω e_raw
        如果 χ² ≤ Φ：s ← 1
        否则：s ← 2Φ / (Φ + χ²)
        r ← s · Λ · e_raw
        J ← s · Λ · J
        Accumulate(H, b, r, J)
    Δ ← SolveDampedNormalEquation(H, b)
    X ← Retract(X, Δ)
输出：X，以及最后一轮的 s[e]

6.4 错误回环的处理位置

错误回环在优化内部的权重计算过程中被处理。

DCS 可以让错误边逐渐失去影响，但它不会自动从候选集合中删除边，也不会天然记录“这条边曾经被拒绝”的历史状态。

因此，DCS 的输出更接近下面这种逐轮判断

当前这一轮，这条边应该有多大权重

连续权重还需要通过额外策略转换为离散状态

长期低权重 -> 待剔除
剔除后重优化 -> 验证地图是否恢复
新证据支持 -> 允许恢复

6.5 小结

优点

1. 不增加优化变量；
2. 容易嵌入现有图优化框架；
3. 计算过程简单；
4. 适合与 SC 做速度和状态维度对照；
5. 对异常边保留连续降权能力。

限制

1. 权重依赖当前位姿估计；
2. 初始估计很差时，正确约束也可能出现大残差；
3. 单条边残差无法完整表达候选集合的全局一致性；
4. 连续权重需要额外策略才能转换成明确的剔除决策；
5. 参数 $\Phi$ 需要结合噪声尺度和数据集进行调节。

方法认识

SC 与 DCS 形成了清晰的对照。SC 保留显式状态变量，诊断信息更丰富；DCS 直接从残差计算权重，状态维度和计算开销更小。两者都依赖当前位姿估计，也都需要结合集合一致性或历史状态才能形成完整的回环判断。

7. 长期运行中的回环重评估

论文 Robust Loop Closing Over Time for Pose Graph SLAM
作者 Yasir Latif、César Cadena、José Neira
会议版本 RSS 2012；增量版本 iRRR 发表在 IROS 2012
期刊版本 IJRR 2013
论文链接 RSS PDF · IJRR DOI

本算法简称RRR（ Realizing, Reversing, Recovering）。前面几种方法主要关注一次优化中的异常约束。RRR 进一步研究长期运行系统。系统今天接受的回环，未来可能被新证据证明错误。

7.1 增量运行中的历史错误

很多 SLAM 系统采用增量方式运行

1. 机器人移动；
2. 前端产生回环；
3. 后端加入回环并优化；
4. 地图继续扩展；
5. 新数据继续到来。

如果系统在第 3 步接受了一条错误回环，后续地图可能已经围绕这条错误边发生变化。仅仅在当前优化中降低一条边的权重，还需要处理历史决策和地图恢复。

• 过去接受的回环需要根据后续证据重新审查；
• 撤销错误回环后需要重新恢复合理的地图估计。

7.2 核心思路和直觉

RRR 建立在以下假设上

• 正确回环通常会和里程计、其他正确回环形成一致；
• 错误回环容易与里程计或正确回环产生冲突；
• 多条错误回环之间通常难以形成稳定共识。

因此，系统需要寻找一组能够共同解释轨迹的回环集合。随着新数据到来，这个集合也可以发生变化。

RRR 的名字代表三个阶段

1. Realizing 识别当前信息支持的回环共识；
2. Reversing 发现历史决策与新证据冲突后，撤销相关决策；
3. Recovering 移除错误约束，重新计算地图估计。

这个思路把回环从一个静态边扩展成一个具有生命周期的决策对象。

为了让“共识”可以计算，需要为候选集合定义支持度。一个便于实现的抽象写法是

$$\operatorname{Score}(\mathcal C) = \sum_{e\in\mathcal C}q_e -\alpha \sum_{\{e,f\}\subset\mathcal C} \operatorname{conflict}(e,f),$$

其中 $q_e$ 可以综合前端匹配置信度、几何验证结果和当前后端残差，$\operatorname{conflict}(e,f)$ 表示两条回环在里程计骨架或相对变换上的矛盾。Realizing 阶段选择得分较高且内部冲突较少的集合；Reversing 阶段比较新集合与历史 accepted 集合；Recovering 阶段通过移除、重优化和指标比较确认撤销是否成立。这个分数是工程实现的接口，具体论文实现可以使用不同的一致性检验和增量数据结构。

7.3 历史回环的重评估流程

RRR 的处理流程包含以下环节。

第一步 保存历史回环

系统不能只保存当前有效边。每条回环都需要记录以下信息

• 产生时间；
• 关联的关键帧；
• 测量值和协方差；
• 前端置信度；
• 曾经被接受、拒绝或恢复的记录；
• 与其他回环的冲突关系。

第二步 建立回环之间的关系

对于回环集合，检查每条回环与以下信息的一致性

• 里程计轨迹；
• 已接受的回环；
• 新到达的回环；
• 不同 session 之间的空间关系。

如果一组回环可以共同解释轨迹，它们就形成一个共识区域。

第三步 识别需要撤销的回环

当新证据到达后，重新计算一致性。如果过去接受的回环与新的共识发生冲突，就把它们放入撤销集合。

第四步 移除错误回环并恢复

从有效图中暂时移除被撤销的回环，然后重新进行图优化。恢复结果需要重新检查以下指标

• 里程计残差；
• 剩余回环残差；
• 轨迹是否出现跳变；
• 新的共识集合是否稳定。

第五步 支持增量运行

iRRR 版本关注每次新数据到来后的增量更新。系统不需要每次从零开始处理所有数据，但需要保留足够的历史状态，以便局部重评估。

把一次新回环到达后的处理过程写成伪代码如下。关键点是 history 不会因为某条边暂时被移除而丢弃它。

输入：历史回环 history，当前有效边 active，新候选 new_loops
history ← history ∪ new_loops
consensus ← RealizeConsensus(history, current_trajectory)
contradicted ← {e ∈ active ∩ history | e 不在 consensus}

如果 contradicted 为空：
    active ← active ∪ new_loops
    X ← IncrementalOptimize(active, X)
否则：
    trial ← active 删除 contradicted
    X_trial ← OptimizeFromCheckpoint(trial, X)
    如果 GlobalConsistency(trial, X_trial)
       > GlobalConsistency(active, X)：
        active ← trial
        对每条 e ∈ contradicted：state[e] ← removed
        X ← X_trial
    否则：
        对每条 e ∈ contradicted：state[e] ← monitored

对 history 中尚未激活的边：
    如果它重新进入 consensus 且通过局部验证：state[e] ← recovered
输出：X、active，以及完整 history

7.4 错误回环的处理位置

错误回环在图的生命周期和共识管理阶段被处理。

它可能经历以下状态变化

candidate
  -> accepted
  -> contradicted
  -> reversed
  -> removed
  -> recovered

这种处理方式覆盖了“错误回环已经影响地图之后”的阶段。SC 和 DCS 主要给出当前优化中的软处理能力，RRR 进一步提供历史决策撤销能力。

7.5 小结

优点

1. 支持长期运行；
2. 允许系统重新考虑历史判断；
3. 可以处理增量和多 session 场景；
4. 把回环从一次性输入变成可管理状态；
5. 适合记录“曾经接受的边后来被撤销”的原因。

限制

1. 需要保存较多历史信息；
2. 需要定义回环之间的共识和冲突关系；
3. 撤销后通常需要重新优化；
4. 正确共识不足时，系统可能难以区分多个冲突区域；
5. 长期运行系统需要控制历史候选数量。

方法认识

RRR 将回环判断放进图的生命周期。迭代剔除也可以被理解为一种历史状态管理过程

1. 每轮优化后记录所有回环的 residual、$\chi^2$、switch 或 weight；
2. 对低可信边增加“观察次数”；
3. 连续多轮低可信时进入待剔除集合；
4. 剔除后重新优化；
5. 根据全局指标变化确认剔除或恢复；
6. 新 session 到来后重新检查历史 rejected 边。

这里的关键点是使用多轮证据，减少单次优化波动造成的误判。

8. Pairwise Consistent Measurement Set Maximization for Robust Multi-Robot Map Merging

论文 Pairwise Consistent Measurement Set Maximization for Robust Multi-Robot Map Merging
作者 Joshua G. Mangelson、Derrick Dominic、Ryan M. Eustice、Ram Vasudevan
会议 ICRA 2018
论文链接 PDF · DOI

本篇文章简称PCM（Pairwise Consistent Measurement）， PCM 把错误回环处理进一步前移。它的重点是候选测量之间的相互一致性，尤其适合多机器人地图合并和多 session 对齐。

8.1 多地图合并中的集合一致性

多机器人或多地图合并有几个特殊困难

1. 每个机器人可能拥有自己的局部坐标系；
2. 机器人之间的相对位姿初值可能未知；
3. 错误跨地图匹配可能很多；
4. 系统可能缺少一条足够强的 odometry backbone；
5. 直接把所有候选回环放入非线性优化，容易让初始估计失效。

传统鲁棒优化方法通常使用当前轨迹和单条边残差判断异常。多机器人地图合并初期，系统缺少可靠的全局位姿，这个判断基础可能很弱。

PCM 在候选测量进入位姿图之前，根据测量之间的相互一致性选择较大的可信集合。

8.2 核心思路和直觉

如果两条回环都正确，它们应当能够共同解释两个局部地图之间的相对关系。即使系统还不知道全局相对位姿，这两条测量之间仍然可以进行一致性检查。

PCM 暂时不依赖全局轨迹，而是先判断两条回环共同成立的条件。

如果两个候选测量互相一致，就在它们之间连边。如果两个候选测量互相冲突，就不连边。

这样，候选回环可以转化为一个 consistency graph

• 顶点表示候选回环测量；
• 边表示两条测量满足 pairwise consistency；
• 团表示其中任意两个顶点都相互一致；
• 最大团表示最大的两两一致候选集合。

8.3 一致性图的构建过程

第一步 准备候选测量

设候选回环集合为

$$\mathcal{Z} = \{z_1,z_2,\ldots,z_m\}.$$

每条测量包含以下信息

• 关联的两端节点；
• 相对位姿；
• 协方差或噪声尺度；
• 前端匹配信息。

第二步 两两检查一致性

对任意两个候选 $z_i,z_j$，根据它们关联的节点和各自的相对位姿，组合出一个闭环关系。

如果两条测量在同一坐标关系下都成立，组合误差应当较小，可以写成抽象形式

$$d_{ij} = \left\| \operatorname{Log} \left( T_{ij}^{-1} \right) \right\|_{\Sigma_{ij}}^2.$$

其中 $T_{ij}$ 表示由两条测量和局部地图关系组合得到的相对变换。具体表达式依赖于 $SE(2)$ 或 $SE(3)$ 的节点关系和测量方向。

可以用一个具体的闭环组合来说明这个计算。设候选 $z_i$ 把局部帧 $A_i$ 变换到 $B_i$，记为 $Z_i={}^{B_i}T_{A_i}$；局部轨迹给出 $A_{ji}={}^{A_j}T_{A_i}$ 和 $B_{ji}={}^{B_j}T_{B_i}$。从 $A_i$ 到 $B_j$ 有两条路径

$$B_{ji}Z_i \quad\text{和}\quad Z_jA_{ji}.$$

因此两条候选的相容误差可以写成

$$r_{ij} = \operatorname{Log} \left( (B_{ji}Z_i)^{-1} Z_jA_{ji} \right), \qquad d_{ij}=r_{ij}^{T}\Sigma_{ij}^{-1}r_{ij}.$$

$\Sigma_{ij}$ 由两条回环测量和两段局部轨迹的协方差通过一阶误差传播得到。这个写法把“候选之间是否一致”变成一个可以计算的马氏距离，也明确了测量方向；实现时只需要根据自己的 $T_{ba}$ 约定调整乘法顺序。

当

$$d_{ij}<\tau_{\text{PCM}},$$

即可认为 $z_i$ 和 $z_j$ 两两一致。

第三步 构建 consistency graph

对每一对候选测量进行检查

• 一致时添加边；
• 冲突时不添加边。

最终得到一个无向图 $G_c$。

第四步 求最大一致集合

在 $G_c$ 中寻找最大 clique。最大 clique 中任意两个顶点都存在边，因此它对应一个两两一致的候选测量集合。

实际系统可以使用以下方法

• 分支定界；
• 最大团启发式；
• 近似算法；
• 根据前端分数或时间窗口先缩小候选集。

一个直接的实现骨架如下。最大团搜索本身可以替换成分支定界或近似算法，前面的两两一致性判定保持不变。

输入：候选 Z[1..m]，局部轨迹和协方差，阈值 τ_PCM
G ← 无向图，包含 m 个顶点
对 i = 1 ... m：
    对 j = i + 1 ... m：
        r_ij, Σ_ij ← ComposeCycle(Z[i], Z[j], local_trajectory)
        d_ij ← r_ijᵀ Σ_ij⁻¹ r_ij
        如果 d_ij < τ_PCM：
            在 G 中连接 i 和 j
C ← MaximumClique(G, vertex_score = frontend_confidence)
accepted ← {Z[i] | i ∈ C}
deferred ← {Z[i] | i 不在 C}
输出 accepted、deferred，以及 consistency graph G

第五步 将选出的候选送入后端

PCM 输出的测量集合可以作为后续图优化的输入。没有进入最大一致集的候选应当保留日志，并标记为当前阶段 rejected 或 deferred。

8.4 错误回环的处理位置

错误回环在非线性位姿优化之前被处理。

PCM 通过候选之间的相互冲突关系，减少错误测量进入后端的机会。它的输出是一个候选集合，结果形式和 SC 的连续 switch、DCS 的动态权重不同。

8.5 小结

优点

1. 处理位置早，可以减少错误边污染初始优化；
2. 关注候选集合的全局结构；
3. 对初始相对位姿要求较低；
4. 适合多机器人、多 session 地图合并；
5. 输出集合容易进入后续 SC 或 DCS 后端。

限制

1. 两两一致性检查需要 $O(m^2)$ 次候选比较；
2. 最大团问题的组合搜索成本可能较高；
3. 阈值和噪声模型会影响 consistency graph；
4. 两两一致性集合仍然需要后续位姿优化验证；
5. 候选数量过大时需要先进行空间、时间或置信度预筛选。

方法认识

PCM 位于非线性位姿优化之前，负责从候选集合中提取相互一致的测量。SC 和 DCS 接收筛选后的候选，分别处理连续可信度和动态权重。三者形成了从集合判断到单边优化的处理链

1. 前端生成候选回环；
2. 完成局部几何验证；
3. 计算候选之间的 pairwise consistency；
4. 构建 consistency graph；
5. 选择最大一致集合；
6. 将保留候选送入 SC 或 DCS；
7. 对被排除候选保留状态和证据。

PCM 处理候选之间的共同成立关系，SC 和 DCS 处理保留候选在当前优化中的影响程度。

9. 几类方法之间的关系

9.1 后端开始管理约束拓扑

后端问题的边界由固定图优化扩展到约束选择。回环边是否继续存在，也可以由后端参与判断。

SC 把这个思想变成了显式 switch。后端可以通过连续变量表达一条回环的有效程度。

9.2 约束可信度扩展为概率模型

Max-Mixture 进一步说明，回环测量可以拥有多个概率解释。正常测量和异常测量可以由不同分量承担。

它有助于理解鲁棒优化中的一个重要事实。异常并不总是简单的“开”或“关”，测量模型本身也需要表达不确定性。

9.3 显式状态与动态权重的取舍

DCS 关注计算效率。它根据当前 residual 直接计算权重，减少 SC 的额外状态变量。

这形成了清晰的工程取舍

• SC 的状态更明确，诊断信息更丰富；
• DCS 的实现更轻量，变量数量更少；
• 二者都依赖当前优化状态；
• 二者都需要配合集合级别的筛选或历史管理。

9.4 回环判断进入图生命周期

RRR 解决了时间维度的问题。系统过去的回环决策可以在新证据到来后被重新检查。

一次优化得到的低权重应当先进入观察状态，多轮证据一致后再做离散剔除。

9.5 候选集合需要独立的一致性判断

PCM 解决了优化之前的候选集合选择问题。它不依赖一次完整的全局位姿估计，先从候选之间的相互关系中提取一致集合。

这使它尤其适合以下场景

• 多机器人地图合并；
• 多 session 地图对齐；
• 初始相对位姿不可靠；
• 错误回环数量较多的场景。

10. 回环鲁棒后端的组合方式

根据这些方法各自处理的对象，可以将后端组织成五层流程。

10.1 候选生成

前端负责提供候选回环

• 关键帧对；
• 相对位姿；
• 协方差；
• 匹配数量；
• 几何内点率；
• 视觉或激光前端置信度；
• 候选产生的时间和 session。

前端输出应当保留候选，即使它后续被后端拒绝。这样才能分析前端候选质量和后端纠错效果。

10.2 局部几何验证

对候选回环进行必要的局部验证

• RANSAC；
• PnP 或相对位姿估计；
• 2D/3D 几何一致性；
• 深度或激光重叠检查；
• 协方差估计。

这一层提供局部证据，负责尽早过滤明显错误。它无法替代后续的全局一致性判断。

10.3 PCM 前置筛选

候选数量较多，或者系统处于多 session、多机器人合并场景时，可以执行以下步骤

1. 对候选回环进行两两一致性检查；
2. 构建 consistency graph；
3. 求最大一致集合或可控近似；
4. 将保留候选送入后端；
5. 将排除候选标记为 deferred 或 rejected；
6. 保留被排除的原因。

候选较少时，可以降低 PCM 的计算频率，或者仅在候选冲突明显时触发。

10.4 SC 或 DCS 鲁棒优化

PCM 保留下来的候选仍然可能包含异常边。后端可以采用以下方法

• SC 用于获取明确的 switch 诊断；
• DCS 用于降低状态维度和计算开销；
• Max-Mixture 用于处理多峰测量模型。

从机制的可解释性看，SC 提供了最直观的观察入口

1. 便于直接观察每条回环的有效程度；
2. 便于设计迭代剔除规则；
3. 便于和论文公式对应；
4. 便于构建错误回环识别实验。

在此基础上，DCS 可以作为轻量化对照，比较以下指标

• 运行时间；
• 优化迭代次数；
• 轨迹误差；
• 错误回环识别率；
• 对初始估计的敏感性；
• 诊断信息的完整程度。

10.5 迭代重评估和历史管理

每次优化后保存以下信息

• 原始 residual；
• 白化 residual；
• $\chi^2$；
• SC switch 或 DCS weight；
• 与其他回环的冲突数量；
• 优化前后的全局 cost；
• 轨迹误差或地图一致性指标；
• 当前边状态；
• 状态变化的原因。

回环状态可以采用以下状态机

candidate
  -> verified
  -> accepted
  -> monitored
  -> downweighted
  -> rejected
  -> reactivated

状态转移需要设置证据条件

• candidate -> verified 表示局部几何验证通过；
• verified -> accepted 表示 PCM 保留且后端优化稳定；
• accepted -> monitored 表示 switch 或 weight 开始异常；
• monitored -> downweighted 表示连续多轮异常；
• downweighted -> rejected 表示剔除后全局指标改善；
• rejected -> reactivated 表示新证据使其重新进入一致集合。

这个状态机可以把论文中的连续权重、离散剔除和历史恢复放到同一个工程框架中。

11. 迭代剔除的实现流程

下面的流程适用于迭代剔除

第一步 初始化

使用里程计边和经过局部验证的回环建立初始图。回环 switch 初始为 1，或者 DCS 权重初始为 1。

第二步 执行鲁棒优化

使用 SC 或 DCS 进行若干轮优化，获得当前位姿和回环权重。

第三步 收集诊断量

对每条回环记录以下诊断量

$$\left( \chi^2_e, s_e, n_{\text{conflict}}, \Delta \text{cost}, \text{state} \right).$$

其中

• $\chi^2_e$ 表示回环的马氏距离；
• $s_e$ 表示 SC switch；DCS 则同时记录残差缩放 $s_e$ 或信息权重 $w_e=s_e^2$；
• $n_{\text{conflict}}$ 表示与其他候选的冲突数量；
• $\Delta \text{cost}$ 表示处理该边前后的全局代价变化。

第四步 进入观察状态

单轮异常边先进入 monitored，保留在图中或使用较小权重。这样可以降低瞬时线性化误差造成的误判。

第五步 生成剔除候选

满足以下多个条件时，将边放入剔除候选

• 连续多轮低 switch；
• 连续多轮高 $\chi^2$；
• 与一致集合存在冲突；
• 删除后全局 cost 改善；
• 删除后里程计残差没有明显恶化。

第六步 剔除并重新优化

暂时移除候选边，重新执行图优化，并比较移除前后的指标

• 轨迹误差；
• 里程计残差；
• 剩余回环残差；
• 地图闭环误差；
• 优化收敛情况。

第七步 确认或恢复

如果地图质量改善，确认边为 rejected。如果地图质量下降，恢复该边，记录本轮判断失败，并调整后续观察策略。

把这套策略合并成一个可运行的后端循环，可以写成

输入：候选回环 loops，里程计边 E_odom，初始状态 X
history ← 保存 loops 的测量、协方差、来源和状态
active ← E_odom

对每个新到达的候选 batch：
    verified ← LocalGeometricVerification(batch)
    history ← history ∪ verified
    accepted_set ← PCM(history 中尚未永久拒绝的候选)
    active ← E_odom ∪ accepted_set

    如果使用 SC：
        X, switch ← OptimizeWithSwitches(active, X)
    否则如果使用 DCS：
        X, scale ← OptimizeWithDCS(active, X)
    否则如果使用 Max-Mixture：
        X, component ← OptimizeWithMixtures(active, X)

    对每条回环 e：
        记录 χ²[e]、可信度、冲突数和全局 cost
        更新 e 的 monitored / downweighted 计数
    remove_set ← 满足连续异常条件的候选
    trial ← active 删除 remove_set
    X_trial ← Reoptimize(trial, X)
    如果 GlobalMetrics(trial, X_trial) 改善：
        active ← trial
        将 remove_set 标为 rejected
        X ← X_trial
    否则：
        保留 active，并把 remove_set 留在 monitored
    对 history 中被新证据重新支持的边：
        重新局部验证后允许 reactivated

输出：轨迹 X、当前 active 图、完整回环 history 和诊断日志

12. 回环鲁棒性的实验设计

只在干净数据上运行一次优化，无法验证鲁棒回环方案。分层实验更能呈现不同方法的作用边界。

12.1 基础对照

至少比较以下方法

1. 普通最小二乘；
2. 仅 SC；
3. 仅 DCS；
4. PCM + SC；
5. PCM + DCS；
6. PCM + SC + 迭代剔除。

研究历史回环恢复时，再增加以下实验

7. 增量回环加入；
8. 历史回环重评估；
9. rejected 回环恢复。

12.2 控制错误回环比例

在真实或合成数据中逐步增加错误回环比例，例如

0%
5%
10%
20%
30%

每个比例下重复多次实验，记录均值和方差。

12.3 实验指标

位姿和地图指标

• ATE；
• RPE；
• 关键帧相对位姿误差；
• 地图闭环误差；
• 最终全局 cost。

回环决策指标

• 错误回环识别 precision；
• 错误回环识别 recall；
• 正确回环保留率；
• 误剔除率；
• 从接受到拒绝的平均延迟；
• 被拒绝回环恢复成功率。

工程指标

• 优化时间；
• 迭代次数；
• 变量数量；
• 内存占用；
• PCM 两两检查耗时；
• 最大团搜索耗时；
• 重新优化次数。

12.4 重点测试场景

1. 初始位姿估计较差；
2. 错误回环数量较多；
3. 多条错误回环互相支持；
4. 正确回环数量较少；
5. 多 session 逐步加入；
6. 错误回环先加入，正确回环后到达；
7. 地图中存在重复纹理或感知混淆区域。

这些实验可以分别检验 PCM、SC、DCS 和 RRR 风格历史管理的作用边界。

13. 回环鲁棒性的核心认识

13.1 后端约束选择

后端可以参与回环有效性的判断，图结构可以成为优化对象的一部分。

13.2 显式可信度

可以为每条回环增加一个可优化的可信度变量，让错误回环通过降低 switch 逐渐失去影响。

13.3 多峰误差模型

测量误差可以拥有多个概率解释。正常测量和异常测量可以由不同分量共同表达。

13.4 动态权重

通过残差动态计算权重，可以获得轻量级的优化内鲁棒处理，减少额外状态变量。

13.5 历史状态

回环判断具有生命周期。系统需要能够重新审查、撤销和恢复历史决策。

13.6 集合一致性

在进入非线性优化之前，可以先检查候选回环之间的集合一致性，选择一个相互支持的测量子集。

14. 回环鲁棒后端的组合路线

这些方法在系统中的组合顺序可以写成

1. 普通位姿图后端提供干净数据上的基线；
2. SC 通过 switch 表达回环可信度；
3. 观察、剔除和恢复机制将连续权重连接到离散状态；
4. DCS 作为减少状态变量的轻量化方案；
5. PCM 位于多回环和多 session 场景的前置筛选位置；
6. 历史状态支持增量重评估；
7. 控制错误比例的实验用于评价整体方案。

最终得到的后端可以表示为

$$\text{候选生成} \rightarrow \text{局部几何验证} \rightarrow \text{PCM 集合筛选} \rightarrow \text{SC/DCS 鲁棒优化} \rightarrow \text{诊断与迭代剔除} \rightarrow \text{历史回环重评估}.$$

这条路线将几类方法放在各自适合的位置

• PCM 处理候选集合；
• SC 处理显式可信度；
• DCS 处理轻量动态权重；
• Max-Mixture 提供多假设误差模型；
• RRR 提供长期历史管理；
• 可调整图结构的方法提供整体问题定义。

参考文献

1. N. Sünderhauf and P. Protzel, “Towards a Robust Back-End for Pose Graph SLAM,” ICRA, 2012.
2. N. Sünderhauf and P. Protzel, “Switchable Constraints for Robust Pose Graph SLAM,” IROS, 2012.
3. E. Olson and P. Agarwal, “Inference on Networks of Mixtures for Robust Robot Mapping,” RSS, 2012; IJRR, 2013.
4. P. Agarwal, G. D. Tipaldi, L. Spinello, C. Stachniss, and W. Burgard, “Robust Map Optimization using Dynamic Covariance Scaling,” ICRA, 2013.
5. Y. Latif, C. Cadena, and J. Neira, “Robust Loop Closing Over Time for Pose Graph SLAM,” IJRR, 2013.
6. J. G. Mangelson, D. Dominic, R. M. Eustice, and R. Vasudevan, “Pairwise Consistent Measurement Set Maximization for Robust Multi-Robot Map Merging,” ICRA, 2018.