90_鲁棒估计与外点剔除

博士前数学路线图·第五批·子专题 F：鲁棒估计与外点剔除——从 M-estimator 到 GNC 的工程化路线¶

前置自测¶

答不出 2 题以上，建议先回 B（因子图与 Gauss-Newton）和 C（iSAM2）复习。

Gauss-Newton 的正规方程是什么？Jacobian \(J\) 和信息矩阵 \(\Omega\) 如何组合？
SLAM 因子图中"回环约束"是什么？它为什么容易出错？
最小二乘 \(\min\sum\|r_i\|^2\) 隐含了什么分布假设？这个假设何时失效？
\(\chi^2\) 分布是什么？\(\chi^2_{d,0.99}\) 代表什么含义？
什么是凸函数？Huber 损失为什么是凸的而 Geman-McClure 不是？

本章目标¶

学完本章后，你应当能够：

解释为什么 L2 损失（最小二乘）在有外点时失效，用 breakdown point 量化其脆弱性
写出 M-estimator 的 IRLS 正规方程，理解加权 GN 如何自然嵌入三大 SLAM 库
区分凸（Huber/Fair）与非凸（Tukey/GM/Welsch/TLS）鲁棒核的适用场景和初值敏感性
推导 Black-Rangarajan 半二次对偶，理解 GNC 的退火机制及其与鲁棒核族的关系
在 GTSAM/Ceres/g2o 中正确配置鲁棒因子，理解三库阈值单位的差异并正确映射

知识树总览¶

鲁棒估计与外点剔除
├── 动机与问题定义
│   ├── 外点来源五类（§F.1）
│   ├── L2 的 breakdown point = 1/N（§F.2）
│   └── 影响函数 ψ(r) 的有界性需求
├── M-estimator 理论
│   ├── 正规方程与 IRLS（§F.3）
│   ├── 鲁棒代价函数全景表（§F.4）
│   │   ├── 凸：Huber / Fair / Pseudo-Huber
│   │   └── 非凸：Cauchy / GM / Tukey / Welsch / TLS / DCS
│   └── Triggs 修正与 Ceres Corrector（§F.7）
├── 半二次对偶与 GNC
│   ├── Black-Rangarajan 1996 半二次框架（§F.9）
│   ├── GNC-GM / GNC-TLS 退火调度（§F.10-F.11）
│   └── Yang-Carlone RA-L 2020 收敛保证
├── Switchable Constraints 与 DCS
│   ├── Sünderhauf 2012: SC 辅助变量（§F.13）
│   ├── Agarwal 2013: DCS 闭式权重（§F.15）
│   └── Max-Mixture（§F.14）
├── Pairwise Consistency Maximization
│   ├── PCM 一致性矩阵与最大团（§F.16）
│   └── 与 GNC 的组合策略
├── 工程实践
│   ├── GTSAM / Ceres / g2o 三库映射（§F.23）
│   ├── 阈值单位转换表（§F.23）
│   └── 常见陷阱 14 条（§F.24）
└── RANSAC 与方法对比
    ├── RANSAC 成功率公式（§F.25）
    └── 组合/连续/认证方法的互补

与主干内容的关系：B 和 C 建立的因子图优化框架默认所有因子（约束）都是正确的——残差服从高斯分布，二次代价 \(\|r\|^2\) 即最优。但真实世界中，感知混叠、动态物体、传感器故障会产生大量外点（outlier），闭环阶段外点比例常达 10-50%，多机器人地图合并可达 >80%。单个错误回环就能通过二次代价的无界影响函数摧毁整张位姿图。本专题提供从鲁棒统计学到 SLAM 工程的完整路线：M-estimator 的正规方程、IRLS 迭代、Black-Rangarajan 半二次对偶、GNC 退火调度，以及 GTSAM/Ceres/g2o 三库的阈值映射。这些工具直接作用于 B/C 建立的因子图后端，与 E（Certifiable Perception）互补——E 解决非凸性问题（局部 vs 全局最优），F 解决数据污染问题（内点 vs 外点）。建议在完成 B/C 后阅读本专题。

底线陈述（BLUF）：二次损失 \(\|r\|^2\) 的 breakdown point = 1/N，单个错误回环就能毁掉整张位姿图。工业级 SLAM 后端必须替换为**鲁棒损失**。档位 3 的最小可用套装是：Huber（凸、非 redescending、最安全兜底）+ DCS（闭式权重、近似 Geman-McClure、ICRA'13 开创性）+ GNC-TLS（Yang-Carlone RA-L'20，>80% 外点仍收敛，MIT-SPARK / Kimera-RPGO 的默认方案）。档位 4 的进阶内容是：Black-Rangarajan 半二次对偶（把非凸 \(\min\sum\rho(r_i)\) 转为 \(\min_{x,w}\sum[w_i r_i^2+\Phi(w_i)]\) 的交替优化，理论根基）+ Barron 统一损失族（\(\alpha\) 可学习）+ PCM（回环检测后、优化前的一次性剔除，最大团求解）。工程上最易踩的坑是**三库阈值单位不一**：Ceres 作用在 \(s=\|r\|^2\)（平方残差空间）、GTSAM 作用在 \(\tilde r=\Sigma^{-1/2}r\)（白化标量空间，即 \(\sigma\) 单位）、g2o 作用在 \(e=r^\top\Sigma^{-1}r\)（Mahalanobis 平方空间）——同一个 \(\delta=1.0\) 在三者中含义迥异。另一易错点是**GNC 与 iSAM2 不兼容**：GNC 本质是 batch outer-loop，必须全图重求解；iSAM2 的 fluid relinearization 无法一边增量一边退火。因此在线 VIO 用 DCS/Huber，离线/周期性 PGO 用 GNC+PCM。

§F.1 外点在 SLAM 中从何而来 ⭐¶

外点（outlier）不是小概率现象，而是机器人感知的**系统性结构缺陷**。按来源可分为五类。

感知混叠（perceptual aliasing）是最致命的来源。DBoW2/NetVLAD 的词袋或描述子在走廊、相似房间、对称建筑外立面上产生**几何错位的正确视觉匹配**——RANSAC 能通过几何验证，但当三维结构本身对称时几何验证也会通过。一个错误回环把两条完全无关的轨迹强行绑在一起，PGO 会拉出 10 米以上的系统偏差。动态物体（行人、车辆、摆动的树叶）在 VO/VIO 中被作为静态特征跟踪，在短时间内其运动与相机自运动混淆成视差噪声；遮挡边缘的特征点因深度歧义产生近似正确但尺度错的对应。传感器层面的外点包括 GPS 多径（urban canyon）、LiDAR 在玻璃/远距离反射的错点、IMU 的偶发 spike。最后是**数据关联错误**——在 EKF-SLAM 或 ORB-SLAM 的局部地图中把观测绑到错误的地标（JCBB 的 \(\chi^2\) gate 失效）。

核心观察：外点比例在闭环阶段常达 10–50%，多机器人地图合并可达 >80%（Mangelson 2018 的实验数据）。这就是为什么鲁棒后端不是可选项而是必选项。

§F.2 二次损失的脆弱性与 breakdown point ⭐⭐¶

Breakdown point（BP）（Donoho-Huber 1983）定义为：能使估计器跑到无穷的最小污染比例。普通最小二乘（OLS / L2）的 BP \(=1/N\)：一个足够大的外点可以把样本均值拽到任意值，在回归中拽歪整个参数向量。这是 \(\rho(r)=r^2/2\) 的**二次增长**直接导致的——一个 \(|r|=100\) 的残差贡献 \(5000\) 的代价，主导了 \(N-1\) 个 \(|r|\approx 1\) 的内点（总代价 \(\approx N/2\)）。

鲁棒代价函数反其道而行：

\[\text{inlier regime}:\ \rho(r)\approx r^2/2 \qquad \text{outlier regime}:\ \rho(r) = O(|r|) \text{ 或 } O(1)\]

影响函数（influence function）\(\psi(r)=\rho'(r)\) 量化单个观测的"拉力"：L2 的 \(\psi(r)=r\) 无界，L1 的 \(\psi(r)=\mathrm{sign}(r)\) 有界，redescending \(\rho\)（Tukey/GM/Welsch）的 \(\psi(r)\to 0\) 使巨大外点**完全失声**。

BP 的详细对比（Huber 1984 Ann. Stat.；Maronna-Bustos-Yohai 1979）：

估计器	位置参数 BP	回归 BP（固定 \(p\)）	备注
OLS (L2)	0	\(1/N\)	一个外点即失效
L1（中位数）	1/2	\(1/N\)	位置下最优；回归中 leverage 仍能击穿
Huber（单调有界 \(\psi\)）	1/2	\(1/(p+1)\)	高维下 BP 迅速恶化
Redescending（Tukey/GM/Welsch）单独使用	\(\approx 1/2\)	\(1/(p+1)\)	非凸 → 需要好初值
MM-/S-estimator（两阶段）	1/2	up to 1/2	Yohai 1987：先 S 再 M-step

教材参考：Huber Robust Statistics 2nd ed. 2009 Ch.1 §1.4；Hampel et al. Robust Statistics: The Approach Based on Influence Functions 1986 §1.2, §2.3。

直觉：SLAM 后端的 \(p\)（状态维度）几乎等于位姿数 × 6，是**数万级**。Huber 单独用的回归 BP \(\sim 1/p\) 趋于 0——这就是为什么工业界真正依赖的不是 Huber 本身，而是 Huber 作为 GNC 的起点**或 **DCS 的 fallback。

§F.3 M-estimator 的正规方程 ⭐⭐¶

把 \(\min\sum\|r_i\|^2\) 替换为

\[\min_x \sum_i \rho\big(\|r_i(x)\|\big)\]

其中 \(\rho:\mathbb R_{\ge 0}\to\mathbb R_{\ge 0}\) 单调非减、\(\rho(0)=0\)。定义

\[\psi(r) = \rho'(r),\qquad w(r) = \psi(r)/r\]

若 \(r_i\) 是标量残差，梯度为

\[g=\sum_i J_i^\top\psi(r_i)=\sum_i w(r_i)J_i^\top r_i=J^\top Wr,\]

其中 \(W=\mathrm{diag}(w(r_i))\)、\(J\) 堆叠 Jacobian。若 \(r_i\) 是块残差，通常先对白化后的范数 \(s_i=\|r_i\|\) 定义 \(\rho(s_i)\)，再把同一个标量权重 \(w_i=\psi(s_i)/s_i\) 乘到整个残差块上。Gauss-Newton 近似（固定当前 \(w\)，丢掉二阶项）给出**加权正规方程**：

\[\boxed{\Big(\sum_i w(r_i)\, J_i^\top J_i\Big)\delta = -\sum_i w(r_i)\, J_i^\top r_i}\]

这就是 IRLS 的核心。关键洞察：形式上和标准 Gauss-Newton 一样，只是所有求和被 \(w(r_i)\) 加权——因此三大 SLAM 库（Ceres/GTSAM/g2o）几乎不用改优化器，只需在每次迭代更新权重。

思维陷阱：看到"形式和 GN 一样"容易误以为收敛性也一样。但对非凸 \(\rho\)（Tukey/GM/Welsch），权重 \(w(r_i)\) 本身是 \(x\) 的函数，IRLS 每步固定 \(w\) 再解 LS 等价于一种特殊的 coordinate descent。若初值让真内点也被降权（\(w_i\approx 0\)），该因子对 Hessian 无贡献，优化器看不到它，即便后续迭代拉近也无法恢复——这是 GNC 从凸起点退火的根本动机。

参考：Huber 2009 Ch.4；Hampel 1986 §2.3；Zhang 1997 Parameter Estimation Techniques §4.2。

IRLS 收敛性的直觉与条件¶

IRLS 每步"固定权重 → 解加权 LS → 更新权重"，本质是一种**半迭代 (half-iteration)** 或 MM 算法（Majorize-Minimize）。其收敛性取决于 \(\rho\) 的凸性：

凸 \(\rho\)（如 Huber）的收敛保证：

若 \(w(r)=\psi(r)/r\) 关于 \(|r|\) 非增，且 \(r^2 w(r)\) 关于 \(r^2\) 凸，则函数

\[Q(x;\bar{x}) = \sum_i w(r_i(\bar{x}))\cdot r_i(x)^2\]

是 \(\sum_i \rho(r_i(x))\) 的**上界（majorant）**，即 \(Q(x;\bar{x})\ge \sum_i\rho(r_i(x))\) 对所有 \(x\)，且等号在 \(x=\bar{x}\) 处成立。因此：

\[F(x_{k+1}) \le Q(x_{k+1}; x_k) \le Q(x_k; x_k) = F(x_k)\]

第一个不等号来自 majorant 性质；第二个不等号来自 \(x_{k+1}\) 最小化 \(Q(\cdot; x_k)\)。这保证了 IRLS 的**单调下降**。对严格凸 \(\rho\)，任何聚点都是全局最优。

非凸 \(\rho\)（如 GM/Tukey）的局面：

majorant 性质可能不成立（因为 \(w(r)\) 不满足所需的单调性条件），IRLS 可能**不单调**。工程上仍有可能收敛（类似 EM 算法对非凸似然），但无全局保证。这就是 GNC 必须出场的原因。

数值稳定性注意事项：

当 \(r_i\approx 0\) 时，\(w(r)=\psi(r)/r\) 的计算可能出现 \(0/0\)。对 Huber：\(w(r)=\min(1, k/|r|)\)，当 \(r\to 0\) 时 \(w\to 1\)（连续）。对 Tukey：\(w(r)=(1-(r/c)^2)^2\) 当 \(|r|<c\)，\(r=0\) 时 \(w=1\)。实现中通常加一个小量 \(\epsilon=10^{-8}\) 防止除零：\(w=\psi(r)/(r+\epsilon)\)。

反事实推理——如果不用 IRLS 而直接对非凸 \(\rho\) 做梯度下降会怎样？ 梯度为 \(g=\sum_i\psi(r_i)J_i^\top\)，步长需要 line search。对 redescending \(\rho\)，梯度在大残差区趋于零—— 这使梯度下降在外点附近几乎停滞，收敛极慢。IRLS 通过在每步"冻结"权重然后求解**加权最小二乘**（凸子问题），获得了比原始梯度下降快得多的收敛速度——本质上是利用了残差的**二次结构**做局部 Gauss-Newton 加速。

§F.4 常见鲁棒代价函数全景表 ⭐⭐¶

下表中残差 \(r\) 通常已**白化**（\(r\leftarrow \Sigma^{-1/2}r\)），常数 \(k\)/\(c\) 是作用在白化残差空间的阈值。数值给 95% Gaussian 渐近效率推荐值（Zhang 1997 Table 1；MacTavish-Barfoot CRV 2015 Table I）。

名称	\(\rho(r)\)	\(\psi(r)\)	\(w(r)=\psi/r\)	凸	Redescending	推荐 \(k/c\)
L2	\(r^2/2\)	\(r\)	\(1\)	✓	×	—
L1	\(\lvert r\rvert\)	\(\mathrm{sign}(r)\)	\(1/\lvert r\rvert\)	✓（不严格）	×	—
Huber	\(\begin{cases}r^2/2,&\lvert r\rvert\le k\\ k\lvert r\rvert-k^2/2,&\lvert r\rvert>k\end{cases}\)	\(\begin{cases}r,&\lvert r\rvert\le k\\ k\,\mathrm{sign}(r),&\lvert r\rvert>k\end{cases}\)	\(\min(1,k/\lvert r\rvert)\)	✓	×	\(k=1.345\)
Fair	\(c^2\big(\lvert r\rvert/c-\ln(1+\lvert r\rvert/c)\big)\)	\(r/(1+\lvert r\rvert/c)\)	\(1/(1+\lvert r\rvert/c)\)	✓	×	\(c=1.3998\)
Pseudo-Huber / Charbonnier	\(c^2\big(\sqrt{1+(r/c)^2}-1\big)\)	\(r/\sqrt{1+(r/c)^2}\)	\(1/\sqrt{1+(r/c)^2}\)	✓	×	—
Cauchy / Lorentzian	\(\tfrac{c^2}{2}\ln\big(1+(r/c)^2\big)\)	\(r/(1+(r/c)^2)\)	\(1/(1+(r/c)^2)\)	×	软	\(c=2.3849\)
Geman-McClure	\(\dfrac{c^2\,r^2/2}{c^2+r^2}\)	\(\dfrac{c^4\,r}{(c^2+r^2)^2}\)	\(\dfrac{c^4}{(c^2+r^2)^2}\)	×	✓	\(c\approx 1\)（DCS \(\Phi\)）
Tukey biweight	\(\begin{cases}\tfrac{c^2}{6}\big[1-(1-(r/c)^2)^3\big],&\lvert r\rvert\le c\\ c^2/6,&\lvert r\rvert>c\end{cases}\)	\(\begin{cases}r(1-(r/c)^2)^2,&\le c\\ 0,&>c\end{cases}\)	\(\begin{cases}(1-(r/c)^2)^2,&\le c\\ 0,&>c\end{cases}\)	×	✓（硬截断）	\(c=4.6851\)
Welsch / Leclerc	\(\tfrac{c^2}{2}\big[1-\exp(-(r/c)^2)\big]\)	\(r\exp(-(r/c)^2)\)	\(\exp(-(r/c)^2)\)	×	✓	\(c=2.9846\)
TLS	\(\tfrac12\min(r^2,\bar c^2)\)	\(\begin{cases}r,&\lvert r\rvert\le\bar c\\ 0,&>\bar c\end{cases}\)	\(\{1,0\}\)	×	✓（0/1 硬）	\(\bar c^2=\chi^2_{n,\alpha}\)
DCS	见 §F.15	—	\(\min\!\big(1,(\tfrac{2\Phi}{\Phi+r^2})^2\big)\)	×	✓	\(\Phi=1\)

阅读要点： - Huber 是唯一既凸又鲁棒的"主力"。在线性回归或整体凸目标中，它配合充分下降的 IRLS 子问题有全局收敛语义；但在 SLAM 非线性残差中，凸损失并不会消除整体非凸性。它的 \(\psi\) 在外点区恒定（不 redescending），大外点仍有持续拉力——所以它"稳"但不"狠"。 - Tukey 与 Welsch 的权重随 \(r\) 指数级衰减，外点几乎被完全压制，但非凸 → 必须配好初值或 GNC。 - GM 和 DCS 是**同一家族的不同参数化**（§F.15 证明）。 - L1 不可微，SLAM 中罕用；L2-with-deadzone 对传感器零点漂移有特殊用途。

§F.5 影响函数与 redescending 的代价 ⭐⭐⭐¶

\(\psi(r)=\rho'(r)\) 称为**影响函数**，直观意义是"单个残差为 \(r\) 的观测对估计的瞬时拉力"。

Redescending 的定义：\(\psi(r)\to 0\) as \(\lvert r\rvert\to\infty\)。更强的版本要求 \(\psi\) 具有紧支集（Tukey：\(\lvert r\rvert>c\) 时 \(\psi\equiv 0\)）。

Huber 不 redescending：\(\psi=k\,\mathrm{sign}(r)\) 恒为 \(\pm k\)，一个外点残差无论多大都持续贡献 \(k\) 的拉力。好处是**凸性**。
Cauchy 是边界情形：标准 redescending 判据看 \(\psi(r)\to 0\)，因此 Cauchy 常被归入 soft redescending；但它的 \(\rho\to\infty\)，不像 Tukey/Welsch/GM 那样让总代价饱和。若工程上强调"外点总代价封顶"，应单独称为**有界损失**，不要把它和 redescending 混成一个判据。
Tukey/Welsch/GM 严格 redescending：\(\rho\) 有界 → 外点对总代价贡献封顶。

代价：redescending ⇒ \(\rho\) 非凸 ⇒ 存在多个局部极小。具体表现为

\[\text{rejection region}:\ \{r:\psi(r)\approx 0\}\]

一个观测一旦掉进此区，它的 Jacobian 对 Hessian 无贡献——如果**初值错到让正确内点也落入 rejection region**，优化器将永远看不到它，陷入伪局部极小。这正是 GNC（§F.10）要解决的核心问题：用凸起点保证全局视野，再退火到非凸终点保证强鲁棒。

跨领域类比：redescending M-estimator 中的 rejection region 类似于深度学习中的 "dead ReLU" 现象。当 ReLU 神经元的输入为负时，梯度为零，该神经元对学习无贡献——如果初始化不当导致大量神经元"死亡"，网络容量严重萎缩。同理，当鲁棒核的外点 rejection 使正确内点也被降权为零，优化器的"有效容量"（能看到的观测数）缩小，导致欠约束。两者的解决思路也类似：深度学习用 Leaky ReLU / ELU（不完全为零）或 careful initialization；鲁棒估计用 GNC（从凸起点退火）或 Huber warm-start（先不截断）。

设计决策的穷举分类——选择鲁棒核时，需要在四个维度上权衡：

维度	凸核（Huber/Fair）	非凸核（GM/Tukey/Welsch）	TLS（0/1 硬截断）
全局收敛性	单次 IRLS 即可收敛（凸）	依赖初值	最非凸——NP-hard
外点抑制力	弱（\(\psi\) 恒定，外点仍有拉力）	强（\(\psi\to 0\)，外点失声）	最强（外点权重=0）
内点效率	接近 L2（95% asymptotic efficiency at \(k=1.345\)）	接近 L2（合理 \(c\) 下）	等同 L2（内点区完全二次）
与 GNC 配合	作为起点	作为终点	GNC-TLS 的终极目标
与 iSAM2 兼容	完全兼容	兼容（权重逐帧更新）	不兼容（需 batch）

本质洞察：鲁棒代价函数的选择不是"哪个最好"的问题，而是"不同阶段用不同强度"的退火策略问题。 GNC 的哲学正是把这个认识形式化：从最温和的凸核（全局视野）平滑过渡到最激烈的非凸核（外点完全剔除）。

§F.5b GNC 的退火直觉：从模拟退火到参数空间连续化 ⭐⭐⭐¶

GNC（Graduated Non-Convexity）的核心思想可以从多个角度理解：

角度 1：模拟退火的确定性版本

模拟退火（Simulated Annealing）在高温下允许"上坡"跳出局部极小，逐步降温直到冻结在全局极小附近。GNC 类似但完全确定性：高 \(\mu\)（"高温"）使代价函数接近凸 → 优化器找到全局最优；逐步降低 \(\mu\)（"降温"）使代价函数逼近目标非凸核 → 解逐步过渡到非凸目标的高质量局部极小。

角度 2：代价函数的连续形变

GNC 构造了一族代价函数 \(\{\rho_\mu\}_{\mu\ge 1}\)，满足： - \(\rho_{\mu=\mu_0}\)：凸（如 L2 或 Huber） - \(\rho_{\mu=1}\)：目标非凸核（如 GM 或 TLS） - \(\rho_\mu\) 关于 \(\mu\) 连续变化

Yang-Carlone RA-L 2020 对 GM 给出的具体构造为：

\[\rho_\mu^{\text{GM}}(r) = \frac{\mu\bar{c}^2 r^2}{2(\mu\bar{c}^2 + r^2)}\]

当 \(\mu\to\infty\) 时，\(\rho_\mu\to r^2/2\)（L2）；当 \(\mu=1\) 时，\(\rho_1=\rho^{\text{GM}}\)（Geman-McClure）。

对 TLS：

\[\rho_\mu^{\text{TLS}}(r) = \begin{cases} r^2/2 & |r|\le\bar{c}\sqrt{\mu} \\ \mu\bar{c}^2/2 & |r|>\bar{c}\sqrt{\mu} \end{cases}\]

当 \(\mu\to\infty\) 时退化为 L2；当 \(\mu=1\) 时恢复 TLS。

角度 3：Black-Rangarajan 对偶中 \(\mu\) 的含义

在半二次对偶框架中，\(\rho(r)\) 被写成

\[\rho(r) = \min_w \left[w\cdot r^2 + \Phi_\rho(w)\right]\]

其中 \(\Phi_\rho(w)\) 是与 \(\rho\) 配对的惩罚函数。GNC 的 \(\mu\) 调节 \(\Phi\) 的"宽度"——\(\mu\) 大时 \(\Phi\) 很宽，使最优 \(w^\star\approx 1\)（所有观测同等对待）；\(\mu\) 小时 \(\Phi\) 变窄，使外点的 \(w^\star\to 0\)。

GNC 算法流程（Yang-Carlone RA-L 2020 Alg. 1）：

输入：因子图 F, 初始 μ₀ (大), 步长因子 α ∈ (1, 2], 终止 μ_final = 1
输出：优化解 x*, 权重 w*

μ ← μ₀
x ← solve(F, weights=1)      # 初始：全部等权 L2 求解
while μ > 1:
    # 1. 给定当前 x, 计算每个因子的最优权重
    for each factor i:
        r_i ← residual(x, factor_i)
        w_i ← optimal_weight(r_i, μ, ρ_type)
    # 2. 给定权重, 做加权最小二乘
    x ← solve(F, weights=w)
    # 3. 退火
    μ ← max(1, μ / α)
return x, w

收敛保证（Yang-Carlone 2020, Proposition 4）：对 GM 和 TLS，在每个 \(\mu\) 步内部做到（加权）GN 收敛后再退火，则外层循环单调减少总代价。最终权重 \(w_i\in\{0,1\}\)（TLS）或 \(w_i\in(0,1]\)（GM）给出内点/外点的软/硬分类。

为什么 TLS 在高外点率下优于 GM：GM 的权重 \(w_i=\bar{c}^4/(r_i^2+\bar{c}^2)^2\in(0,1]\)——大外点被强烈降权但永远不会变为 0。当外点率 >70% 时，这些小但非零的权重积累起来仍可能偏移估计。TLS 的权重 \(w_i\in\{0,1\}\)——外点被完全切断，不留任何残余拉力。经验上（Yang-Carlone 2020 实验），GNC-TLS 在 >80% 外点下仍能恢复正确拓扑，GNC-GM 在 >60% 时开始退化。

§F.6 IRLS：迭代重加权最小二乘 ⭐⭐¶

IRLS 把 M-estimation 转为一串加权 LS 子问题：

IRLS(residuals r_i(x), weight function w, initial x₀):
  x ← x₀
  for t = 0, 1, ..., maxIter:
      r_i ← r_i(x)                       # 评估当前残差
      w_i ← w(r_i)                       # 计算权重（固定）
      δ ← solve  (Σ w_i J_iᵀ J_i) δ = -Σ w_i J_iᵀ r_i
      x ← x ⊞ δ                          # 流形上更新
      if |ΔF| / F < tol: break
  return x

MM（Majorization-Minimization）视角：当 \(w(r)=\psi(r)/r\) 非增且确实构造出当前点处相切的二次上界时，\(\tfrac12 w(\bar r)\,r^2+\text{const}\) 在 \(\bar r\) 处**从上方逼近** \(\rho(r)\)。若每次 IRLS 都充分降低这个加权子问题，则原目标可单调下降（Holland & Welsch 1977）。在 SLAM 中子问题通常由 GN/LM 近似求解，因此工程实现还需要 line search 或 trust-region 检查实际下降量。

收敛性结论（Holland-Welsch 1977 Commun. Stat. 6(9):813–827）： - 凸 \(\rho\)（L2/L1/Huber/Fair/Charbonnier）：在线性回归或整体凸目标中，若加权子问题被充分求解，\(F(x_{t+1})\le F(x_t)\) 且聚点可达到全局最优；在 SLAM 这类**非线性残差**问题中，凸鲁棒损失并不会让整体目标变凸，IRLS/GN 仍只应期待驻点或局部极小。 - 非凸 \(\rho\)（Tukey/GM/Welsch/Cauchy）：需要更谨慎。若 MM 上界和下降检查都成立，可获得单调下降到局部解；若只是简单重加权再做几步 GN/LM，则没有无条件单调性。收敛点依赖初值；应从高 BP 初值（LMS/LTS/Huber 解）bootstrap。

§F.7 Triggs 修正：把 \(\sqrt{w}\) 内嵌到残差/Jacobian ⭐⭐⭐¶

Ceres 和 GTSAM 都**不**直接改正规方程。它们把鲁棒性**塞进残差与 Jacobian**，让优化器用标准 GN/LM 即可。做法来自 Triggs et al. 2000 Bundle Adjustment — A Modern Synthesis §4.3 Eq.(10)(11)。

设 \(s=\|r\|^2_W=r^\top W r\)（\(W=\Sigma^{-1}\)），鲁棒代价 \(\tfrac12\rho(s)\)。Triggs 推导出

\[g = \rho'\,J^\top W r,\qquad H \approx J^\top\Big(\rho' W + 2\rho''(Wr)(Wr)^\top\Big) J \quad(\text{Eq.10})\]

假设已预白化（\(W=I\)），则 GN 子问题等价于对**修正后的残差与 Jacobian** 做**无权最小二乘**：

\[\tilde r = \frac{\sqrt{\rho'}}{1-\alpha}\,r,\qquad \tilde J = \sqrt{\rho'}\Big(I-\alpha\,\frac{rr^\top}{\|r\|^2}\Big)J\]

\[\alpha = 1-\sqrt{1+2\|r\|^2\rho''/\rho'}\ <\ 1 \quad(\text{Triggs Eq.11})\]

"简单"版本（\(\alpha=0\)）**就是众所周知的 \(\tilde r=\sqrt{w}\,r\)，\(\tilde J=\sqrt{w}\,J\)——多数教科书到此为止。完整的 **Triggs 修正项（\(\alpha\ne 0\)）来源于 \(\sqrt{w}\) 对 \(r\) 求导的二阶展开，保证 GN 近似的**二阶精度**。

Ceres 的工程实现（internal/ceres/corrector.{h,cc}）： 1. LossFunction::Evaluate(s, out) 返回 \((\rho(s),\rho'(s),\rho''(s))\)； 2. Corrector 类据此计算 \(\alpha\)； 3. 关键 safeguard：源码 corrector.cc 强制 \(\rho''\le 0\)（设到 0），因为"使用 Triggs 修正当 \(\rho''>0\) 会损害性能"（见 corrector.cc 注释）。即**只在凹区（外点）用完整修正，内点区退化为标量重加权**。

直觉：\(\alpha\) 捕捉了"权重随残差滑动"这一高阶效应。忽略它（\(\alpha=0\)）相当于假设权重是常数，在凸 \(\rho\) 上工程上够用；但在强非凸 \(\rho\)（Tukey/GM）上 \(\alpha\) 不可忽略。Zach-Bourmaud ECCV 2018 指出 Triggs 的二次替代函数在展开点处是**凹的**，因此可能过冲；这是 IRLS（保持 \(\psi\) 对称性）与 Triggs（保持二阶精度）之间的张力。

§F.8 Barron 的统一鲁棒损失族 ⭐⭐⭐¶

Barron CVPR 2019 A General and Adaptive Robust Loss Function（arXiv:1701.03077）给出单参数族

\[\rho(r,\alpha,c) = \frac{\lvert\alpha-2\rvert}{\alpha}\left[\left(\frac{(r/c)^2}{\lvert\alpha-2\rvert}+1\right)^{\alpha/2}-1\right]\]

（\(\alpha\ne 0,2\)；\(\alpha=0,2,-\infty\) 需取极限）。完整定义（Barron Eq.8）：

\(\alpha\)	\(\rho(r,\alpha,c)\)	等价于
\(2\)	\(\tfrac12(r/c)^2\)	L2
\(1\)	\(\sqrt{(r/c)^2+1}-1\)	Charbonnier / Pseudo-Huber
\(0\)	\(\ln\!\big(\tfrac12(r/c)^2+1\big)\)	Cauchy / Lorentzian
\(-2\)	\(\dfrac{2(r/c)^2}{(r/c)^2+4}\)	Geman-McClure
\(-\infty\)	\(1-\exp\!\big(-\tfrac12(r/c)^2\big)\)	Welsch

关键性质（Barron §1）： - \(\rho\) 是 \((r,\alpha,c)\) 的 \(C^\infty\) 函数，\(\rho(0)=0\)，\(\rho\) 对 \(\alpha\) 单调非减 → GNC 可通过 \(\alpha\) 实现（从大 \(\alpha\) 凸起点退火到小 \(\alpha\) 非凸）。 - 尺度不变：\(\rho(kr,\alpha,kc)=\rho(r,\alpha,c)\)。 - \(\alpha<1\) 时影响函数 redescending。 - 最大意义：\(\alpha\) 可作为可学习参数，在训练中自适应选择鲁棒程度（深度学习中的鲁棒回归、NeRF 离群抗性都用它）。

§F.9 Black-Rangarajan 半二次对偶 ⭐⭐⭐⭐¶

Black & Rangarajan 1996 IJCV 19(1):57–91 提出的 unification 视角——一切 redescending M-estimator 等价于带 line process 的联合优化。

核心定理（IJCV Fig.10 与 §3）。设 \(\phi(z):=\rho(\sqrt z)\)（\(z=r^2\)），若 \(\phi\) 满足

\[\lim_{z\to 0}\phi'(z)=1,\quad \lim_{z\to\infty}\phi'(z)=0,\quad \phi''(z)<0\text{（在 }z\text{ 上凹）}\]

则

\[\rho(r) \equiv \min_{w\in[0,1]} \big[\, w\cdot r^2 + \Phi_\rho(w)\,\big]\]

这一节采用 Black-Rangarajan / GNC 文献的无 \(\tfrac12\) 归一化，便于和 Yang-Carlone 的 \(\bar c^2\)、\(\mu\) 更新公式对应。与 §F.4 的鲁棒统计表相比，它只差一个整体常数；权重函数和内外点判定不变，但实现阈值必须按库的代价约定换算。

其中 \(\Phi_\rho\) 由 \(\phi\) 的 Legendre-Fenchel 共轭给出：\(\Phi_\rho(w)=\phi(z^*(w))-w\,z^*(w)\)，\(z^*\) 满足 \(\phi'(z^*)=w\)。

意义：原本的非凸 \(\min_x\sum\rho(r_i(x))\) 变为**联合**优化

\[\min_{x,\,w_1,\ldots,w_N\in[0,1]}\ \sum_i\Big[w_i\,r_i^2(x)+\Phi_\rho(w_i)\Big]\]

对 \(x\) 固定 \(w\)：就是**加权最小二乘**（凸的 GN 子问题，可用 GTSAM/Ceres 的标准求解器）。对 \(w\) 固定 \(x\)：每个 \(w_i\) 有闭式解 \(w_i^*=\mathrm{argmin}[w r_i^2+\Phi_\rho(w)]\)。

\(w_i\) 的语义：被自然解释为**线过程（line process）或**外点权重——\(w_i\approx 1\) 内点、\(w_i\approx 0\) 外点。这正是 switchable constraints（§F.13）的理论源头。

具体 \(\Phi_\rho(w)\) 与 \(w^*(r)\)（Yang et al. RA-L 2020 Props.3–4；Black-Rangarajan 1996 Table 1）：

\(\rho\)	\(\Phi_\rho(w)\)	\(w^*(r)\)
Geman-McClure \(\frac{\bar c^2 r^2}{\bar c^2+r^2}\)	\(\bar c^2(\sqrt w-1)^2\)	\(\big(\bar c^2/(r^2+\bar c^2)\big)^2\)
Welsch \(\tfrac{\bar c^2}{2}(1-e^{-r^2/\bar c^2})\)	\(\tfrac{\bar c^2}{2}(w\ln w-w+1)\)	\(\exp(-r^2/\bar c^2)\)
Tukey \(\tfrac{\bar c^2}{6}[1-(1-(r/\bar c)^2)^3]_{+}\)	\(\tfrac{\bar c^2}{6}(2w^{3/2}-3w+1)\)	\((1-(r/\bar c)^2)_+^2\)
TLS \(\min(r^2,\bar c^2)\)	\(\bar c^2(1-w)\), \(w\in\{0,1\}\)	\(\mathbb 1\{r^2\le\bar c^2\}\)

与 IRLS 的关系：IRLS 是这个联合优化的"一个特定的 coordinate descent"——更新 \(x\)（加权 LS）、更新 \(w\)（\(w^*(r)\)）、交替。但 Black-Rangarajan 的框架允许把**连续参数 \(\mu\)** 插到 \(\Phi_\rho\) 里，得到一族 surrogate \(\rho_\mu\)——这就是 GNC。

§F.10 Graduated Non-Convexity (GNC) ⭐⭐⭐¶

GNC 起源：Blake & Zisserman 1987 Visual Reconstruction Ch.4–5，用于 weak-string/weak-membrane 的早期视觉。现代 SLAM 版本：

Yang, Antonante, Tzoumas, Carlone, RA-L 2020 5(2):1127–1134（arXiv:1909.08605）——ICRA 2020 Best Paper in Robot Vision。
Antonante, Tzoumas, Yang, Carlone, T-RO 2022 38(1):281–301（arXiv:2007.15109）——硬度分析与 minimally-tuned 算法。
Yang, Carlone, TPAMI 2023 45(3):2816–2834（arXiv:2109.03349）——SDP 松弛的可认证最优。

通用 GNC 算法（Yang et al. 2020 §IV）：

GNC(graph G, initial x₀, target ρ, μ schedule):
  x ← x₀
  μ ← initializeMu(x, ρ)                # 起点：凸替代
  loop:
      # (a) 变量更新：加权 LS 子问题，调 GN/LM
      x ← argmin_x Σ wᵢ · rᵢ²(x)
      # (b) 权重更新：闭式（由 Black-Rangarajan 对偶得到）
      for each i: wᵢ ← w*_ρ_μ(rᵢ(x))
      # (c) 退火
      μ ← updateMu(μ)                   # GM: μ/1.4; TLS: μ·1.4
      if convergence(μ, w, cost): break
  return x, {wᵢ}

GNC-GM（目标损失 Geman-McClure）： - 替代 \(\rho_\mu(r)=\dfrac{\mu\bar c^2\,r^2}{\mu\bar c^2+r^2}\)； - \(\mu\to\infty\) 时 \(\rho_\mu\to r^2\)（L2，凸）；\(\mu=1\) 时 \(\rho_\mu=\) GM； - 初始化 \(\mu=2r^2_{\max}/\bar c^2\)，更新 \(\mu\leftarrow\mu/1.4\)，终止 \(\mu\le 1\)； - 权重 \(w_i=\big(\mu\bar c^2/(r_i^2+\mu\bar c^2)\big)^2\)（Yang 2020 Eq.12）； - 权重始终 \(\in(0,1]\)——软加权。

GNC-TLS（目标损失 Truncated LS）： - 替代 \(\rho_\mu(r)\) 为 Yang 2020 Eq.6 的分段形式； - \(\mu\to 0^+\) 凸起点；\(\mu\to\infty\) TLS（\(w\in\{0,1\}\)）； - 初始化 \(\mu=\bar c^2/(2r^2_{\max}-\bar c^2)\)（若分母 \(\le 0\) 则直接 L2，跳过 GNC）； - 更新 \(\mu\leftarrow 1.4\mu\)； - 权重三段式（Yang 2020 Eq.14）：

\[w_i = \begin{cases} 1,& r_i^2\le \tfrac{\mu}{\mu+1}\bar c^2 \\[2pt] \dfrac{\bar c}{|r_i|}\sqrt{\mu(\mu+1)}-\mu,& \tfrac{\mu}{\mu+1}\bar c^2 < r_i^2 < \tfrac{\mu+1}{\mu}\bar c^2 \\[4pt] 0,& r_i^2\ge \tfrac{\mu+1}{\mu}\bar c^2 \end{cases}\]

GNC-GM vs GNC-TLS：

属性	GNC-GM	GNC-TLS
替代凸起点	\(\mu\) 大 → 接近 L2	\(\mu\) 小 → 接近 L2
目标损失	GM（软）	TLS（硬 0/1）
权重范围	\((0,1]\)（永不为零）	\([0,1]\)，终态 \(\{0,1\}\)
外点容忍（经验）	~70–80%	~80–90%
是否可认证最优	否	是（配 SDP；TPAMI 2023）
收敛判据	\(\\|\mu-1\\|<10^{-9}\) 或 cost	权重二值化 \(\\|w_i-\mathrm{round}(w_i)\\|<\epsilon\)

实际效果：在 PGO 数据集上（sphere2500、torus、cubicle）人工注入 50–80% 错误回环，GNC-TLS 几乎恢复 ground truth 拓扑；普通 Huber/DCS 会被拉偏。

§F.11 GTSAM GncOptimizer 的精确 API ⭐⭐¶

源码：gtsam/nonlinear/GncOptimizer.h（borglab/gtsam develop 分支，557 行，22.3KB）。作者 Jingnan Shi、Luca Carlone、Frank Dellaert。实现精确对应 Yang et al. RA-L 2020 的 Remark 5 调度。

类声明：

template<class GncParameters>
class GncOptimizer {
 public:
  using BaseOptimizer = typename GncParameters::OptimizerType;

  GncOptimizer(const NonlinearFactorGraph& graph,
               const Values& initialValues,
               const GncParameters& params = GncParameters());

  // 阈值设置（与 χ² gate 对齐）
  void setInlierCostThresholds(const double inth);
  void setInlierCostThresholds(const Vector& inthVec);
  void setInlierCostThresholdsAtProbability(const double alpha);  // 默认 0.99
  void setWeights(const Vector w);

  // 核心 API
  Values optimize();

  // 结果访问
  const Vector& getWeights() const;                // 每因子权重
  const Vector& getInlierCostThresholds() const;
};

参数类 GncParams<BaseOptimizerParams>：

enum class GncLossType { GM, TLS };
enum class GncScheduler { Linear, SuperLinear };

void setLossType(GncLossType type);      // 默认 TLS
void setMaxIterations(size_t maxIter);   // 默认 100
void setMuStep(double step);             // 默认 1.4
void setRelativeCostTol(double v);       // 默认 1e-5
void setWeightsTol(double v);            // TLS 二值化判据
void setVerbosityGNC(Verbosity v);       // SILENT|SUMMARY|MU|WEIGHTS|VALUES

std::vector<size_t> knownInliers;        // 强制内点（权重≡1）
std::vector<size_t> knownOutliers;       // 强制外点（权重≡0）
bool allowNonNoiseModelFactors = false;

\(\chi^2\) 阈值的代码实现（构造函数内）：

double alpha = 0.99;
setInlierCostThresholdsAtProbability(alpha);
// 内部：barcSq_[k] = 0.5 * Chi2inv(alpha, factor_dim)

系数 0.5 源自 GTSAM 的**约定：每个因子误差 \(f(x)=\tfrac12\|r\|_\Omega^2\)**。因此 3-DoF 因子 \(\alpha=0.99\) 时，白化残差平方门限是 \(\chi^2_{3,0.99}\approx 11.345\)，而 GTSAM factor error 门限为 \(\bar c^2\approx 0.5\times 11.345\approx 5.67\)。二者相差一半，只是代价函数是否带 \(\tfrac12\) 的约定差异。

\(\mu\) 更新（updateMu，源码逐字）：

case GM:   return std::max(1.0, mu / params_.muStep);    // 向 1 递减
case TLS:  return mu * muStep;                           // 线性调度，向 ∞ 递增

权重更新（calculateWeights）： - GM：调用 noiseModel::mEstimator::GemanMcClure::Weight(u2, mu*barcSq)； - TLS：严格按 Yang 2020 Eq.14 的三段式，Linear 调度与 SuperLinear 调度略有不同。

与鲁棒噪声模型的交互：若因子已带 noiseModel::Robust，构造函数**剥离**鲁棒层只保留底层 Gaussian——GNC 期望普通高斯因子。权重通过**缩放信息矩阵**注入：newInfo = w_i * information，等价于重加权 \(r_i^2\)。

最小可用代码：

#include <gtsam/nonlinear/GncOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtParams.h>

NonlinearFactorGraph graph;   // 所有因子用普通 noiseModel::Gaussian
Values initial;

LevenbergMarquardtParams lmParams;
GncParams<LevenbergMarquardtParams> gncParams(lmParams);
gncParams.setLossType(GncLossType::TLS);
gncParams.setMuStep(1.4);
gncParams.setRelativeCostTol(1e-5);
// 关键 SLAM 惯用：里程计全部标记为 known inliers
gncParams.knownInliers = odomFactorIndices;

GncOptimizer<GncParams<LevenbergMarquardtParams>> opt(graph, initial, gncParams);
Values result = opt.optimize();
Vector w = opt.getWeights();   // 在此挑出被 GNC 判为外点的回环

Kimera-RPGO 集成（include/KimeraRPGO/RobustSolver.h）：RobustSolverParams 的 use_gnc_ 开关决定走 PCM → GNC → LM 还是旧的 PCM → LM 管线。Kimera2（Abate et al. ISER 2023，arXiv:2401.06323）明确采用 GNC 替代单纯 PCM，对虚假回环更稳健。

§F.12 TLS 与 GM 的关系再析 ⭐⭐⭐¶

两者常被混淆：

TLS：\(\rho_{\text{TLS}}(r)=\min(r^2,\bar c^2)\)，硬截断，\(\rho\) 有界；权重 \(w\in\{0,1\}\)。
GM：\(\rho_{\text{GM}}(r)=\dfrac{\bar c^2 r^2}{r^2+\bar c^2}\)，软截断，\(\rho\) 有界（上界 \(\bar c^2\)）；权重 \(w\in(0,1]\)。

直观上两者都在 \(r\approx\bar c\) 处"转向"，但： - TLS 在 \(r=\bar c\) 处**不光滑**（\(\rho\) 有 kink），Barron 族无法覆盖； - GM 处处 \(C^\infty\)，属于 Barron \(\alpha=-2\)。

GNC 选择： - 想要**彻底剔除**外点、外点比例高（>70%）、且可接受 batch 计算 → GNC-TLS； - 想要**软降权**、内点有残余噪声、需可导优化 → GNC-GM 或直接 Huber/Cauchy。

Yang-Carlone TPAMI 2023 进一步证明：TLS 目标 + SE-Sync 的 SDP 松弛可**全局认证最优**，GM 无此结论。

§F.13 Switchable Constraints (SC) ⭐⭐⭐¶

Sünderhauf & Protzel IROS 2012 1879–1884 是 SLAM 社区接受鲁棒后端的里程碑。核心目标（IROS'12 Eq.1）：

\[\begin{aligned}X^\*,S^\* = \arg\min_{X,S}\ & \sum_i \|f(x_i,u_i)-x_{i+1}\|^2_{\Sigma_i} \quad\text{(里程计)} \\[-2pt] + & \sum_{ij}\|\Psi(s_{ij})\cdot(f(x_i,u_{ij})-x_j)\|^2_{\Lambda_{ij}} \quad\text{(可开关回环)} \\[-2pt] + & \sum_{ij}\|\gamma_{ij}-s_{ij}\|^2_{\Xi_{ij}} \quad\text{(开关先验)}\end{aligned}\]

\(s_{ij}\in\mathbb R\)：每个回环的隐标量（新加入的状态变量）；
\(\Psi:\mathbb R\to[0,1]\)：开关函数，推荐 linear clipping \(\Psi_{\text{lin}}(s)=\mathrm{clip}(s,0,1)\)（sigmoid 平台与梯度消失使优化变慢）；
\(\gamma_{ij}=1\)：默认倾向"接受"；\(\Xi_{ij}\) 控制"关掉需要多大代价"，RPE 在 \(\xi\in[0.3,1.5]\) 稳定。

概率等价：若 \(\Omega\) 是信息矩阵，\(\Psi(s)\) 乘残差等价于把有效信息缩放为 \(\hat\Omega=\Psi(s)^2\Omega\)；等价协方差为 \(\hat\Sigma=\Sigma/\Psi(s)^2\)。也就是说，开关越小，信息越少、协方差越大。DCS 正是沿着“动态放大外点协方差”的方向消除开关变量。

g2o/Vertigo 实现（Sünderhauf 开源库，openslam-org/vertigo）： - VertexSwitchLinear：开关变量节点； - EdgeSwitchPrior：一元先验（\(\gamma=1\)）； - EdgeSE2Switchable / EdgeSE3Switchable：实现 \(\Psi(s)\cdot r\) 的回环因子。

局限： 1. 每个候选回环**加一个状态变量**，Hessian 稠密度上升； 2. 对 \(\Xi_{ij}\) 敏感：若 \(\Xi_{ij}\) 按协方差理解，太小会把 \(s\) 强拉回 1、关不掉真外点；太大又会让好回环也容易被关掉； 3. ICRA'13 比较论文显示，单个硬外点偶尔无法被关闭——这是 DCS 论文要"消除 \(s_{ij}\)"的主要动机。

§F.14 Max-Mixture ⭐⭐⭐¶

Olson & Agarwal RSS 2012 / IJRR 2013 32(7):826–840。因子建模为**高斯混合**，用 max 替代 sum：

\[f_i(x) = \max_k\ w_k\cdot\mathcal N(r_i(x);0,\Sigma_k)\]

典型两分量： - 分量 1：内点高斯，\(\Sigma_\text{in}\) 为传感器协方差； - 分量 2：外点（"null hypothesis"），\(\Sigma_\text{null}\) 很大（近似 uniform）。

为何用 \(\max\) 而非 \(\sum\)（IJRR §3.2）： - \(\log\sum_k w_k\mathcal N_k\) 无闭式 → 破坏 \(\log\prod=\sum\log\) → 每个因子注入 log-sum-exp 项，无法堆叠到标准 NLLS 的 \(Ax=b\)； - \(\log\max_k=\max_k\log\) → argmax 选出的分量就是当前"活跃分量"，残差与 Jacobian 直接取自该分量，与标准高斯因子一致。决策边界上有不连续性，但 LM 实际中处理得很好。

与 null hypothesis 因子的等价性：两分量 MM 就是"数据驱动的 gating"——若回环残差小于阈值走内点分量，否则"关闭"走 null 分量。这种思路与 SC 在高层上一致，但不引入额外状态。

已知问题（max-mixture README）：若某地标的所有关联边都选中 null 分量，Hessian 可能局部断开——与 SC 的"好回环被关"是同一失败模式。

工程实践：MM 在教学上价值高（展示 GM-vs-sum 的关键区别），但近年不如 GNC/DCS 流行，因为 MM 的超参（\(\Sigma_\text{null}\) 和权重 \(w_k\)）难以系统化调。

§F.15 Dynamic Covariance Scaling (DCS) ⭐⭐⭐¶

Agarwal, Tipaldi, Spinello, Stachniss, Burgard ICRA 2013。核心思想：从 SC 中消掉开关变量，留下闭式的协方差动态放大。

从 SC 出发可以先看一个简化目标：对 \(s\) 求导并解 \(\partial/\partial s=0\)，

\[\frac{\partial}{\partial s}\big[s^2\chi^2_l+(1-s)^2\Phi\big]=0 \ \Rightarrow\ s = \frac{\Phi}{\chi^2_l+\Phi}\]

其中 \(\Phi=\Xi^{-1}\)（开关先验方差的倒数，当作可调 gating 尺度），\(\chi^2_l=r^\top\Sigma^{-1}r\)。Agarwal et al. 的 DCS 并不是把这个一阶条件原样当作最终权重，而是在消除开关变量后选择更激进的动态尺度函数，并加 \(s\le 1\) 截断保护内点：

\[\boxed{\ s^2 = \min\left(1,\ \left(\frac{2\Phi}{\Phi+\chi^2}\right)^2\right) \ }\]

等价于**把 \(\Sigma\) 动态放大为 \(\Sigma/s^2\)，外点协方差越放越大，权重越降越低。**无额外状态变量——每次迭代仅做标量计算。

与 Geman-McClure 的等价性（Agarwal'13 §IV-A）：推得权重 \(w(x)=1/(1+x^2)^2\)——这正是 GM 的权重。"在 \(\Phi=1\) 且不强制 \(s\le 1\) 条件下，DCS 与 GM 近乎相同"。准确而言，DCS 最终权重 \(s^2=(2\Phi/(\Phi+\chi^2))^2\) 在 \(\Phi=1\) 时为 GM 权重的 4 倍；"近乎相同"指定性行为（均为有界 redescending）而非数值完全一致。DCS 相对 GM 的两个工程性差异： 1. \(\Phi\) 可调（通常设为 \(\chi^2\) 分位点）； 2. \(\min(1,\cdot)\) 截断避免对内点的过度处理。

阈值设置：\(\Phi=\chi^2_{n,\alpha}\)（\(n\) 是因子 DoF，\(\alpha\) 显著性水平）。Agarwal 原文默认 \(\Phi=1\)（继承 SC 的 \(\Xi=1\) 建议）。

g2o 源码（RobustKernelDCS::robustify）：

const double& phi = _delta;
double scale = (2.0*phi) / (phi + e2);
if (scale >= 1.0) { rho[0]=e2; rho[1]=1.; rho[2]=0; }
else {
  rho[0] = scale*e2*scale;
  rho[1] = scale*scale;
  rho[2] = 0.;
}

这段按 g2o 常见实现返回鲁棒化后的误差值和一阶权重；若单独对数学表达式求导，可以得到更复杂的二阶项，但那不是该实现实际交给求解器的内容。

性能：Agarwal 2013 报告 DCS 相对 SC ~10× 加速，核心原因是不加变量 → Hessian 稀疏结构完全保留 → CHOLMOD 可以用原始 PGO 的 variable ordering。

§F.16 Pairwise Consistency Maximization (PCM) ⭐⭐⭐¶

Mangelson et al. ICRA 2018 2916–2923。与 M-estimator/GNC 正交：**在回环检测后、优化前**做**一次性**外点剔除。

核心度量（Eq.4）：对两个候选回环 \(z^{ab}_{ik}\) 和 \(z^{ab}_{jl}\)，用 robot-a 的里程计 \(\hat x^a_{ij}\)、两条回环、robot-b 的里程计 \(\hat x^b_{lk}\) 组成一个闭合环路：

\[C(z^{ab}_{ik},z^{ab}_{jl}) = \big\|\,(\ominus z^{ab}_{ik})\oplus\hat x^a_{ij}\oplus z^{ab}_{jl}\oplus\hat x^b_{lk}\,\big\|_{\Sigma}\]

真实回环闭合环路应 \(\to 0\)。Mahalanobis 范数 \(\chi^2\) 分布 → 阈值 \(\gamma=\sqrt{\chi^2_{n,\alpha}}\)。

PCM 管线： 1. 构造**一致性图**：节点=候选回环；边 \((u,v)\) iff \(C(z_u,z_v)\le\gamma\)； 2. 求**最大团**（Maximum Clique，NP-hard 但 PMC by Pattabiraman 等现代求解器在 SLAM 规模秒级）； 3. 只保留最大团中的回环，送入优化器。

Kimera-RPGO 的改进（MIT-SPARK/Kimera-RPGO）： - 增加**里程计一致性预过滤**：单个回环与里程计组成的闭环先过 \(\chi^2\)； - 增量 clique 维护：在线添加新回环时避免重建整图。

完整管线：PCM（离线剔除极端外点） → GNC（剩余残差下的软/硬加权） → LM（最终精化）。

扩展：Forsgren et al. IROS 2022 / IJRR 2024 的 Group-k Consistent Measurement Set Maximization，把两两一致性推广到 \(k\)-一致性（\(k\)-uniform hypergraph clique）——对单因子局部一致但全局不一致的"共谋"外点更鲁棒。

§F.17 Ceres LossFunction 精确 API ⭐⭐¶

头文件 include/ceres/loss_function.h，实现 internal/ceres/loss_function.cc。

关键约定：Ceres 的 \(\rho\) 作用在 \(s=\|f(x)\|^2=r^\top r\)——平方残差空间。\(r\) 是 CostFunction 的返回值；Ceres 不帮你白化，你需自己在 CostFunction 中乘 \(\Sigma^{-1/2}\)。因此"squared residual"在 Ceres 里和"Mahalanobis squared"是否一致，取决于你白化没白化。

类	\(\rho(s)\)	参数
`TrivialLoss`	\(s\)	—
`HuberLoss(a)`	\(s\text{ if }s\le a^2;\ 2a\sqrt s-a^2\text{ else}\)	\(a\) 是\(\lvert r\rvert\) 的阈值（内部存 \(b=a^2\)）
`SoftLOneLoss(a)`	\(2a^2(\sqrt{1+s/a^2}-1)\)	平滑 L1（\(\approx\) Pseudo-Huber）
`CauchyLoss(a)`	\(a^2\ln(1+s/a^2)\)	—
`ArctanLoss(a)`	\(a^2\arctan(s/a^2)\)	—
`TukeyLoss(a)`	\((a^2/6)[1-(1-s/a^2)^3]\) if \(s\le a^2\), else \(a^2/6\)	redescending
`TolerantLoss(a,b)`	\(b\ln(1+e^{(s-a)/b})-b\ln(1+e^{-a/b})\)	deadzone + 饱和
`ComposedLoss(f,g)`	\(f(g(s))\)	链式
`ScaledLoss(ρ,a,own)`	\(a\cdot\rho(s)\)	整体缩放
`LossFunctionWrapper`	运行时可替换	可 `Reset(new HuberLoss(...))`

使用：

ceres::Problem problem;
problem.AddResidualBlock(cost_function,
                         new ceres::HuberLoss(1.0),  // a=1.0
                         x);

**Corrector 的二阶修正**在 internal/ceres/corrector.cc，见 §F.7。

§F.18 GTSAM noiseModel::mEstimator 精确 API ⭐⭐¶

头文件 gtsam/linear/LossFunctions.h（旧版在 NoiseModel.h）。

关键约定：GTSAM 的 \(\rho\) 作用在**白化残差的标量** \(x=\lvert\tilde r\rvert=\lvert\Sigma^{-1/2}r\rvert\)（或 error.norm()）——\(\sigma\) 单位的 Mahalanobis 距离。因此阈值 \(k=1.345\) 直接读作"1.345 个 \(\sigma\)"，这是 Zhang-IVC 的标准 95% 效率常数。

类	`loss(x)` （\(x\) 为白化标量）	默认参数
`Null`	0	—
`Fair(c)`	\(c^2(\lvert x\rvert/c-\ln(1+\lvert x\rvert/c))\)	\(c=1.3998\)
`Huber(k)`	\(0.5x^2\) if \(\lvert x\rvert<k\); \(0.5k^2+k(\lvert x\rvert-k)\) else	\(k=1.345\)
`Cauchy(k)`	\(\tfrac12 k^2\ln(1+x^2/k^2)\)	\(k=0.1\)
`Tukey(c)`	\(c^2/6\cdot[1-(1-x^2/c^2)^3]\) if \(\lvert x\rvert<c\); \(c^2/6\) else	\(c=4.6851\)
`Welsch(c)`	\(\tfrac12 c^2(1-\exp(-x^2/c^2))\)	\(c=2.9846\)
`GemanMcClure(c)`	\(0.5x^2\cdot c^2/(c^2+x^2)\)	\(c=1.0\)
`DCS(c)`	Agarwal'13，\(c\) 作 \(\Phi\)	\(c=1.0\)
`L2WithDeadZone(k)`	0 if \(\lvert x\rvert<k\), else \(0.5(\lvert x\rvert-k)^2\)	\(k=1.0\)

使用：

auto base = noiseModel::Isotropic::Sigma(3, 0.1);           // σ = 0.1 m
auto robust = noiseModel::Robust::Create(
    noiseModel::mEstimator::Huber::Create(1.345),            // 1.345 σ
    base);
graph.emplace_shared<BetweenFactor<Pose3>>(i, j, meas, robust);

reweight 机制：Base::reweight(Vector& error) 对白化误差调 sqrtWeight(error.norm())，并乘回 error——内部即 §F.7 的 \(\sqrt w\) 版本。

与 GncOptimizer 的关系：GNC 会**剥离** noiseModel::Robust 包裹，只用底层 Gaussian；因此 GNC 的用户**不应**给因子加 robust——而是让 GNC 自己做 TLS/GM 退火。

§F.19 g2o RobustKernel 精确 API ⭐⭐¶

基类 g2o/core/robust_kernel.h，实现 g2o/core/robust_kernel_impl.{h,cpp}。

class RobustKernel {
 public:
  virtual void robustify(double squaredError, Vector3& rho) const = 0;
  // rho[0]=ρ(e), rho[1]=ρ'(e), rho[2]=ρ''(e)
  virtual void setDelta(double delta);
  double delta() const { return _delta; }
};

关键约定：robustify 的入参是 Mahalanobis 平方 \(e=r^\top\Omega r\)（\(\Omega=\Sigma^{-1}\)）。setDelta 的 \(\delta\) 内部平方后比较 \(e\le\delta^2\)——所以 \(\delta\) 在 \(\sigma\) 单位上，与 GTSAM 的 \(k\) 一致（但不同于 Ceres 的 \(a\)，见 §F.20）。

具体类：

类	语义
`RobustKernelHuber`	\(\rho=e\) if \(e\le\delta^2\); else \(2\delta\sqrt e-\delta^2\)
`RobustKernelPseudoHuber`	\(2\delta^2(\sqrt{e/\delta^2+1}-1)\)
`RobustKernelCauchy`	\(\delta^2\ln(1+e/\delta^2)\)
`RobustKernelGemanMcClure`	\(e/(1+e)\)
`RobustKernelWelsch`	Welsch/Leclerc
`RobustKernelFair`	Fair
`RobustKernelTukey`	Tukey biweight
`RobustKernelSaturated`	\(e\le\delta^2\): \(\rho=e,\rho'=1\); else 硬截断 \(\rho=\delta^2,\rho'=0\)
`RobustKernelDCS`	§F.15 公式

使用：

auto* rk = new g2o::RobustKernelHuber();
rk->setDelta(1.0);
edge->setRobustKernel(rk);

kernel 通过 G2O_REGISTER_ROBUST_KERNEL(Huber, RobustKernelHuber) 注册，可由 RobustKernelFactory 按字符串名创建。

§F.20 鲁棒代价函数全景对比大表 ⭐⭐¶

函数	\(\rho(r)\) 简记	\(w(r)\)	凸	Redescending	BP (loc/reg)	GTSAM 类	Ceres 类	g2o 类	主场
L2	\(r^2/2\)	1	✓	×	0/\(\tfrac1N\)	`Null`	`TrivialLoss`/`nullptr`	无	无外点基准
L1	\(\lvert r\rvert\)	\(1/\lvert r\rvert\)	✓	×	1/2 /\(\tfrac1N\)	—	—	—	位置中位数
Huber	分段	\(\min(1,k/\lvert r\rvert)\)	✓	×	1/2 /\(\tfrac{1}{p+1}\)	`Huber`	`HuberLoss`	`RobustKernelHuber`	默认兜底
Fair	类 log	\(\frac1{1+\lvert r\rvert/c}\)	✓	×	—	`Fair`	—	`RobustKernelFair`	软 L1
Pseudo-Huber	\(c^2(\sqrt{1+(r/c)^2}-1)\)	\(\frac1{\sqrt{1+(r/c)^2}}\)	✓	×	—	—	`SoftLOneLoss`	`RobustKernelPseudoHuber`	Huber 光滑版
Cauchy	\(\tfrac{c^2}{2}\ln(1+(r/c)^2)\)	\(\frac1{1+(r/c)^2}\)	×	软	~1/2 /\(\tfrac{1}{p+1}\)	`Cauchy`	`CauchyLoss`	`RobustKernelCauchy`	深度估计、VO
GM	\(\tfrac{\bar c^2 r^2}{\bar c^2+r^2}\)	\(\big(\frac{\bar c^2}{r^2+\bar c^2}\big)^2\)	×	✓	~1/2 /\(\tfrac{1}{p+1}\)	`GemanMcClure`	—	`RobustKernelGemanMcClure`	GNC-GM 目标
Tukey	分段 3 次	\((1-(r/c)^2)_+^2\)	×	✓硬	~1/2 /\(\tfrac{1}{p+1}\)	`Tukey`	`TukeyLoss`	`RobustKernelTukey`	照片级 BA
Welsch	\(\tfrac{c^2}{2}(1-e^{-(r/c)^2})\)	\(e^{-(r/c)^2}\)	×	✓软	—	`Welsch`	—	`RobustKernelWelsch`	深度学习
TLS	\(\min(r^2,\bar c^2)\)	\(\{0,1\}\)	×	✓硬	up to 1/2 (via MM/S)	—	—	`RobustKernelSaturated`（近似）	GNC-TLS 目标
DCS	§F.15	§F.15	×	✓	—	`DCS`	—	`RobustKernelDCS`	PGO 主力
Barron	\(\rho(r,\alpha,c)\)	族	↕ (依 \(\alpha\))	↕	—	手写	手写	手写	可学习

其中 BP 给的是"位置 / 回归（固定 \(p\)）"两列；redescending 类单独作为 M-estimator 在回归下的 BP 不比 Huber 好，实际高 BP 来自 MM/S-estimator 组合（Yohai 1987）。

§F.21 SLAM 专用鲁棒方法对比 ⭐⭐¶

方法	辅助变量	凸	初值需求	外点容忍	典型耗时	代表论文	代表实现
M-estimator (Huber)	无	✓	低	<30%	1×	Huber 1964	GTSAM/Ceres/g2o
M-estimator (GM/Tukey)	无	×	高	50%	1×	Zhang 1997	GTSAM/Ceres/g2o
Switchable Constraints	\(s_i\)/回环（+1 维/因子）	×	中	50–70%	3–5×	Sünderhauf IROS'12	Vertigo/g2o
Max-Mixture	无（但多分量）	×	中	50–70%	2×	Olson-Agarwal RSS'12	max-mixture repo
DCS	无	×	中	50–70%	1× (~SC/10)	Agarwal ICRA'13	g2o/GTSAM
PCM	无（预处理）	—	低（不涉及优化）	80%+	max-clique 代价	Mangelson ICRA'18	Kimera-RPGO
GNC-GM	无（内部 \(w_i\)）	起点凸	低	70–80%	5–10×（外 loop）	Yang RA-L'20	GTSAM `GncOptimizer`
GNC-TLS	无（内部 \(w_i\)）	起点凸	低	80–90%	5–10×	Yang RA-L'20	GTSAM `GncOptimizer`
Cert.-Opt (SDP+TLS)	SDP	—	无	高，但依赖噪声界、内点可分性与松弛紧性	SDP 代价	Yang-Carlone TPAMI'23	CertifiablyRobustPerception

选型建议： - 在线 VIO / LIO 因子图：DCS 或 Huber（iSAM2 兼容）； - 离线 PGO / 多机器人合并：PCM → GNC-TLS → LM； - 回环比例不确定、要求高鲁棒：GNC-TLS（MIT-SPARK 默认）； - 研究/基准：Barron \(\alpha\) 可学习 + Cert.-Opt 做 ground-truth。

§F.22 核心教材与论文清单 ⭐¶

基础教材： - P.J. Huber & E.M. Ronchetti, Robust Statistics, 2nd ed., Wiley 2009（Ch.1 §1.4 breakdown；Ch.4 M-estimator）。 - F. Hampel, E. Ronchetti, P. Rousseeuw, W. Stahel, Robust Statistics: The Approach Based on Influence Functions, Wiley 1986（§1.2, §2.3）。 - R. Maronna, D. Martin, V. Yohai, Robust Statistics: Theory and Methods, Wiley 2006（MM-estimator 完整讨论）。

教程/综述： - Z. Zhang, Parameter Estimation Techniques: A Tutorial with Application to Conic Fitting, IVC 15(1):59–76, 1997（表格最全）。 - K. MacTavish & T. Barfoot, At all Costs: A Comparison of Robust Cost Functions for Camera Correspondence Outliers, CRV 2015。

M-estimator 与 BA： - P. Holland & R. Welsch, Robust Regression Using IRLS, Commun. Stat. 6(9):813–827, 1977。 - B. Triggs et al., Bundle Adjustment — A Modern Synthesis, LNCS 1883:298–375, 2000（§3.3, §4.3 Eq.10–11 Triggs correction）。 - S. Agarwal, N. Snavely, S. Seitz, Bundle Adjustment in the Large, ECCV 2010。 - J.T. Barron, A General and Adaptive Robust Loss Function, CVPR 2019（arXiv:1701.03077）。

Black-Rangarajan 与 GNC： - M. Black & A. Rangarajan, On the Unification of Line Processes, Outlier Rejection, and Robust Statistics, IJCV 19(1):57–91, 1996。 - A. Blake & A. Zisserman, Visual Reconstruction, MIT Press 1987。 - H. Yang, P. Antonante, V. Tzoumas, L. Carlone, Graduated Non-Convexity for Robust Spatial Perception, RA-L 2020 5(2):1127–1134（arXiv:1909.08605）。 - P. Antonante, V. Tzoumas, H. Yang, L. Carlone, Outlier-Robust Estimation: Hardness, Minimally-Tuned Algorithms, and Applications, T-RO 2022 38(1):281–301（arXiv:2007.15109）。 - H. Yang & L. Carlone, Certifiably Optimal Outlier-Robust Geometric Perception, TPAMI 2023 45(3):2816–2834。

SLAM 专用： - N. Sünderhauf & P. Protzel, Switchable Constraints for Robust Pose Graph SLAM, IROS 2012。 - N. Sünderhauf, Robust Optimization for SLAM, PhD Thesis, Chemnitz U., 2012。 - E. Olson & P. Agarwal, Inference on Networks of Mixtures for Robust Robot Mapping, IJRR 32(7):826–840, 2013。 - P. Agarwal, G.D. Tipaldi, L. Spinello, C. Stachniss, W. Burgard, Robust Map Optimization using Dynamic Covariance Scaling, ICRA 2013。 - J. Mangelson, D. Dominic, R. Eustice, R. Vasudevan, Pairwise Consistent Measurement Set Maximization for Robust Multi-Robot Map Merging, ICRA 2018。 - M. Abate, Y. Chang, N. Hughes, L. Carlone, Kimera2: Robust and Accurate Metric-Semantic SLAM, ISER 2023（arXiv:2401.06323）。

§F.23 C++/Python 库映射 ⭐⭐¶

功能	GTSAM（C++/Python）	Ceres（C++）	g2o（C++）
头文件	`gtsam/linear/LossFunctions.h`, `gtsam/nonlinear/GncOptimizer.h`	`ceres/loss_function.h`, `internal/ceres/corrector.{h,cc}`	`g2o/core/robust_kernel_impl.h`
命名空间	`gtsam::noiseModel::mEstimator::*`, `gtsam::Robust`	`ceres::`	`g2o::`
Huber 构造	`noiseModel::mEstimator::Huber::Create(1.345)`	`new ceres::HuberLoss(1.0)`	`new g2o::RobustKernelHuber; rk->setDelta(1.0)`
阈值/尺度约定	白化残差范数（\(\sigma\) 单位）	Loss 接收 \(s=\\|r\\|^2\)，但参数 \(a\) 是残差范数尺度，转折在 \(s=a^2\)	Mahalanobis 平方 \(r^\top\Omega r\)，`delta` 通常对应白化范数阈值
组装鲁棒因子	`noiseModel::Robust::Create(m, gaussian)`	`AddResidualBlock(cf, loss, x)`	`edge->setRobustKernel(rk)`
GNC	`GncOptimizer<LevenbergMarquardtParams>`	需手写外 loop	需手写外 loop
DCS	`noiseModel::mEstimator::DCS::Create(1.0)`	无官方，可手写 `LossFunction`	`new g2o::RobustKernelDCS`
Switchable	无原生，手写 factor	无	Vertigo 库 `VertexSwitchLinear`+`EdgeSE3Switchable`
Max-Mixture	手写 `NoiseModelFactor` with argmax	手写	Agarwal `max-mixture` repo
PCM	Kimera-RPGO `OutlierRemoval` 类	无	无
Python 绑定	`gtsam.noiseModel.mEstimator.Huber.Create(k)`, `gtsam.GncLMOptimizer`	`pyceres`	无官方（`pypangolin` 仅可视化）

§F.24 常见陷阱（≥10 条） ⭐⭐¶

阈值单位混淆。同一个 \(\delta=1.0\)：Ceres 是 \(\lvert r\rvert=1\) 触发 Huber 转折（假设你已白化），GTSAM 是 \(\lvert\tilde r\rvert=1\sigma\)，g2o 是 \(r^\top\Omega r=\chi^2=1\)（即 \(\lvert\tilde r\rvert=1\sigma\)）。GTSAM 与 g2o 用同一 \(\sigma\) 单位，Ceres 看你是否白化。跨库搬家必重算阈值。
忘记白化 → Ceres 的 Huber 阈值无物理意义。Ceres CostFunction 返回的 \(r\) 必须是 \(\Sigma^{-1/2}\) 乘过的；否则 \(\rho(s=\|r\|^2)\) 的阈值 \(a^2\) 与传感器单位混在一起，调参只能靠试。
非凸 \(\rho\) + 坏初值 → 陷入局部极小。Tukey/GM/Welsch 单独用对 PGO 初值敏感。必须 bootstrap：先 Huber 暖起动、或先跑 GN 几轮再切 redescending、或直接用 GNC。
GNC 与 iSAM2 不兼容。GNC 是 batch outer loop（每次全图加权 LS），iSAM2 的 fluid relinearization 是增量——两者不能混。在线系统应用 DCS/Huber，周期性触发 GNC 做离线修正。
DCS 阈值 \(\Phi\) 设错。\(\Phi\) 过大 → 降权变弱，外点仍可能拉坏图；\(\Phi\) 过小 → 降权过激，内点也容易被压低。建议 \(\Phi=\chi^2_{n,\alpha}\)（\(n\) 因子 DoF，\(\alpha\in[0.95,0.99]\)）。
SC 的 \(\Xi\) 调太大 → 好回环也可能被关。若把 \(\Xi\) 作为先验协方差，太小会让 \(s\) 很难偏离 1，外点关不掉；太大则先验过弱，内点回环也可能被优化器牺牲。推荐 \(\Xi\in[0.3,1.5]\)（Sünderhauf'12 实验），并结合回环质量调参。
Max-Mixture 的 null 方差过小 → 外点抑制失败。\(\Sigma_\text{null}\) 需显著大于 \(\Sigma_\text{in}\)（经验 \(100\times\)），否则 argmax 永远选 inlier 分量。
Tukey/Welsch 的极小权重使 Jacobian 贡献近零。Tukey 在截断区可给出 \(w=0\)，Welsch 通常只是趋近 0；此时该因子在本次线性子问题中几乎无贡献。如果实现层直接 pruning 掉 0 权重因子，信息才会永久丢失。GNC 通过**退火**缓解这个问题（起始权重都 >0，逐步收紧）。
鲁棒因子与 marginalization 的交互。MHE/滑窗 VIO 中对滑窗外变量边缘化时，若边缘化时刻某因子权重临时低，其对先验的贡献被错误压低。建议：边缘化前固定一次权重；或先判定内点再边缘化（不要把"动态权重"编码进固定先验）。
Ceres Corrector 的 \(\rho''>0\) 会被截断。内点区的 Triggs 修正在 Ceres 里被强制退化为标量重加权（ρ'' ≤ 0 clamp）。对某些非标准损失（你自定义的 \(\rho\) 内点区二阶导正）导致与理论期望不一致——查 corrector.cc。
GNC 将因子的 noiseModel::Robust 包裹剥离。同时给 GNC 和 Huber 包裹 → GncOptimizer 构造函数会 strip robust，只保留 Gaussian；你期望的"GNC + Huber"实际变成"GNC + L2"。要么 GNC 要么 Robust，不要双层。
\(\chi^2\) gate 阈值在 GTSAM 中带 0.5 因子。GTSAM 的 NonlinearFactor::error 返回 \(\tfrac12\|r\|^2_\Omega\)，所以 barcSq = 0.5·Chi2inv(α, dof)——和 Yang'20 论文的 \(\bar c^2=\chi^2_{n,\alpha}\) 差一个因子 2。跨软件对比阈值时要乘除 2。
PCM 的 clique 规模 vs 错误回环规模。PCM 基于"最大团 = 最可信集合"假设；当错误回环彼此相互一致（对称环境里很常见），它们也能组成大 clique 甚至压过真实 clique。此时需要 \(k\)-一致性或配 GNC 兜底。
Barron \(\alpha\) 作为可学习参数时 \(c\) 要同步优化。\(\rho(r,\alpha,c)\) 的尺度性质要求 \(c\) 也要更新，否则 \(\alpha\) 退火时影响区误差放大。

§F.25 RANSAC：随机一致性为什么仍然是鲁棒估计的入口 ⭐⭐⭐¶

RANSAC（Random Sample Consensus, Fischler & Bolles 1981）比现代 M-estimator、GNC、SDP 都早。

它的核心思想很直接：

从观测中随机抽取一个最小样本集；
用最小样本解一个模型；
统计有多少观测与该模型一致；
重复多次，取一致集最大的模型；
用该一致集重新做一次最小二乘精化。

它不是连续优化方法。

它是数据关联和几何模型估计中的**组合搜索**。

成功概率推导¶

设观测总数为 \(N_{\text{obs}}\)。

内点比例为 \(w\)。

最小样本数为 \(s\)。

一次随机抽样全是内点的概率是

\[ p_{\text{good}}=w^s. \]

一次抽样失败的概率是

\[ 1-w^s. \]

独立抽 \(K\) 次都失败的概率是

\[ (1-w^s)^K. \]

因此至少成功一次的概率是

\[ P_{\text{success}}=1-(1-w^s)^K. \]

给定目标成功率 \(P\)，所需迭代次数为

\[ \boxed{ K = \frac{\log(1-P)}{\log(1-w^s)}. } \]

这个公式解释了 RANSAC 的全部工程边界。

若 \(w=0.5\)、\(s=3\)、\(P=0.99\)：

\[ K\approx \frac{\log(0.01)}{\log(1-0.125)} \approx 35. \]

若 \(w=0.1\)、\(s=3\)：

\[ K\approx 4603. \]

若 \(w=0.01\)、\(s=3\)：

\[ K\approx 4.6\times 10^6. \]

内点率一旦降到 1%，盲随机采样几乎不可用。

这就是 TEASER、PCM、GNC 出现的原因。

它们不是简单替代 RANSAC，而是在 RANSAC 失败的高外点区间提供结构化搜索或连续退火。

最小样本数决定复杂度¶

问题	最小样本数 \(s\)	典型求解器
2D 直线拟合	2	两点定线
Homography	4	DLT
Fundamental matrix	7/8	7-point / 8-point
Essential matrix	5	Nister 5-point
PnP	3/4	P3P / EPnP
3D 刚体配准	3	SVD/Kabsch

\(s\) 越大，\(w^s\) 越小。

因此高维几何问题在低内点率下更容易让 RANSAC 爆炸。

例如内点率 \(w=0.2\) 时，\(s=5\) 的成功样本概率只有 \(0.00032\)。

要达到 \(99\%\) 成功率，需要约 \(14389\) 次采样。

一致性阈值必须在白化空间中定义¶

RANSAC 判断一个观测是否属于内点，常用条件

\[ \|r_i(\hat x)\|\le \tau. \]

若残差未白化，\(\tau\) 没有统一意义。

正确形式应为

\[ r_i^\top\Sigma_i^{-1}r_i \le \chi^2_{d_i,\alpha}. \]

其中 \(d_i\) 是残差维度。

这与 GNC-TLS 的截断阈值、PCM 的一致性阈值、DCS 的 \(\Phi\) 选择是一条线。

鲁棒估计中最稳定的调参方式是：

建立每类量测的噪声协方差；
白化残差；
用 \(\chi^2\) 分位点设置 gate；
再根据经验略放宽或收紧。

RANSAC 与 M-estimator 的关系¶

RANSAC 先离散选择内点集合，再做最小二乘。

TLS/GNC 把这个过程写成优化：

\[ \min_x\sum_i\min(\|r_i(x)\|^2,\bar c^2). \]

若最终 TLS 权重为

\[ w_i\in\{0,1\}, \]

则它也选择了一个内点集合。

区别在于：

方法	内点选择方式	优点	风险
RANSAC	随机采样 + consensus	可跳出局部极小	高外点率迭代爆炸
M-estimator	连续降权	易接入 GN/LM	坏初值会陷局部
GNC-TLS	退火到 0/1 权重	高外点率鲁棒	batch 成本高
PCM	一致性图最大团	利用成对结构	对共谋外点敏感

RANSAC 适合前端快速筛选。

M-estimator 适合在线后端持续降权。

GNC/PCM 适合回环或多机器人合并这种高外点、低频触发场景。

工程边界¶

RANSAC 的失败模式主要有四类。

失败模式	说明	缓解
内点率太低	\(w^s\) 太小，迭代数爆炸	PROSAC、TEASER、PCM
阈值太紧	真内点被判外点	用 \(\chi^2\) gate，估计噪声
阈值太松	外点混入一致集	局部优化后重评估
退化样本	最小样本几何不可解	degeneracy check
多模型混杂	最大一致集不是目标模型	多模型 RANSAC 或图聚类

常见变体¶

变体	核心改动	用途
LO-RANSAC	找到模型后局部优化一致集	提高精度
PROSAC	按匹配置信度优先采样	特征匹配有排序时
MLESAC	用似然代替内点数	阈值更平滑
USAC	集成采样、验证、局部优化	工业级几何验证
Graph-Cut RANSAC	加空间一致性	图像几何

练习¶

计算 \(w=0.2,s=5,P=0.999\) 时 RANSAC 需要多少次迭代。
对 2D 重投影残差，用 \(\chi^2_{2,0.99}\) 设置 RANSAC 阈值，并把它转换成像素阈值（给定 \(\sigma=0.8\) px）。
比较 RANSAC 内点集合与 GNC-TLS 最终 \(w_i=1\) 的集合，分析不一致的样本。

§F.26 阈值、白化与 \(\chi^2\)：鲁棒后端的统一刻度 ⭐⭐⭐¶

鲁棒估计的公式很多，但阈值只有一个正确坐标系：白化残差空间。

设因子残差为

\[ r_i(x)\in\mathbb R^{d_i},\qquad \epsilon_i\sim\mathcal N(0,\Sigma_i). \]

白化残差为

\[ e_i=\Sigma_i^{-1/2}r_i. \]

若模型正确，则

\[ s_i=e_i^\top e_i = r_i^\top\Sigma_i^{-1}r_i \sim \chi^2_{d_i}. \]

这条关系把 RANSAC gate、Huber 阈值、DCS \(\Phi\)、TLS 截断常数、PCM 一致性阈值串成一个系统。

多维残差的阈值¶

对多维残差，不能直接用 \(\|e_i\|\le k\) 与标量 Huber 常数混用。

更自然的是平方马氏距离：

\[ s_i\le \chi^2_{d_i,\alpha}. \]

常用数值：

自由度 \(d\)	\(\chi^2_{d,0.95}\)	\(\chi^2_{d,0.99}\)
1	3.841	6.635
2	5.991	9.210
3	7.815	11.345
6	12.592	16.812

若使用残差范数阈值，则

\[ \|e_i\|\le \sqrt{\chi^2_{d_i,\alpha}}. \]

Huber 的标量常数与多维因子¶

Huber \(k=1.345\) 来自一维高斯 95% 渐近效率。

对多维因子，有两种实现风格：

对每个残差分量单独 Huber；
对整块残差范数 \(\|e_i\|\) 做 Huber。

GTSAM 的 mEstimator 更接近第二种：对白化误差向量的 norm 计算权重。

Ceres 的 LossFunction 作用在

\[ s=\|e_i\|^2. \]

g2o 的 RobustKernel 也接收 Mahalanobis 平方误差。

因此跨库移植时要确认：

库	输入给 loss/kernel 的量	阈值理解
GTSAM	白化残差 norm 或标量	\(k\) 个 sigma
Ceres	\(s=\\|r\\|^2\)，是否白化取决于 CostFunction	`a` 是 norm 阈值
g2o	\(e=r^\top\Omega r\)	`delta` 是 norm 阈值

TLS/GNC 阈值¶

TLS 写作

\[ \rho_{\text{TLS}}(e) = \min(\|e\|^2,\bar c^2). \]

若希望保留 \(\alpha\) 置信内点，应取

\[ \bar c^2=\chi^2_{d,\alpha}. \]

GTSAM 的 GncOptimizer 因为 NonlinearFactor::error() 返回

\[ \frac12\|e\|^2, \]

所以内部阈值常出现

\[ \bar c^2_{\text{GTSAM}}=\frac12\chi^2_{d,\alpha}. \]

这不是论文公式错，而是库的 error 约定不同。

PCM 阈值¶

PCM 的 pairwise consistency 残差是一个闭环误差。

若闭环误差维度为 \(d\)，协方差为 \(\Sigma_C\)，则一致性条件应为

\[ C^\top\Sigma_C^{-1}C \le \chi^2_{d,\alpha}. \]

这里 \(\Sigma_C\) 不是单条回环的协方差，而是闭合环路上多条边协方差的传播。

若直接用固定阈值，长路径和短路径会不公平。

长路径里程计不确定性更大，应允许更大的闭环误差。

阈值选型流程¶

确定残差维度 d
  ↓
建立或估计协方差 Σ
  ↓
白化 residual: e = Σ^{-1/2}r
  ↓
选择置信度 α，例如 0.95/0.99
  ↓
设 gate: ||e||² <= χ²(d, α)
  ↓
根据任务代价微调 α

常见误区¶

误区	后果
用同一个像素阈值处理所有尺度图像	金字塔层级间不一致
对 IMU 15 维残差用一维 Huber 阈值	几乎所有 IMU 因子被过度降权
PCM 忽略路径协方差	长距离闭环被错误剔除
Ceres residual 未白化	loss 参数与物理单位混杂

练习¶

对 Pose3 between factor 的 6 维白化残差，计算 99% TLS 截断阈值 \(\bar c\)。
解释为什么 IMU 预积分 15 维因子的 \(\chi^2\) gate 比视觉 2 维因子大得多。
在 Ceres 中实现一个白化 wrapper，使 HuberLoss(1.0) 真正表示 1 sigma。

§F.27 GNC 的退火路径：从凸起点到硬外点剔除 ⭐⭐⭐⭐¶

GNC 的目标不是简单地多跑几次鲁棒核。

它的目标是构造一条从易解目标到目标鲁棒代价的连续路径。

以 TLS 为例，目标是

\[ \rho_{\text{TLS}}(r)=\min(r^2,\bar c^2). \]

这个函数在 \(r^2=\bar c^2\) 处有折点，且非凸。

直接优化会遇到"正确内点因初值差而被过早关掉"的问题。

GNC 引入参数 \(\mu\)，构造一族 surrogate：

\[ \rho_\mu(r) \quad\Longrightarrow\quad \rho_{\text{TLS}}(r) \quad\text{as}\quad \mu\to\infty. \]

在小 \(\mu\) 时，函数更接近二次损失，所有因子都有较高权重。

随着 \(\mu\) 增大，过大残差逐渐被推向 0 权重。

用权重区间理解 GNC-TLS¶

GNC-TLS 的权重为

\[ w_i = \begin{cases} 1,& r_i^2\le \frac{\mu}{\mu+1}\bar c^2\\[2pt] \frac{\bar c}{|r_i|}\sqrt{\mu(\mu+1)}-\mu, & \frac{\mu}{\mu+1}\bar c^2<r_i^2<\frac{\mu+1}{\mu}\bar c^2\\[4pt] 0,& r_i^2\ge \frac{\mu+1}{\mu}\bar c^2. \end{cases} \]

随着 \(\mu\) 增大：

区间	下界	上界	变化
保留区	\(\frac{\mu}{\mu+1}\bar c^2\)	接近 \(\bar c^2\)	越来越严格
过渡区	两个阈值之间	宽度变窄	权重趋向二值
剔除区	\(\frac{\mu+1}{\mu}\bar c^2\) 以上	接近 \(\bar c^2\)	越来越接近 TLS

最后权重趋于

\[ w_i\to \begin{cases} 1,& r_i^2<\bar c^2\\ 0,& r_i^2>\bar c^2. \end{cases} \]

这就是硬内点集合。

为什么 GNC 能比单次 Tukey 更稳¶

单次 Tukey/GM/Welsch 在初值处立即给权重。

若初值差，真内点残差大，也会被降权。

一旦 \(w_i\approx0\)，该因子对下一步 Hessian 贡献消失，优化器就失去把它拉回来的信息。

GNC 的起点让所有因子先参与低曲率的 L2-like 优化。

只有当模型逐步靠近内点一致区域后，才逐渐关掉外点。

这是一种 continuation method。

初始化 \(\mu\) 的含义¶

GNC-TLS 常用

\[ \mu_0=\frac{\bar c^2}{2r_{\max}^2-\bar c^2}. \]

这个选择使最大残差落在 surrogate 的边界附近。

若

\[ 2r_{\max}^2-\bar c^2\le0, \]

说明所有残差本来就在阈值附近，直接优化 L2/TLS 差别不大。

GNC-GM 则从大 \(\mu\) 开始，因为 GM surrogate 在 \(\mu\to\infty\) 时接近 L2。

这解释了两者调度方向相反：

方法	起点	终点	更新
GNC-GM	\(\mu\) 大	\(\mu=1\)	\(\mu\leftarrow\mu/1.4\)
GNC-TLS	\(\mu\) 小	\(\mu\to\infty\)	\(\mu\leftarrow1.4\mu\)

内层求解器选择¶

每个 GNC 外层都需要解一个加权最小二乘：

\[ \min_x\sum_i w_i\|r_i(x)\|^2. \]

可用 GN、LM、Dogleg。

建议：

情况	内层
初值好、pose graph	GN 或 Dogleg
初值一般、非线性强	LM
大回环、坏几何	LM + 强先验
每层只需近似	少迭代 GN，下一层继续

内层不必每次求到极高精度。

GNC 外层仍会更新权重，过度求解早期 surrogate 反而浪费。

GNC 与边缘化的冲突¶

GNC 权重是动态的。

如果在某个 \(\mu\) 下把旧变量边缘化，得到的先验固定了当时的权重。

后续 \(\mu\) 变化，先验中的信息无法重新加权。

因此 GNC 不应与滑窗边缘化交织执行。

正确方式是：

在固定图上完成 GNC；
得到最终内点集合或权重；
再根据最终权重做边缘化或构建先验。

终止条件¶

GNC-TLS 常用三类终止：

条件	含义
权重二值化	\(\\|w-\operatorname{round}(w)\\|_\infty<\epsilon\)
代价相对变化小	外层继续无收益
最大外层次数	防止异常数据拖死

若权重还大量处于 \((0,1)\) 中间区，不宜过早终止。

这说明 \(\mu\) 尚未把问题推到目标 TLS。

练习¶

给定 \(\bar c^2=9\)、\(\mu=1\)，计算 GNC-TLS 的保留区、过渡区、剔除区。
解释为什么 GNC-TLS 的 \(\mu\) 增大时权重趋向 0/1。
设计一个实验比较 "Tukey 一次优化" 与 "GNC-TLS 退火" 在坏初值下的差异。

§F.28 PCM 深入：从两两一致性到最大团的工程边界 ⭐⭐⭐¶

PCM 处理的是一种特殊外点：错误回环。

错误回环常常残差很大，但在优化前无法直接用残差判定。

因为没有全局一致位姿时，某个候选回环相对于当前估计看起来大，并不一定是假。

PCM 的思路是：不问单个回环是否正确，而问两个回环能否同时正确。

两个回环的一致性闭环¶

设机器人 A 的位姿 \(a_i,a_j\)，机器人 B 的位姿 \(b_k,b_l\)。

候选跨机器人回环为

\[ z_{ik}^{ab}: a_i\to b_k,\qquad z_{jl}^{ab}: a_j\to b_l. \]

如果两个回环都正确，则沿下面闭环走一圈应回到原点：

\[ (z_{ik}^{ab})^{-1} \oplus \hat x_{ij}^{a} \oplus z_{jl}^{ab} \oplus \hat x_{lk}^{b} \approx I. \]

取 Log 得到闭环误差：

\[ c_{uv} = \operatorname{Log}\left( (z_u)^{-1}\hat x^a z_v \hat x^b \right). \]

一致性判断为

\[ c_{uv}^\top\Sigma_{uv}^{-1}c_{uv} \le \chi^2_{d,\alpha}. \]

若成立，则在一致性图中连接候选回环 \(u\) 与 \(v\)。

为什么最大团代表一致回环集合¶

一致性图的节点是候选回环。

边表示两两可同时为真。

一个 clique 表示其中任意两个回环都两两一致。

最大团就是最大的两两一致集合。

在假设"真回环彼此一致、假回环与真回环大多不一致"时，最大团就是内点集合。

这是一种组合版的 consensus。

与 RANSAC 的随机 consensus 不同，PCM 使用成对几何关系构造确定性图。

最大团是 NP-hard，但为何可用¶

Maximum clique 一般 NP-hard。

但 SLAM 中候选回环数量通常不是百万级，而是几百到几千。

同时一致性图很稀疏或具有明显核心结构。

现代 PMC/k-core/branch-and-bound 在这些图上可快速求解。

若候选数太多，可先做：

描述子分数筛选；
空间/时间邻域非极大抑制；
单回环 \(\chi^2\) 预过滤；
k-core 剪枝；
启发式最大团。

PCM 的失败边界¶

PCM 的假设是"错误回环彼此不一致"。

但对称环境会打破它。

例如长走廊有多个相同房间。

如果前端把整段轨迹整体错配到平行走廊，错误回环之间也可能相互一致。

它们会组成一个大 clique。

此时 PCM 可能选择错误集合。

缓解方法：

方法	作用
使用 odometry consistency 预过滤	剔除单个明显不可能回环
加语义/高度/时间约束	打破对称
Group-k consistency	从两两推广到多元一致性
PCM 后接 GNC	让连续优化再检验残差
SE-Sync 证书	验证剩余模型全局最优，但不验证数据真假

PCM 与 GNC 的互补¶

PCM 是离散预处理。

GNC 是连续优化。

PCM 能把外点率从 90% 降到 20%，GNC 再处理剩余难例。

如果跳过 PCM，GNC 可能仍能处理，但外层次数、局部极小风险都会增加。

如果只用 PCM，不用 GNC，剩余外点或共谋错误可能直接污染 PGO。

因此 Kimera-RPGO 采用的顺序是自然的：

candidate loop closures
  -> PCM / max clique
  -> GNC-TLS weighted PGO
  -> LM refinement
  -> optional certifiable check

阈值传播¶

闭环误差协方差 \(\Sigma_{uv}\) 应来自四段测量的组合：

\[ \Sigma_{uv} \approx \Sigma_{z_u} +\operatorname{Ad}_{z_u^{-1}}\Sigma_{\hat x^a}\operatorname{Ad}_{z_u^{-1}}^\top +\cdots \]

实际实现常简化为固定阈值。

这会让长里程计路径被过度惩罚。

更精确的做法是沿位姿图提取两节点之间的相对位姿协方差，再传播到闭环误差。

代价较高，但在多机器人大地图合并中更稳。

练习¶

对三个候选回环构造 \(3\times3\) 一致性矩阵，手动找最大团。
设计一个对称走廊例子，使错误回环两两一致，说明 PCM 失败原因。
写出 SE(2) 情形下闭环误差 \(c_{uv}\) 的完整 Log 表达式。

§F.29 学习时间预算 ⭐¶

档位 3（博士入学级别，目标：能用 GTSAM/Ceres 搭鲁棒 PGO，理解论文）：15–20 小时 - M-estimator 家族与 IRLS 推导：3–4h - Huber/Cauchy/GM/Tukey 的公式熟练（§F.4, §F.5）：2h - Ceres/GTSAM/g2o 三库 API 与阈值单位：3h（做一个跨库移植实验） - SC/DCS 的推导（§F.13–15）：3h - GNC 算法流程与在 GTSAM 的调用（§F.10–11）：3h - PCM（§F.16）：2h - 一个实验：sphere2500 + 30% 注入外点，跑 Huber/DCS/GNC 对比：2–3h

档位 4（进阶，目标：能改库、能写 Barron 自适应损失、能推 Black-Rangarajan）：再加 8–12 小时 - Black-Rangarajan 对偶推导（§F.9）：2h - Triggs correction 完整推导（§F.7）与 Ceres Corrector 源码：2–3h - Yang-Carlone RA-L'20 全文细读（含 Props.3–4 推导）：3h - Barron CVPR'19 全文与 \(\alpha\) 可学习：2h - Cert.-Opt（TPAMI'23）的 SDP 松弛轮廓：2h

§F.30 自测题（≥8 道） ⭐⭐¶

T1（推导）. 给出 Huber 函数 \(\rho_H(r)=\tfrac12 r^2\)（\(\lvert r\rvert\le k\)）、\(k\lvert r\rvert-\tfrac12 k^2\)（\(\lvert r\rvert>k\)）。推导 \(\psi_H(r)\) 与 \(w_H(r)\)，并说明为什么 Huber 不 redescending。

T2（IRLS 收敛）. 证明：若 \(w(r)=\psi(r)/r\) 非增且 \(\rho\) 凸，则 IRLS 每步满足 \(F(x_{t+1})\le F(x_t)\)，且任意聚点为全局最优。（提示：二次 majorant \(\tfrac12 w(\bar r)r^2+\text{const}\)。）

T3（Black-Rangarajan GM）. 对 Geman-McClure \(\rho(r)=\dfrac{\bar c^2 r^2}{\bar c^2+r^2}\)，令 \(\phi(z)=\rho(\sqrt z)\)，推导 Legendre-Fenchel 对偶 \(\Phi_\rho(w)\)，证明 \(\Phi_\rho(w)=\bar c^2(\sqrt w-1)^2\)，并给出最优权重 \(w^*(r)=\bar c^4/(r^2+\bar c^2)^2\)。

T4（工程 Ceres）. 用 Ceres 对一个 2D PGO（从 g2o sphere2500 或 MIT 格式读入），注入 30% 错误回环，分别用 TrivialLoss、HuberLoss(1.0)、CauchyLoss(1.0)、TukeyLoss(4.6851)，对比最终 ATE 与迭代数。关键：CostFunction 中对残差乘 \(\Sigma^{-1/2}\)，以让 Huber 阈值 a=1 有"\(1\sigma\)"语义。

T5（工程 GTSAM GNC）. 用 GTSAM 的 GncOptimizer<LevenbergMarquardtParams> 对 sphere2500 + **50% 随机错误回环**做鲁棒 PGO。分别试 GncLossType::GM 与 GncLossType::TLS，muStep=1.4，setInlierCostThresholdsAtProbability(0.99)。把里程计因子加入 knownInliers。比较 getWeights() 与 ground-truth 标签的混淆矩阵；记录最终 ATE。

T6（DCS 推导）. 从 Switchable Constraints 的简化目标（§F.13）出发，对 \(s\) 求一阶导数并解析求解 \(\partial/\partial s=0\)，得到 \(s=\Phi/(\chi^2+\Phi)\)。再对照 DCS 的最终动态尺度 \(s^2=\min(1,(2\Phi/(\Phi+\chi^2))^2)\)，解释 Agarwal 2013 为什么采用更激进的尺度函数、为什么需要 \(\min(1,\cdot)\) 保护内点，以及 \(\Phi=1\) 附近 DCS 与 Geman-McClure 权重的关系。

T7（GNC-GM vs GNC-TLS）. 设计实验：对同一 PGO 数据集以外点比例 \(\in\{10\%,30\%,50\%,70\%,85\%\}\) 分别跑 GNC-GM 和 GNC-TLS，记录每种 (method, ratio) 的 (最终 ATE、GNC 外 loop 迭代数、grad norm、judgment accuracy)。绘制 ATE-vs-outlier-ratio 曲线，验证 TLS 在高外点比下显著优于 GM。

T8（Switchable Constraints 实现）. 在 g2o 里实现 EdgeSE3Switchable（参考 Vertigo）：(a) 定义 VertexSwitchLinear；(b) 定义一元先验 EdgeSwitchPrior 把 \(s\) 拉到 \(\gamma=1\)；(c) 重载原 EdgeSE3 的 computeError()，乘以 \(\Psi(s)=\mathrm{clip}(s,0,1)\)。在 10% 外点的 manhattan3500 上测试，比较 SC 与 DCS 的耗时与 ATE。

T9（Triggs Corrector）. 阅读 ceres/internal/ceres/corrector.cc。写出 \(\alpha\) 的精确公式，指出 Ceres 对 \(\rho''>0\) 的 clamp 策略。解释：为什么 Triggs 论文的"完整修正"在凸区反而有害？（提示：Zach-Bourmaud ECCV 2018 的观察——凹二次面在展开点过冲。）

T10（Barron 退火）. 实现一个简单的 2D robust fitting（给定一堆点拟合直线，80% 内点 + 20% 外点）。用 Barron \(\rho(r,\alpha,c)\)，从 \(\alpha=2\)（L2）开始，按 α ← α - 0.5 退火到 \(\alpha=-2\)（GM）。画出每个 \(\alpha\) 下的拟合结果和权重分布，展示 GNC 效果。

§F.31 结论：鲁棒估计不是"加一行 robust kernel" ⭐⭐¶

最核心的认知转变：从 5-B 的"MAP = WNLS 假设高斯 → 最小化 \(\sum\|r\|^2\)"到本章的"外点普遍存在 → 最小化 \(\sum\rho(r)\)"，代价函数的选择本身成为建模决策。Huber、Cauchy、Tukey 不是随便挑——它们对应了对外点分布的不同假设：Huber 假设外点是 Laplace 尾巴（对称重尾），Tukey 假设外点完全与信号无关（可直接丢弃），DCS 假设外点可通过方差膨胀吸收。

理论与工程的两条主线交汇于 GNC。理论线：Black-Rangarajan 1996 的半二次对偶把非凸 M-estimation 转为辅助变量的联合优化；Yang-Carlone 2020 把这个框架用到 SLAM，在 GM 和 TLS 上给出精确的 \(\mu\) 调度和收敛保证；Yang-Carlone 2023 配 SDP 进一步给出可认证全局最优性。工程线：Sünderhauf 2012（SC）加变量、Agarwal 2013（DCS）消变量、Mangelson 2018（PCM）预处理、MIT-SPARK 2020+（Kimera-RPGO 的 PCM+GNC 管线）集大成。现代鲁棒 SLAM 后端的默认配方就是 PCM → GNC-TLS → LM。

新手最容易犯的错不是选错损失，而是阈值单位错乱。把 Ceres 习惯带到 GTSAM（或反之）会让 Huber 阈值差若干个数量级，直接使鲁棒核失效（"Huber 行为退化为 L2"或"所有因子都被当外点"）。记住 §F.23 的映射表比记所有公式更重要。

RL / 具身智能 / VIO 工程师的直接可落地建议： 1. 在线 VIO 因子图用 noiseModel::Robust::Create(Huber::Create(1.345), gaussian)——与 iSAM2 兼容，0 调参风险； 2. 动态场景加 DCS（\(\Phi=\chi^2_{3,0.99}\)）对付偶发外点； 3. 离线 / 重定位 / 多轨迹合并时，切换到 Kimera-RPGO 的 PCM + GncOptimizer(TLS) 管线； 4. 若研究里有 Barron 式可学习鲁棒性需求，参考 CVPR'19 原文手写 LossFunction 在 Ceres/pyceres 里注入。

§F.32 鲁棒估计的全景方法对比 ⭐⭐⭐¶

把本章涉及的所有方法放在一个统一框架中对比，帮助读者建立全局视图。

按"何时起作用"分类：

阶段	方法	作用时机	与因子图后端的关系
预处理	RANSAC	前端几何验证（图像匹配/配准）	在因子图之前
预处理	PCM	回环检测后、优化前	删除不一致边
优化中	Huber/DCS	每次 GN 迭代更新权重	嵌入因子图，兼容 iSAM2
优化外	GNC	batch outer loop 退火	包裹因子图，不兼容 iSAM2
优化后	SE-Sync + 证书	验证解是否全局最优	后验检查

按"需要什么输入"分类：

方法	需要的先验知识	超参数	对初值的敏感性
Huber	噪声协方差（用于白化）+ 阈值 \(k\)	1 个标量	低（凸）
Cauchy	同上 + 尺度 \(c\)	1 个标量	中（非凸但宽碗）
Tukey/Welsch	同上	1 个标量	高（redescending）
DCS	同上 + \(\Phi\)（通常用 \(\chi^2\)）	1 个标量	中
SC	同上 + 先验方差 \(\Xi\)	2 个参数	高
GNC-GM	同上 + 退火参数 \(\mu_0, \alpha\)	3 个参数	低（从凸起点）
GNC-TLS	同上 + 截断阈值 \(\bar{c}^2\)	3 个参数	低（从凸起点）
PCM	位姿不确定性 + 一致性阈值	2 个参数	无（图论方法）
RANSAC	内点阈值 + 迭代数	2 个参数	无（随机采样）

终极选型决策树：

你的外点率是多少？
├── < 10%（偶发外点）
│   └── Huber 或 DCS 足矣
│       ├── 在线 iSAM2：Huber (最安全)
│       └── 需要更强剔除：DCS (Φ=χ²_{d,0.99})
├── 10-50%（常见回环外点）
│   ├── 在线：DCS + PCM 预筛
│   └── 离线：GNC-GM + batch LM
├── 50-80%（多机器人合并）
│   └── PCM + GNC-TLS（Yang-Carlone 管线）
└── > 80%（极端场景）
    └── TEASER++ (配准) 或 PCM + GNC-TLS + SE-Sync (PGO)

反事实推理——如果不做鲁棒处理会怎样？ 一条错误回环（外点）在 L2 下的代价为 \(\frac{1}{2}\|r\|^2\)。若闭合误差为 10m（\(|r|\approx 100\) 在白化后），它贡献 \(5000\) 的代价——而 1000 条正确边的总代价可能只有 \(\approx 500\)（每条 \(\sim 0.5\)）。优化器会牺牲所有 1000 条正确边来降低这一条错误边的代价——因为 L2 让一条大残差的"声量"是千条小残差的 10 倍。结果：整张地图被拉坏。这就是 breakdown point = 1/N 的直观体现：一条外点，毁掉全图。

§F.33 Barron 统一损失族——所有鲁棒核的连续参数化 ⭐⭐⭐⭐¶

Barron CVPR 2019 提出了一个优雅的统一框架，把几乎所有常见鲁棒核纳入一个双参数族 \(\rho(r, \alpha, c)\)：

\[\rho(r, \alpha, c) = \frac{|\alpha-2|}{\alpha}\left(\left(\frac{(r/c)^2}{|\alpha-2|}+1\right)^{\alpha/2}-1\right)\]

当 \(\alpha\) 取特殊值时退化为经典鲁棒核：

\(\alpha\)	对应核	性质
\(2\)	L2	凸，无鲁棒性
\(1\)	Pseudo-Huber / Charbonnier	凸，轻微鲁棒
\(0\)	Cauchy / Lorentzian	非凸，软 redescending
\(-2\)	Geman-McClure	非凸，redescending
\(-\infty\)	Welsch	非凸，强 redescending

连续退火的自然实现：GNC 可以用 \(\alpha\) 从 2（L2）逐步退火到负值（GM/Welsch），路径上每个 \(\alpha\) 对应一个平滑的鲁棒核。这比 Yang-Carlone 2020 的 \(\mu\) 参数化更自然——\(\alpha\) 直接控制核的"形状"，\(c\) 控制"尺度"。

可学习性：Barron 2019 的核心贡献不仅是统一参数化，还在于把 \((\alpha, c)\) 当作**可学习参数**嵌入神经网络训练。对 robust fitting 任务，网络可以为不同观测自动选择不同的 \((\alpha_i, c_i)\)——这相当于"自适应鲁棒核"，避免了手动设置阈值的痛苦。

实现注意事项：\(\alpha=2\) 和 \(\alpha=0\) 是奇异点（分母为零），需要用 L'Hopital 极限替代。具体地： - \(\alpha\to 2\)：\(\rho\to r^2/(2c^2)\) - \(\alpha\to 0\)：\(\rho\to \log((r/c)^2/2+1)\)

Barron 在论文中给出了数值稳定的实现（用 log1p 避免 cancellation），可以在 Ceres 中通过自定义 LossFunction 直接使用。

§F.34 三库阈值映射的数学推导 ⭐⭐¶

这是工程中最容易出错的地方。三大 SLAM 库对鲁棒核的接口设计不同，导致同一个物理阈值在不同库中表现为不同的数值。

核心差异来源：

GTSAM：noiseModel::Robust::Create(m_estimator, gaussian) 内部先对残差做白化 \(\tilde{r}=\Sigma^{-1/2}r\)，然后 m_estimator 作用在 \(|\tilde{r}|\) 上。因此阈值 \(k\) 的单位是"白化后残差"（即"\(\sigma\) 单位"）。
Ceres：AddResidualBlock(cost, loss, params) 中，loss 接收的是 \(s=\|r\|^2\)（残差范数的平方）。但构造 HuberLoss(a) 时，\(a\) 是残差**范数**（非平方）的阈值。Ceres 内部计算 \(\rho(s)\) 在 \(s=a^2\) 处切换。前提：CostFunction 必须已经完成白化（返回 \(\Sigma^{-1/2}r\)），否则 \(a\) 没有物理意义。
g2o：edge->setRobustKernel(rk) 中，rk->setDelta(delta) 的 delta 作用在 白化残差范数 上（与 GTSAM 相同单位）。g2o 内部先计算 \(e=r^\top\Omega r\)（Mahalanobis 平方），然后用 \(\sqrt{e}\) 与 delta 比较。

映射表：

设物理目标：在白化空间中 \(|\tilde{r}|=k_\sigma\) 处切换到 robust region。

库	构造代码	参数含义	设置值
GTSAM	`Huber::Create(k)`	\(k\) = 白化残差范数阈值	\(k_\sigma\)
Ceres	`HuberLoss(a)`	\(a\) = 白化残差范数阈值（前提：CostFunction 已白化）	\(k_\sigma\)
g2o	`rk->setDelta(d)`	\(d\) = 白化残差范数阈值	\(k_\sigma\)

看起来三者相同？陷阱在于"是否已白化"的假设不同：

GTSAM 的 noiseModel::Robust 会**自动白化**——你传入 raw 残差和 gaussian noise model，它内部白化后再传给 m_estimator。
Ceres 的 LossFunction 不白化——它假设 CostFunction 已经返回了白化残差。如果你的 CostFunction 返回原始残差 \(r\)（未乘 \(\Sigma^{-1/2}\)），则 HuberLoss(a) 中的 \(a\) 对应的是原始残差单位，不是 \(\sigma\) 单位。
g2o 的 chi2() 方法返回 \(r^\top\Omega r\)——已经是 Mahalanobis 平方，robust kernel 的 delta 作用在 \(\sqrt{\chi^2}\) 上。

最容易犯的错：

❌ 从 GTSAM 搬家到 Ceres 时，直接写 HuberLoss(1.345)
   但 CostFunction 没有白化 → 阈值含义完全不同

✅ 正确做法：在 CostFunction::Evaluate 中返回 Σ^{-1/2} * r
   然后 HuberLoss(1.345) 就与 GTSAM Huber::Create(1.345) 等价

GTSAM factor error 带 0.5 因子的陷阱：

GTSAM 的 NonlinearFactor::error() 返回 \(\frac{1}{2}\|r\|^2_\Omega\)（已含 \(\frac{1}{2}\)），而 Yang-Carlone 2020 论文中 TLS 的截断阈值 \(\bar{c}^2=\chi^2_{d,\alpha}\) 不含 \(\frac{1}{2}\)。因此在 GTSAM 中设置 GncParams::setInlierCostThreshold 时，应使用 \(\frac{1}{2}\chi^2_{d,\alpha}\)。这个 \(\frac{1}{2}\) 差异在论文和代码之间反复造成混淆（参见 GTSAM Wiki 和 Kimera-RPGO 源码注释）。

§F.35 从 Fisher 到 Huber：鲁棒统计学的百年简史 ⭐⭐⭐¶

鲁棒估计不是最近才有的——它有深厚的统计学根基。理解这段历史有助于把 SLAM 中的鲁棒后端放在正确的学术语境中。

时期	贡献者	核心贡献	对 SLAM 的影响
1805	Legendre / Gauss	最小二乘法	L2 = 高斯分布的 MLE
1887	Edgeworth	识别异常值对均值的影响	外点问题的首次认识
1960	Tukey	Box plots + 外点诊断	可视化外点检测
1964	Huber	M-estimator + minimax 鲁棒性	现代鲁棒统计学奠基
1971	Hampel	影响函数 + breakdown point	量化鲁棒性的数学工具
1981	Fischler-Bolles	RANSAC	计算机视觉的标准鲁棒工具
1987	Yohai	S-estimator / MM-estimator	高 BP + 高效率
1992	Blake-Zisserman	Visual reconstruction with robust cost	首次在视觉中用非凸鲁棒核
1996	Black-Rangarajan	半二次对偶 + graduated	GNC 的理论基础
2012	Sünderhauf-Protzel	Switchable Constraints	第一个 SLAM 专用鲁棒后端
2013	Agarwal et al.	DCS	闭式权重，比 SC 快
2018	Mangelson et al.	PCM	图论预处理
2020	Yang-Carlone	GNC for SLAM	MIT-SPARK 的标准方案
2021	Yang et al.	TEASER++	配准 + 证书
2023	Yang-Carlone	TPAMI 统一框架	鲁棒 + 可认证的集大成

这条线索展示了一个重要的学术模式：统计学理论（Huber 1964）→ 计算机视觉应用（Blake 1992、Black 1996）→ SLAM 工程落地（Sünderhauf 2012、Yang 2020）。每一步都间隔了 20-30 年。当前的前沿（可学习鲁棒核、certifiable + robust）可能在 2030-2035 年成为工业标准。

对博士生的研究方向建议：

方向	开放程度	难度	潜在影响
在线自适应鲁棒核（Barron \(\alpha\) 在线调）	高	⭐⭐⭐	消除手动调阈值
GNC + iSAM2 的无缝集成	高	⭐⭐⭐⭐	在线鲁棒 SLAM 的理想形态
学习型外点分类器替代 PCM	中	⭐⭐⭐	超越图论方法的上限
鲁棒估计的 estimation contracts	高	⭐⭐⭐⭐	Carlone FnT 2023 的核心概念
动态物体的鲁棒 SLAM	极高	⭐⭐⭐⭐	自动驾驶核心问题

本质洞察：鲁棒估计的核心困难不在于"有一种万能方法"，而在于**不同外点率、不同外点结构需要不同强度的工具**。正如医学中"轻症用药、重症手术"，鲁棒估计中"偶发外点用 Huber、系统性外点用 GNC、对称环境外点用 PCM 预处理"。理解每种工具的适用边界，比掌握任何一种工具的实现细节更重要。

常见陷阱与故障排查¶

⚠️ 陷阱一：阈值单位跨库照搬。 GTSAM 的 factor error 带 \(\tfrac12\)，g2o 常用 Mahalanobis 平方，Ceres 的 loss 接收 \(s=\|r\|^2\)。

⚠️ 陷阱二：把 redescending 和有界损失混为一谈。 Cauchy 的影响函数趋零，但代价不封顶；Tukey/GM/Welsch 才是更强的饱和代价。

⚠️ 陷阱三：在线 iSAM2 中直接跑 GNC。 GNC 是 batch outer loop，会反复改全图权重；在线系统应使用 Huber/DCS，必要时周期性离线重优化。

故障排查现象	可能原因	处理方式
Huber 几乎等同 L2	残差未白化或阈值过大	检查 \(\Sigma^{-1/2}r\)，用 NIS 标定阈值
好回环被 SC 关闭	\(\Xi\) 先验过弱或初值太差	降低先验协方差，先跑 PCM/GNC
GNC 迭代不收敛	\(\mu\) 步长过大或初始 \(\mu\) 选择不当	减小 `muStep`，增大初始 `mu`
DCS 效果比 Huber 差	\(\Phi\) 设置与残差尺度不匹配	按 \(\chi^2_{d,0.99}\) 设置 \(\Phi\)
Ceres Loss + 自定义白化冲突	Loss 接收 \(s=\\|r\\|^2\)，但残差已白化两次	确认 CostFunction 中已白化，Loss 阈值按白化后残差设
GNC 权重振荡	\(\mu\) 步长过大或内层未收敛	减小 `muStep`，提高内层 LM 精度

本质洞察：鲁棒估计不是在最小二乘外面包一层函数，而是在重新定义“哪些观测有资格影响状态”。权重、阈值和白化共同决定这个资格判定。

练习¶

对同一个白化残差 \(r=3\)，分别计算 Huber、Cauchy、GM、TLS 的 \(\rho,\psi,w\)。
把 GTSAM 的 0.5*Chi2inv 阈值换算成白化残差平方阈值，解释为什么差一个因子 2。
在一个小型 pose graph 中注入 20% 错误回环，比较 Huber、DCS 和 GNC-TLS 的最终权重分布。

本章小结¶

核心概念	一句话要义	关键公式	工程工具
Breakdown Point	使估计器跑到无穷的最小污染比例	OLS: BP=1/N	选择鲁棒核的理论指导
M-estimator IRLS	加权 GN，权重 \(w(r)=\psi(r)/r\)	\((J^\top WJ)\delta=-J^\top Wr\)	三库通用（只改权重）
影响函数 \(\psi\)	单个观测对估计的"拉力"	\(\psi=\rho'\)；有界 ⟹ 外点声量有限	判断鲁棒核强度
Huber	凸、非 redescending、最安全兜底	\(\psi(r)=\min(r,k\cdot\text{sign}(r))\)	`HuberLoss` / `Huber::Create`
GM / Tukey / Welsch	非凸、redescending、外点完全失声	\(w\to 0\) for large \(r\)	`CauchyLoss` / `TukeyLoss`
TLS	截断最小二乘——硬 0/1 分类	\(\rho=\min(r^2/2, \bar{c}^2/2)\)	GNC-TLS
DCS	闭式动态权重，近似 GM	\(w=\min(1,(2\Phi/(\Phi+r^2))^2)\)	`DCS::Create`
Black-Rangarajan	半二次对偶——非凸变交替优化	\(\min_{x,w}[wr^2+\Phi(w)]\)	GNC 的理论基础
GNC	从凸退火到非凸，避免局部极小	\(\mu\): 凸(Huber) → 非凸(GM/TLS)	`GncOptimizer`
PCM	预处理：一致性最大团剔除外点	图论最大团	Kimera-RPGO
三库阈值映射	GTSAM(\(\sigma\)) / Ceres(\(\sqrt{s}\)) / g2o(\(\sqrt{\chi^2}\))	§F.23 映射表	跨库移植必查

累积项目：本章新增模块¶

项目名称：从零构建 Mini-LIO 后端 → 添加鲁棒层

本章新增：鲁棒代价函数 + GNC 离线修正

在前序章节中，累积项目已完成： - 5-A：Kalman 滤波递推器 - 5-B：Batch 因子图 GN 求解器 - 5-C：iSAM2 增量后端 - 5-E：SE-Sync 全局验证层

本章新增： 1. 为 iSAM2 后端的回环因子添加 noiseModel::Robust(Huber) 包裹 2. 实现离线 GNC-TLS 修正管线：从 iSAM2 导出因子图 → GncOptimizer → 把清理后的权重回注 3. 实现三库阈值映射工具函数（GTSAM ↔ Ceres ↔ g2o） 4. 对比实验：sphere2500 + 30% 外点，分别用 Huber/DCS/GNC-TLS

# 累积项目骨架
import gtsam
import numpy as np

class RobustBackend:
    def __init__(self, robust_type='huber', threshold=1.345):
        """
        robust_type: 'huber', 'cauchy', 'dcs', 'gnc_tls'
        threshold: 白化残差空间中的阈值
        """
        self.robust_type = robust_type
        self.threshold = threshold

    def create_robust_noise(self, base_noise):
        """创建鲁棒噪声模型"""
        if self.robust_type == 'huber':
            robust = gtsam.noiseModel.mEstimator.Huber.Create(self.threshold)
        elif self.robust_type == 'cauchy':
            robust = gtsam.noiseModel.mEstimator.Cauchy.Create(self.threshold)
        elif self.robust_type == 'dcs':
            robust = gtsam.noiseModel.mEstimator.DCS.Create(self.threshold)
        else:
            return base_noise  # GNC 不在此处设置
        return gtsam.noiseModel.Robust.Create(robust, base_noise)

    def run_gnc(self, graph, initial_values, known_inliers=None):
        """运行 GNC-TLS 离线修正"""
        # TODO: 学生实现
        # 1. 配置 GncParams
        # 2. 设置 knownInliers（里程计因子）
        # 3. 运行 GncOptimizer
        # 4. 返回权重和优化结果
        pass

    @staticmethod
    def threshold_mapping(value, from_lib='gtsam', to_lib='ceres'):
        """三库阈值映射"""
        # TODO: 实现 §F.23 的映射逻辑
        pass

延伸阅读¶

资源	难度	内容	建议阅读方式
Huber Robust Statistics 2nd ed. 2009	⭐⭐⭐	M-estimator 完整理论	Ch.1 + Ch.4
Black & Rangarajan IJCV 1996	⭐⭐⭐⭐	半二次对偶原始论文	§III-IV 推导
Yang & Carlone RA-L 2020	⭐⭐⭐	GNC for SLAM	Props 3-4 收敛保证
Yang & Carlone TPAMI 2023	⭐⭐⭐⭐	Certifiable + Robust 统一	§III SDP + TLS
Agarwal et al. ICRA 2013	⭐⭐	DCS 原始论文	简短实用
Sünderhauf & Protzel IROS 2012	⭐⭐	Switchable Constraints	§III-IV
Mangelson et al. ICRA 2018	⭐⭐⭐	PCM 原始论文	§III 一致性矩阵
Barron CVPR 2019	⭐⭐⭐	统一损失族 \(\rho(r,\alpha,c)\)	§2-3
MacTavish & Barfoot CRV 2015	⭐⭐	鲁棒核在 SLAM 的实践	Table I 推荐阈值
Ceres `corrector.cc` 源码	⭐⭐⭐	Triggs 修正实现	理解 \(\alpha\) clamp 策略
Kimera-RPGO 源码	⭐⭐⭐	PCM + GNC 完整管线	`OutlierRemoval.cpp`

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
Huber 加了但效果等同 L2	残差未白化，或阈值远大于实际残差	1.打印白化后残差分布 2.用 \(\chi^2\) 检验确认阈值 3.检查 Ceres 是否已内部白化	§F.24 #1-2
GNC 不收敛（外循环超过 100 次）	\(\mu\) 步长过大或初始 \(\mu\) 选择错	1.减小 `muStep`（如 1.4→1.2） 2.增大初始 \(\mu_0\) 3.检查是否有 known inliers	§F.10-11
DCS 使内点权重也被降低	\(\Phi\) 设置过小	1.按 \(\chi^2_{d,\alpha}\) 计算合理 \(\Phi\) 2.打印权重分布 3.与 ground truth 对比	§F.24 #5
在线 iSAM2 + GNC 行为异常	GNC 是 batch，与 iSAM2 增量机制冲突	1.分离在线(Huber/DCS)与离线(GNC)管线 2.周期性触发 batch GNC	§F.24 #4
跨库移植后精度骤降	阈值单位混淆（Ceres \(\sqrt{s}\) vs GTSAM \(\sigma\)）	1.查 §F.23 映射表 2.对一个已知因子打印白化残差 3.确认两库的 factor error 定义	§F.23
Tukey/Welsch 使解陷入局部极小	非凸鲁棒核 + 差初值 → 真内点被永久降权	1.先用 Huber 暖启动几轮 2.然后切换到 Tukey/Welsch 3.或直接用 GNC 退火	§F.4, §F.10
鲁棒因子与 marginalization 交互出错	边缘化时刻的动态权重被固化进先验	1.边缘化前固定一次权重判定 2.先确定内点/外点再边缘化	§F.24 #9

跨章综合练习 ⭐⭐⭐¶

题目：综合 5-B（因子图）+ 5-C（iSAM2）+ 5-E（Certifiable）+ 5-F（本章鲁棒估计）的知识：

完整鲁棒后端管线设计：为一个自动驾驶 LiDAR SLAM 系统设计完整的鲁棒后端。要求覆盖以下场景并给出每种场景的处理策略：(a) 正常行驶（无外点）；(b) 动态物体导致的偶发 scan-matching 外点；(c) 对称环境导致的错误回环；(d) GPS 多径导致的错误位置观测。画出完整数据流图，标注每个模块（在线 Huber/DCS + 离线 PCM + GNC-TLS + SE-Sync 验证）的触发条件和接口。
GNC 退火路径可视化：对一个包含 5 个内点和 2 个外点的 2D 直线拟合问题，手动实现 GNC-TLS。从 \(\mu=\mu_0\)（所有权重为 1）开始，按 \(\mu\leftarrow\mu/1.4\) 退火。在每个 \(\mu\) 下画出：(a) 当前拟合直线；(b) 每个点的权重 \(w_i\in[0,1]\)；(c) 目标函数值。验证：外点的权重是否从 1 逐步降到 0？内点的权重是否始终接近 1？
阈值标定实验：对同一个因子图，在 GTSAM 中用 Huber::Create(1.345) 和在 Ceres 中用 HuberLoss(1.0)，验证两者是否给出相同结果。如果不同，解释差异来源（提示：检查 Ceres 的 CostFunction 是否已经包含了白化步骤；GTSAM 的 noiseModel::Robust 内部如何处理白化）。

术语对照表¶

英文术语	中文	首次出现节
M-estimator	M-估计量	§F.3
Influence Function	影响函数	§F.3
Breakdown Point	崩溃点	§F.3
Graduated Non-Convexity (GNC)	渐进非凸化	§F.10
Truncated Least Squares (TLS)	截断最小二乘	§F.10
Geman-McClure (GM)	Geman-McClure 核	§F.4
Dynamic Covariance Scaling (DCS)	动态协方差缩放	§F.7
Switchable Constraints (SC)	可切换约束	§F.6
Pairwise Consistency Maximization (PCM)	成对一致性最大化	§F.15
Half-Quadratic Optimization	半二次优化	§F.9
Iteratively Reweighted Least Squares (IRLS)	迭代重加权最小二乘	§F.9
Triggs Correction	Triggs 修正	§F.23
Annealing Schedule	退火策略	§F.10
Black-Rangarajan Duality	Black-Rangarajan 对偶	§F.9
Convex Envelope	凸包络	§F.10
Adaptive Kernel	自适应核函数	§F.13

方法选型决策树¶

在实际 SLAM 系统中，选择鲁棒方法时可按以下优先级判断：

外点比例 < 5%：单纯 Huber 核即可，简单高效
外点比例 5-20%：Huber 暖启动 + Tukey/DCS 收尾，或在线 DCS
外点比例 20-50%：必须用 GNC-TLS 或 PCM 前置筛选
外点比例 > 50%：前端数据关联有严重问题，鲁棒后端无法救场，需修复前端
需要全局最优保证：GNC-TLS + SE-Sync 认证管线

工程实践注意事项：

鲁棒核的阈值选择必须基于白化后的残差分布，而非原始残差。不同因子类型（视觉重投影 vs LiDAR 点面距离 vs IMU 预积分）的残差量纲完全不同，不能共用同一阈值
GNC 是 batch 方法，不适合直接嵌入 iSAM2 增量更新循环。推荐的工程方案是：在线层用轻量级 Huber/DCS，离线/周期性层用 GNC 做全图清洗
外点剔除后需要重新检查图的连通性——如果剔除过多边导致图断裂，后续优化将出现不可观方向
在多传感器融合系统中，不同传感器的外点率通常差异很大（视觉回环 > GPS > IMU），应对每类因子独立设置鲁棒策略