回环约束的卡方门控：残差变小不等于约束可信

回环候选过了词袋与几何验证，不代表它该以「硬边」进位姿图。常见失败不是残差很大，而是残差碰巧不大、信息矩阵却给得太狠——对称走廊把两段其实不同的轨迹钉在一起，优化几轮后相对位姿误差被压下去，看起来像「收敛了」，全局拓扑已经错了。门控要回答的问题不是「这条边现在残差多少」，而是「在当前先验下，这条测量是否与状态一致」。

主线可以压成一句话：先用创新量做一致性检验，再决定信息矩阵；鲁棒核是第二道防线，不能替代入边门控。

1. 相对位姿边在图里到底在约束什么

设位姿 $T_{i}, T_{j} \in S E (3)$ ，回环给出相对测量 $T ~_{ij}$ 。右乘扰动下，残差可写成

r_{ij} = Log! (T ~_{ij}^{- 1} T_{i}^{- 1} T_{j}) \in R^{6} .

二次型代价为

E_{ij} = ρ! (r_{ij}^{T} Ω_{ij} r_{ij}),

其中 $Ω_{ij} = Σ_{ij}^{- 1}$ 是信息矩阵。 $Ω$ 越大，优化器越愿意扭曲整条轨迹去满足这条边。把 $Ω$ 设成「几何验证分数」或「匹配内点数」的单调函数，而不看当前图上的不确定性，是误回环最常见的放大机制。

几何验证（PnP/ICP）回答的是「这两帧之间能不能找到一个局部一致的相对位姿」；卡方门控回答的是「这个相对位姿与当前全局估计是否一致」。两者都不充分单独决定入边，但后者在长程漂移场景里往往更关键。

残差定义还绑定扰动约定。若实现用左乘扰动 $δ ξ^{\land} T$ ，雅可比与协方差传播的伴随方向会翻转；门控代码必须与 g2o/GTSAM 边类型的扰动约定一致，否则 $S$ 正确、 $d^{2}$ 仍可能系统性偏大或偏小。落地时先在同一套李群库里写一个「已知真值 + 已知噪声」的单元测试，确认残差零点与数值雅可比对齐，再谈门控阈值。

2. 创新量：门控看的是「意外程度」，不是「残差范数」

在入边前，把当前估计 $T^_{i}, T^_{j}$ 当作先验均值，相对位姿的预测为 $T^_{ij} = T^_{i}^{- 1} T^_{j}$ 。创新量为

ν = Log! (T ~ ij^{- 1} T^ij) .

若测量噪声协方差为 $Σ_{z}$ ，相对位姿估计协方差为 $Σ_{T^}$ （由边缘化后的位姿边际协方差或局部线性化得到），则创新协方差近似为

S = Σ_{T^} + Σ_{z} .

一致性统计量是马氏距离平方

d^{2} = ν^{T} S^{- 1} ν .

在线性高斯近似下， $d^{2} \sim χ_{k}^{2}$ ，自由度 $k$ 通常取 6（完整 $S E (3)$ ）；若只约束平面运动， $k = 3$ 。门控阈值取

d^{2} < χ_{k, 1 - α}^{2},

例如 $α = 0.05$ 时 $χ_{6, 0.95}^{2} \approx 12.59$ ， $χ_{3, 0.95}^{2} \approx 7.815$ 。这里的关键点是：同一个 $∣ ν ∣$ ，在 $S$ 很大（两帧都很不确定）时可能通过，在 $S$ 很小（图已经很紧）时必须拒绝。 固定欧氏阈值 $∣ r ∣ < τ$ 正好丢掉了这个尺度。

还可以把门控写成归一化创新平方（NIS）。对多条候选边画 NIS 直方图：真回环应大致贴合 $χ_{k}^{2}$ 的理论密度；假回环会在右尾堆积。若真回环的 NIS 整体偏大，优先怀疑 $Σ_{z}$ 过小或 $Σ_{T^}$ 漏了伴随；若整体偏小，则 $S$ 过松，门控几乎不拒识。

3. 创新协方差的完整推导

把相对位姿写成复合函数。令世界系位姿为 $T_{i}, T_{j}$ ，相对量 $T_{ij} = T_{i}^{- 1} T_{j}$ 。在右乘扰动下

T_{i} \leftarrow T_{i} Exp (ξ_{i}), \qquadT_{j} \leftarrow T_{j} Exp (ξ_{j}),

一阶有

T_{ij} \leftarrow T_{ij} Exp! (A d (T_{ij}^{- 1}) ξ_{j} - ξ_{i})

（符号随 $Exp / Log$ 与伴随定义微调，但结构不变： $j$ 的扰动经 $A d (T_{ij}^{- 1})$ 映到相对系， $i$ 的扰动以负号进入）。因此相对位姿在切空间的协方差为

Σ_{T^} = [- I A d (T^ij^{- 1})] [Σ ii S i g m a_{j i} Σ_{ij} Σ_{j j}] [- I A d (T^_{ij}^{- 1})^{T}] .

展开即

Σ_{T^} = Σ_{ii} + A d (T^ij^{- 1}) Σ j j A d (T^ij^{- 1})^{T} - A d (T^ij^{- 1}) Σ_{j i} - Σ_{ij} A d (T^_{ij}^{- 1})^{T} .

很多实现只写对角块、丢掉交叉项 $Σ_{ij}$ 。当 $i, j$ 已经通过里程计链或其它回环强耦合时，交叉项不可忽略：丢掉它会高估 $Σ_{T^}$ ，门控变松，假回环更容易混进来。短基线相邻帧交叉相关尤其显著。

伴随矩阵 $A d (T) \in R^{6 \times 6}$ （ $S E (3)$ ）完整形式为：若 $T = (R, t)$ ，

A d (T) = [R t^R 0 R],

其中 $t^$ 为平移的反对称矩阵。漏掉 $t^R$ 块，旋转不确定度不会耦合进平移，走廊里「偏航不确定却把横向钉死」的假象就会出现。平面 $S E (2)$ 情形降维为 $3 \times 3$ ：

A d (T) = [cos θ s in θ - sin θ cos θ t_{y} - t_{x} \0 01],

自由度与阈值都要跟着改成 $k = 3$ 。

测量噪声 $Σ_{z}$ 也应在同一切空间表达。若前端给出的是 ICP 点到面海森的逆，或 PnP 像素协方差传播到位姿的 $Σ_{ξ}$ ，要确认其扰动约定与图优化边一致；必要时用伴随把前端协方差搬到边残差所用的右乘/左乘坐标。不一致时，常见现象是真回环 $d^{2}$ 长期偏大，团队却去调 $α$ ，把统计门控调成经验阈值。

4. 协方差从哪里来，错了会发生什么

$S$ 的估计质量决定门控是否有用。实践中常见三条路径：

位姿图边缘协方差：对信息矩阵做稀疏求解（如 Cholmod 下的选定变量协方差），取出 $i, j$ 的边际块与交叉块，再经上一节的伴随复合得到 $Σ_{T^}$ 。成本高，但与优化器状态一致。大型图可对窗口内节点做条件协方差，或用对角线近似作初筛、对通过者再算精确块。
里程计链传播：用两帧之间的里程计/VIO 协方差按复合规则串联。便宜，但忽略其他回环已经注入的信息，容易在「图已收紧」后仍过于宽松。适合作为前端召回阶段的粗门控，不适合最终硬边决策。
经验对角阵：把 $Σ_{z}$ 和 $Σ_{T^}$ 都设成固定对角。实现最快，也最容易在高置信错误匹配上放行。

若 $S$ 被系统性放大，门控形同虚设；若被系统性缩小，真回环在漂移较大时会被误杀。验收时不要只报「门控通过率」，而要分开报：真回环召回、假回环误接收、以及 $d^{2}$ 在真/假集合上的分位数，并画 NIS-QQ 图对照 $χ^{2}$ 理论分位。

计算边际协方差时还有数值坑：信息矩阵在规范自由度（gauge）上奇异，必须固定锚定位姿或使用伪逆/协方差空间投影。对锚定节点相关的回环， $Σ_{ii}$ 接近零，门控会变严——这是预期行为，不是 bug。反过来，若锚定很弱、整图在某方向漂， $S$ 变大，早期回环更容易通过；这解释了为何「刚建图时宽松、多回环后变严」是健康现象。

5. 固定阈值、卡方门控、鲁棒核、可开关约束

四者常被混成一句「加个鲁棒核就行了」。职责应拆开：

机制	决策时刻	主要防什么	失败模式
固定残差阈值	入边前/后	明显离谱的相对位姿	忽略不确定性尺度
卡方门控	入边前	与当前先验不一致的测量	$S$ 估错导致松/紧失真
鲁棒核 $ρ$	优化中	已入边的离群残差	高信息错误边仍可能拉歪图
Switchable / DCS	优化中	结构性错误边的持续拉力	多假边共谋、额外变量成本

鲁棒核（Huber、Cauchy、Geman-McClure）在迭代中降低大残差权重，适合传感器偶发野值。但对结构性错误回环——局部几何说得通、全局拓扑错误——初值残差可能并不大，核函数来不及降权，图已被掰弯。

Switchable constraints（Sünderhauf 等）给每条可疑边乘一个开关变量 $s \in [0, 1]$ ，代价近似 $s^{2} ∣ r ∣_{Ω}^{2} + Ψ (s)$ 。优化可以「关掉」坏边，而不是只靠核函数降权。代价是变量增多、需要先验 $Ψ$ 防止所有边被关掉，且多条共谋的假回环仍可能一起保持 $s \approx 1$ 。

Dynamic Covariance Scaling (DCS) 不显式引入开关变量，而是按当前残差自适应缩放信息矩阵，效果上接近某种鲁棒核的信息域写法，实现侵入更小。它对「残差已经变大」的边很有效；对「初值碰巧对齐的错误回环」同样可能反应偏晚。

更稳的工程顺序是：

几何验证得到 $T ~_{ij}$ 与候选 $Σ_{z}$ ；
用当前图估计算 $d^{2}$ ，不通过则丢弃或降为弱先验；
通过者再以保守 $Ω$ 入边，优化时仍可套鲁棒核或 DCS；
对高风险边（长时差、低纹理、对称场景）用 switchable 变量做二次保险；
优化后做一次后验卡方或开关变量复查， $s$ 被关到接近 0 的边进入审计列表。

「残差变小」只说明优化器找到了使代价下降的状态，不说明测量生成模型正确。后验 $∣ r ∣$ 小却 $d_{prior}^{2}$ 曾极大的边，应进审计列表。卡方门控与 switchable 不是互斥：前者减少坏边进入，后者限制漏网之鱼的破坏力。

6. 实现要点：自由度、延迟入边与信息矩阵封顶

自由度必须与残差维度一致。若回环只提供平面 $x, y, θ$ ，却用 $k = 6$ 的阈值，门控会过松。若对偏航单独建模、竖直方向几乎无约束，也不要把无信息方向的对角元设成巨大信息值——那等于在不可观方向伪造测量。

延迟入边比「立刻钉死」更稳。候选先以很低信息量进入，或仅记录为假想边；等后续更多里程计与其他回环把局部一致性抬高后，再提升 $Ω$ 或做第二次卡方。这对长隧道出口、视觉短暂失效后的第一批回环特别有效。第二次卡方应使用更新后的 $Σ_{T^}$ ，否则延迟没有意义。

信息矩阵封顶是工程上便宜的保险丝。即便几何内点很多，也把 $Ω$ 的特征值上界夹到与里程计边同量级的若干倍，避免单条边支配整图。封顶不是统计最优，但能把「匹配分数直接映射成信息」的伤害卡住。封顶后若仍想保留前端置信度差异，可以用有界单调映射，例如 $Ω = Ω_{0} \cdot (1 + η \frac{n _{inlier}}{n _{inlier} + n _{0}})$ ，并保证 $η$ 不会突破封顶。

伪代码层面，入边检查可以长这样：

cpp

// nu: 6x1 innovation, S: 6x6 innovation covariance
const double cond = S.norm() * S.inverse().norm();  // 或用特征值比
if (cond > kMaxCond) {
  return LoopDecision::Defer;  // 病态：延迟而非硬拒
}
const double d2 = nu.dot(S.ldlt().solve(nu));
constexpr double kChi2_6_0p95 = 12.592;
if (d2 > kChi2_6_0p95) {
  return LoopDecision::Reject;
}
Information Omega = Sigma_z.inverse();
Omega = ClampEigenvalues(Omega, /*max_info=*/info_cap);
graph.addEdge(i, j, T_meas, Omega);

注意 S.ldlt().solve 前要检查 $S$ 的条件数；病态时宁可不入边或延迟，也不要用近零特征值把 $d^{2}$ 撑爆或压扁。对「Defer」状态要有上限次数与超时降级策略，避免候选队列无限膨胀。

7. g2o 与 GTSAM 实践注意

g2o。 EdgeSE3 / EdgeSE3Expmap 的信息矩阵直接进 $Ω$ ；卡方应在 addEdge 之前算，而不是在 optimize 之后看 chi2() 再删边——后验卡方会把「已经被边拉歪的状态」当成新先验，漏检率上升。若用 RobustKernelHuber，记住核函数改的是优化权重，不改变你入边前用的 $Σ_{z}$ 。从 SparseOptimizer 取协方差需要 computeMarginals；只取对角线块时，记得按第三节补交叉项与伴随。多线程优化与边集合修改要加锁：门控线程与优化线程并发改图是线上难复现扭曲的来源之一。

GTSAM。 BetweenFactor<Pose3> 的噪声模型常用 noiseModel::Gaussian::Covariance 或 Isotropic；门控阶段应用同一噪声模型构造创新协方差的测量部分。Marginals 可给出联合边际，适合精确 $S$ 。若图里混用 Robust 噪声模型，入边门控仍应基于未鲁棒化的高斯 $Σ_{z}$ ——否则等于把鲁棒性提前折进一致性检验，阈值语义变糊。ISAM2 更新后边际会变，延迟入边的第二次卡方要拿当前 Bayes 树对应的边际，不要缓存过期协方差。

两边的共同建议：把门控做成与优化器解耦的纯函数——输入 $T^i, T^j, Σ ii, Σ j j, Σ_{ij}, T ~, Σ_{z}$ ，输出 $d^{2}$ 与决策——便于单测与日志回放。

相对位姿协方差的参考实现骨架可以写成：

cpp

Matrix6 relativeCovariance(const Pose3& Ti, const Pose3& Tj,
                           const Matrix6& Sii, const Matrix6& Sjj,
                           const Matrix6& Sij) {
  const Pose3 Tij = Ti.inverse() * Tj;
  const Matrix6 Ad = Tij.inverse().AdjointMap();  // Ad(Tij^{-1})
  // Sigma = Sii + Ad*Sjj*Ad^T - Ad*Sji - Sij*Ad^T
  return Sii + Ad * Sjj * Ad.transpose()
       - Ad * Sij.transpose() - Sij * Ad.transpose();
}

把该函数与有限差分扰动对照：给 $T_{i}, T_{j}$ 加微小右乘扰动，用样本协方差核对解析式。交叉项与 $A d$ 任一写反，单测会立刻失败——这比在实车上调 $α$ 便宜得多。

8. $α$ 、多假设与渐进式置信

显著性水平 $α$ 不是调参旋钮的万金油。 $α = 0.05$ 表示线性高斯假设成立时，真测量仍有约百分之五概率被拒。长轨迹上候选极多，还要考虑多重检验：同一地点反复召回时，若每次独立用 $0.05$ ，假通过期望次数会上升。工程上更稳的是：

对「第一道硬门控」用偏严的阈值（或等价地要求 $d^{2}$ 落在更低分位）；
对通过者仍用信息封顶 + 鲁棒核，而不是一次卡方通过就赋予超高 $Ω$ ；
把历史通过次数、时间间隔、场景标签（走廊/室外）写进决策，而不是只看单次 $d^{2}$ 。

多假设回环（同一查询帧对应多个候选库帧）时，不要把所有通过卡方的边同时以满信息入图。更合理的是保留最优的一条，或对前几名做互斥开关（至多一条 $s \approx 1$ ）。否则共谋假边会在优化里互相「证明」对方合理。

渐进式置信与延迟入边配套：候选边经历「观察 → 低信息试探 → 二次卡方 → 正常信息」四态。试探阶段若优化后残差反而与先验创新一致地恶化周边里程计边，应回滚该边。这比事后从已扭曲地图里删边更可控。

9. 错误回环注入：怎么验收门控真的在工作

离线验收比线上「感觉轨迹顺」可靠。准备一段有真值或高质量参考轨迹的日志，构造四类边：

真回环：时间间隔足够、空间重访成立；
近假回环：对称结构，相对位姿与真值差一个走廊宽度或 $18 0^{\circ}$ 偏航；
远假回环：随机帧对，但强制给一个「看起来合理」的 $Σ_{z}$ ；
共谋假回环：多条近假边同时注入，模拟词袋在重复结构上批量召回。

对每个候选记录：是否通过几何验证、 $d^{2}$ 、 $S$ 的 $λ_{m i n} / λ_{m a x}$ 、是否入边、入边后 ATE/RPE、最大连通分量是否被错误合并、以及 switchable 的最终 $s$ （若启用）。通过标准建议同时满足：

近假集合误接收率明显低于无门控基线；远假接近全拒；
真回环召回不低于可接受下界（按场景分层：室内走廊、开阔室外分别报）；
注入近假后，段间相对位姿误差或楼层/房间标签一致性不被破坏；
共谋集合下，卡方+封顶仍应优于「仅鲁棒核」；若只有 DCS/switchable 能救，说明入边层过松，需要收紧 $S$ 或 $α$ ；
NIS 分位数：真回环的 $d^{2}$ 中位数接近 $k$ （ $χ_{k}^{2}$ 均值），假回环中位数显著更大。

消融建议固定同一前端候选集，只切换后端策略：无门控、固定阈值、卡方、卡方+封顶、卡方+DCS、卡方+switchable。主指标用「拓扑破坏率」而不是平均残差——平均残差会被优化器涂平。若关闭卡方、只留鲁棒核，近假回环仍能把轨迹拉出数米级偏差，而最终平均残差看起来「还行」——这正是「残差变小不等于约束可信」的可复现证据。

验收数据设计要可复现。建议固定随机种子生成远假集合；近假集合用语义标注的对称段（例如「左右墙纹理近似」的走廊帧对）而不是纯随机扰动，否则测不到真实失败模式。对每条注入边保存：帧 ID、 $T ~$ 、 $Σ_{z}$ 来源、门控输入的边际协方差摘要、 $d^{2}$ 、决策、优化后位姿图哈希或关键节点位姿。回归时若某次提交让近假误接收率回升，应能直接 diff 出是 $A d$ 写错、交叉项丢失，还是信息封顶被关掉。

还可以做蒙特卡洛校准：在仿真里按已知 $Σ_{z}$ 污染真回环测量，画经验 CDF 与 $χ_{k}^{2}$ 理论 CDF。两者偏离超过设定带宽时，先修协方差链，再动 $α$ 。现场日志若没有真值，至少用「人工确认的重访段」与「人工确认的错误段」做半监督分层，否则门控阈值只能跟着体感漂。

拓扑破坏率的可操作定义也很重要。一种做法是：把参考轨迹按房间/楼层/子图切开，优化后检查跨段相对位姿是否仍落在参考不确定性内；另一种是维护「禁止合并」的关键帧对列表，优化后若它们的估计距离被错误压到回环尺度以下，记一次破坏。ATE alone 不够——整条轨迹可以整体拧成错误拓扑却仍有看起来还行的绝对误差曲线。

10. 和前端回环验证文章的边界

召回层可以激进，几何验证必须保守，这是前端问题。本文站在后端图优化接口：即便前端已经给出相对位姿，后端仍要用当前先验做一致性检验。两侧阈值不要共用一个魔法数；前端关心匹配几何，后端关心图一致性。日志里应同时留下匹配分数、 $Σ_{z}$ 、边际协方差摘要、 $d^{2}$ 、入边 $Ω$ 、最终核权重或开关变量，否则线上一次扭曲地图后无法回溯是哪一层放行的。

与前端协作的接口建议显式化：前端输出 {T_ij, Sigma_z, quality, hypothesis_id}；后端返回 {decision, d2, S_eigen_ratio, omega_scale}。quality 可以影响封顶前的 $Ω$ 初值，但不得绕过 $d^{2}$ 门控。hypothesis_id 用于多假设互斥。这种契约让两边可以独立做回放：前端不需要跑完整 g2o，后端不需要重跑词袋。

线上事故复盘时，按时间戳对齐三份日志最有效：前端候选、门控决策、优化器边集合变更。若只有最终轨迹，很难区分「前端召回太激进」与「后端 $S$ 过松」。把 $d^{2}$ 与 $S$ 的特征值比做成时序指标，还能在对称场景进入前看到「边际协方差异常收缩/膨胀」的前兆。

卡方门控不是银弹：非高斯重尾、严重非线性、以及错误的 $S$ 都会让 $χ^{2}$ 阈值失真。它的价值在于把「要不要相信这条长程边」从经验阈值推进到可审计的统计量，并逼迫实现者显式维护协方差。做不到完美 $S$ 时，也要宁可保守拒识，也不要让单条漂亮残差改写整张地图。把创新协方差、伴随变换与门控决策放进单测和回放工具链，比把阈值写进配置文件却从不画 NIS 分布，更能防止下一次对称走廊事故。

最后回到开题的那句判断：优化器把残差压小，只说明当前图在给定边集合下找到了更低代价的状态，不说明边集合对应真实拓扑。卡方门控逼你在入边前回答「以现在的不确定性，这条测量算不算意外」；伴随变换与交叉协方差逼你把「不确定性」算对；封顶、延迟与 switchable/DCS 则承认统计近似总会漏网。把这三层叠好，对称走廊里那条「看起来很美」的硬边，才有机会在改写地图之前被拦下。验收时盯住拓扑破坏率与 NIS 校准，而不是平均残差——后者天生会站在优化器那边说话。门控代码一旦与扰动约定、伴随传播和边际协方差同源，这条防线才站得住。