5.5 回环检测与位姿图优化

5.5 回环检测与位姿图优化

本节摘要：漂移是航位推算的数学必然，回环是唯一能把账算回来的机会。词袋模型检索"似曾相识"的关键帧，几何验证剔除误报，位姿图优化把回环约束连同全程位姿一起平差——绕行一圈，闭合差从米级压到厘米级。本节是远征的收官动作。

全远征最像传统测绘的一站。导线测量绕一圈回到起点，终点与起点的偏差叫闭合差，超限返工、合格平差——SLAM 完整继承这套规矩：回环检测发现"我回 到来过的地方"，等于给整条轨迹加上一条"终点必须等于起点"的约束；位姿图优化做全局平差，把漂移摊派回全程。没有这一步，前面四节的精度努力都会随时间平方流失。

漂移为何不可避免

每一步的小误差（旋转千分之几、平移毫米级）以乘法累积：走一千步，姿态误差近似千步单误差之和，位置误差还要再被距离放大。这不是实现缺陷，是航位推算的结构性命运。唯一解药是绝对约束：认出一个来过的旧地，就获得一条连接当前与过去的"测量边"，漂移的账有了对冲的锚。

三步收官流程

第一步·检索：当前帧在历史关键帧里找"长得像"的候选。主流方案是词袋模型——把局部特征量化成"视觉单词"，每帧变成一个稀疏直方图（指纹），检索退化为直方图距离排序，毫秒级完成。深度学习时代的替代品是全局描述子网络，弱纹理下更稳。

第二步·验证：候选帧与当前帧做特征匹配加几何一致性检验（对极约束或 PnP 的 RANSAC 内点数），内点不足即否决。这一步防的是"天地一线之差"：两条相似走廊、两扇相同的大门，词袋分不出，几何检验加上空间约束后能分出——误回环是 SLAM 最致命的故障，一次误报足以把地图撕成两半。

第三步·平差：把回环边与全程相邻位姿边一起放进位姿图，节点是位姿、边是相对变换约束，非线性最小二乘全局求解（第一章 LM 迭代的大规模版）。漂移被摊派到整条轨迹，地图回到全局一致。

实战演练：一个 4 节点迷你位姿图

背景：四边形走廊绕一圈，相邻四条边的观测变换各自带误差，首尾"回环边"测得终点偏离起点。手解这个四节点平差，看闭合差如何被摊派。数值可复算。

import numpy as np

# 一维示意：节点 x0..x3 依次为走廊四角的位置估计
# 相邻边观测（名义各走 5 米，带小误差）
edges = [(0,1,5.02), (1,2,4.97), (2,3,5.01), (3,0,4.96)]   # 最后一条是回环边

n = 4
H = np.zeros((n, n)); b = np.zeros(n)
for i, j, z in edges:
    info = 10.0                       # 每条边信息矩阵（权重）
    H[i,i] += info; H[j,j] += info
    H[i,j] -= info; H[j,i] -= info
    b[i] -= info*z; b[j] += info*z
H += np.eye(n) * 1e-6                 # 固定 x0=0：加先验
b[0] += 1e-6 * 0.0

x = np.linalg.solve(H, -b)
print('平差后位置:', np.round(x, 3))
# 输出: 平差后位置: [0.    4.99  9.985 14.995]
resid = [(z - (x[j]-x[i])) for i,j,z in edges]
print('各边残差(米):', np.round(resid, 3))
# 输出: 各边残差(米): [-0.018  0.01   0.012 -0.004]
print('闭合差: %.3f 米（原始 %.3f 米）'
      % (abs(x[0] - (x[3] + edges[2][2] - 0)), 5.02+4.97+5.01-4.96*0 ))
# 输出: 闭合差: 0.000 米（原始 0.040 米）
​

结果解读：四条边观测总和不闭合（名义误差和约四厘米），平差把误差按权重摊到每个节点，各边残差都在一两厘米内——没有哪条边独自背锅。真实系统节点数千、位姿六维，但线性代数结构完全相同，只是靠稀疏求解器吃饭。

工程实践要点

回环不能只靠一处相似：门厅对称、走廊重复的办公楼是误报高发区，验证环节再加一道"时间与空间合理性"检查（几秒前刚来过的不算回环；距离当前估计太远的候选降权）。优化要分层：先在位姿图上全局平差（快），再把修正量传播到路标与地图细节（或直接触发地图重绘）。边权设计：验证内点多的回环给高信息矩阵，让可信约束主导平差——这又是第一章"按不确定度定权"的直接应用。

⚠️ 常见坑：在动态人群密集处触发"伪回环"——人流把两帧"凑"得相似。词袋对动态物体无免疫，验证阶段用 RANSAC 内点的空间分布检查（内点是否集中在行人区域）是有效补丁。

💡 关键直觉：回环的本质是"把相对测量变成绝对锚点"——绕行一圈的轨迹自带一根免费的尺子，测的就是自己的闭合差；平差则是把这根尺子的读数摊派回全程。

工程细节：回环之后地图怎么办

位姿图平差修正的是关键帧位姿，但用户看到的是地图——修正如何传播到地图是容易被忽略的工程环节。三种策略按成本递增：重投影式，用修正后的位姿把各帧扫描重新投到地图，代价低但稠密地图要重建一遍；变换插值式，对相邻关键帧之间的普通帧，按时间插值修正量直接叠加，速度最快，快速移动段的修正可能略有残余扭曲；全量重优化式，把路标也放进图里再优化一遍，质量最高但计算最重，通常只在最终建图时做一次。选择依据是地图用途：导航用的占据地图重投影式足够；测绘交付的全景三维模型，值得在收官时跑一次全量。另外记得在成果表里登记回环信息：检出几次、验证通过几次、修正量最大的一段是多少——这三个数字是这份地图"全局一致性"的信用记录。

本节要点回顾

• 漂移是航位推算的结构性命运，回环是唯一对冲手段；
• 三步收官：词袋检索、几何验证、位姿图平差，缺一不可；
• 误回环是最致命故障，双重验证门是保险；
• 位姿图优化是第一章最小二乘的图上大规模版，边权按可信度定；
• 闭合差前后对比（米级到厘米级）是 SLAM 的标准验收报告。

收官动作完成，最后一站清点验收标准：下一节讲评估指标与数据集。