回环位姿先验：几何验证之前先收窄搜索

回环检测我最怕的不是漏检——漏了只是漂移继续累积，至少地图形状还自洽。最怕的是误检 + 硬约束：对称走廊里词袋说「这两帧很像」，几何验证 RANSAC 幸运通过，一条错误回环边把整张图拧坏。位姿先验是我现在放在几何验证之前的第一道门：用里程计漂移模型和空间索引，先把「不可能的回环」砍掉。

为什么纯外观检索不够

DBoW2 / Scan Context / NetVLAD 回答的是「看起来像不像」，不是「是不是同一地点」。感知混叠（perceptual aliasing）在以下场景极常见：

• 对称走廊、重复工位
• 相似货架排列的仓库
• 激光的长直隧道（Scan Context 沿方向轴相似）

召回率可以激进——候选多几个无所谓。但每个候选都跑完整几何验证（RANSAC + PnP / ICP）太贵，且 RANSAC 在内点率阈值松时仍可能「幸运通过」。

先验从哪来

里程计漂移模型：相邻关键帧相对位姿带协方差 Σ_ij，沿轨迹传播得到当前帧位姿的不确定度 Σ_t。漂移随距离增大——走了 100m 的位姿先验椭球远大于 10m。

空间索引：维护关键帧的 (x, y, z) 或 submap anchor 空间哈希 / KD-tree。回环候选检索时，只查「先验椭球内」的历史关键帧，而不是全库暴力比描述子。

拓扑距离：共视图 / 生成树上的 graph distance 过小的帧（刚看过）不应作为回环候选——回环的定义是「重访远期地点」。

门控逻辑

候选 (query, candidate) 通过先验门控需满足：

|| Log(T_prior⁻¹ · T_candidate) || < k · σ_prior

T_prior 是用里程计链预测的 candidate 位姿；σ_prior 是传播协方差对应的标准差；k 通常 2–3（类似 Chi2 门控）。

同时检查：

• 时间间隔 > N 秒 / graph distance > M
• 空间距离在 [d_min, d_max]（太近不是回环，太远先验无意义）

通过先验的候选才进入描述子检索 top-K 或直接进入几何验证。

与几何验证衔接

先验通过 ≠ 回环确认。后续仍需：

1. 特征匹配 + 比率测试
2. 本质矩阵 / 单应 RANSAC（视觉）或 ICP fitness（激光）
3. PnP + RANSAC 估计 T_ab
4. 内点率 > 阈值、重投影 RMSE < 阈值
5. Chi2 检验：rᵀ Ω r < χ²_{0.95, dof}

我还会看 Hessian 最小特征值——退化场景下即使 RANSAC 通过，某些方向的几何约束仍弱。

误回环代价与防御

一次错误回环比漏检 100 次更伤。防御策略：

• 延迟回环：同一地点多个候选一致确认后才插入约束
• 软信息矩阵：回环边初始协方差设大（低信息），Chi2 收敛后再收紧
• Switchable constraints：优化中允许关闭可疑回环边
• Atlas / 多地图：不确定时新建地图，确认后再 merge

我的原则：回环我宁愿漏，也不想要一次错误约束。

该量什么

• 先验门控淘汰率 vs 进入几何验证数
• 几何验证通过率
• 最终接纳回环数 vs 误回环数（需 ground truth 或人工标注）
• 误回环后 ATE 恶化量
• 候选检索耗时（有/无空间索引对比）

延迟回环的实现要点

「同一地点多次候选一致」可以简单做成：spatial cell 内累计 N 次独立几何验证通过才插边。N=2 往往就够挡掉单次幸运 RANSAC，代价是正确回环晚几秒——对仓储 AMR 通常可接受。

协方差传播的可信度

里程计边的 Σ 若拍脑袋设常数，先验椭球形状错，门控等于没做。至少应按 match score / ICP fitness 缩放 Σ；LiDAR 退化方向应 inflate Σ 对应特征方向。先验质量决定门控是否有效。

子地图粒度的先验

LIO 的 submap anchor 比 keyframe 更适合做空间索引——一个 submap 代表 10–20m 轨迹，检索库小、先验传播更平滑。跨 submap 回环时先验门控在 anchor 级做，再在 submap 内 fine align。

小结

先验门控把 impossible 候选挡在几何验证外，是误回环防御的第一道低成本滤网。

参考

• Cummins & Newman, FAB-MAP (spatial prior)
• Carlone et al., Pose Graph Optimization with Pose Priors
• ORB-SLAM2 Loop Closing (covisibility + prior)