Bundle Adjustment 实践：滑窗、鲁棒核与收敛

1. 公式三行，真机十毫秒

BA 最小化重投影误差——教科书三行写完。真机要问：10 ms 内跑完吗？误匹配会不会拉偏？迭代到了是收敛还是凑次数？我从 ORB-SLAM 和 Ceres 源码学到的比公式有用：Local 窗口多大、culling 与 BA 是否同流水线、Chi2 是否被检查——这些决定地图是「更新了」还是「真收敛了」。跟踪线程 Pose-only 保实时，Local BA 保局部一致，Global BA 只在回环后拉直——职责分清才不会在错误层调参。

2. Local / Global / Pose-only

Pose-only：只优化位姿，路标固定——跟踪线程每帧，最快。Local BA：共视邻域 5–20 帧 + 数百路标，插关键帧触发，窗外 fix。Global BA：全图，回环后或离线，千帧×千点需分钟级，只能后台线程。Pose-only 精度有限但保跟踪；Local 是实时 SLAM 主力；Global 拉直累积误差——三者触发条件写进状态机，避免 tracking 线程误触 Global。

3. 重投影残差

r_{k} = π (K, T_{k}, P_{j}) - u_{k j}

$π$ 含畸变；对 $T_{k}, P_{j}$ 在切空间（右扰动）求导。信息矩阵 $Ω = (σ_{p x}, I)^{- 1}$ ，通常 $σ = 0.5$ –2 px。不同特征检测精度不同，统一权重是近似——FAST 与 subpixel refinement 混用时，可对 refined 点给更小 $σ$ 。

4. 鲁棒核

Huber：

ρ (r) = {r^{2} /2 ∣ r ∣ \leq δ δ (∣ r ∣ - δ /2) else

$δ$ 取预期 pixel 噪声 2–3 倍（ORB-SLAM 约 1–2 px 归一化坐标）。太大 → 退化为 L2；太小 → 正常点也被降权。回环 BA 外点多时可试 Cauchy。Ceres LossFunction 内部 IRLS——loss 太激进会权重震荡，LM 不收敛；调试时先 L2 收敛再开 Huber，对比 inlier 比例是否合理。

5. Schur 补

Hessian 分块 $H_{pp}, H_{pl}, H_{l l}$ —— $H_{l l}$ 块对角（每路标只见少量帧）：

H_{schur} = H_{pp} - H_{pl} H_{l l}^{- 1} H_{l p}

消路标只求 pose，再回代路标——5000 点消掉后只剩 120 维 pose 系统。g2o BlockSolver、Ceres Schur 皆此。路标被过多帧共视则块对角性破坏，加速失效——大地标、长基线场景要 culling。Schur 是 BA 能在毫秒级运行的主因，不是「可选优化」。

6. 收敛判据

勿固定「10 次迭代」。看： $∣ r ∣$ 变化 < ε；步长 $∣Δ ξ ∣$ 足够小；Chi2 $r^{⊤} Ω r / dof \approx 1$ 。过度拟合时 Chi2 很小但残差结构异常。LM 的 $λ$ ：残差降 → 减 $λ$ ；不降 → 增 $λ$ 。function_tolerance 与窗口大小要一起调，否则「迭代到了」位姿几乎没动。Local BA 日志应记录每次迭代 $∣ r ∣$ 与 max $∣Δ ξ ∣$ ，便于线上对比回归。

7. 窗口与 outlier 生命周期

共视 KF × 每帧观测路标 ≈ 变量规模——我现场上限约 10–15 KF、800–1500 路标，超出则 cull 或 fix 远点。被 2 帧观测、重投影一直大的点应在 Local BA 前剔除，别只靠鲁棒核慢慢降权。ORB-SLAM 的 map point creation/culling 与 BA 同线程——只调 BA 不调 culling，效果有限。窗口变大前先 profile Schur 求解耗时，别等帧率掉了再查。

8. gauge 与 anchor

Local BA 窗口若无 anchor/prior，整窗 6-DOF 漂移——表现为相对形状对但绝对位姿飘。至少 fix 一个 KF 或加 weak prior。Global BA 回环后更要注意 gauge：回环边只约束相对，仍需 world anchor。

9. 案例：只调 BA 不调 culling

地图点被 2 帧观测、重投影持续 > 3 px，Local BA 前未 cull——Huber 慢慢降权仍 poison Hessian，窗口越大越慢且位姿抖。ORB-SLAM 把 creation/culling 与 Local Mapping 绑在同线程，是有原因的。

10. 验收

Local BA 耗时 vs 窗口规模；
每次残差下降比例；
鲁棒核内外点比例；
Global BA 前后 ATE 改善 vs 耗时；
路标深度为负是否剔除；
Chi2 per observation 接近 1。

BA 实践是「在时限内收敛到正确极小值」——旋转用欧拉角、无鲁棒核、不 fix gauge，任何一个都能让地图 silently wrong。Local BA 耗时与窗口规模应做 profiling 曲线，别凭感觉定 keyframe 上限。Global BA 放后台线程时，须与 Local 互斥或 copy graph，避免并发写 map。回环后 Global BA 改善 ATE 但耗分钟级，应可配置关闭以保实时性。Chi2 监控应进 Local BA 日志，而不只在离线工具里看。Pose-only 与 Local 的 fix 策略不同，混线程写 map 前须加锁或双缓冲。Local 窗口扩到上限前，先量 Schur 求解 ms 级耗时曲线。