多数「SLAM 入门」停在模块框图。本文从估计问题本身出发：状态怎么定义、约束怎么进入优化、何时不可观、回环如何改写信息矩阵，以及如何用数值证据判断系统是否真的在工作。

问题的严格形式

把时刻 t 的机器人位姿记为 x_t ∈ SE(3)（或 SE(2)），地图记为 m（路标、子地图、占据栅格等）。观测为 z_{1:T}。经典目标是最大后验（MAP）：

(x*, m*) = argmax  p(x, m | z)
         = argmax  p(z | x, m) p(x, m)

取负对数后变成非线性最小二乘：

min_{x,m}  Σ_k  ρ_k( || r_k(x, m; z_k) ||²_{Σ_k} )

其中：

• r_k：第 k 个因子的残差（重投影、点到面、IMU 预积分、里程计……）
• Σ_k：观测协方差（决定信息矩阵权重）
• ρ_k：鲁棒核（Huber / Cauchy / Geman-McClure），抑制外点

前端负责提出残差与初值；后端负责在流形上迭代求解。把两者混为一谈，就会出现「跟踪看着挺稳，地图却越扭越歪」的典型病症。

因子图：把系统画成可计算的结构

因子图（Factor Graph）是现代 SLAM 的统一语言：变量节点是状态，因子节点是约束。

2.1 常见因子类型

关键洞察：后端并不「知道」相机或雷达，它只吃残差 + 雅可比 + 协方差。

2.2 为什么用流形而不是欧氏向量

旋转不能当 R³ 向量加减。对 SE(3) 扰动常用右乘扰动：

T ⊕ ξ = T · Exp(ξ),   ξ ∈ ℝ⁶

雅可比、协方差都在切空间定义。忽略这一点会出现：

• 更新后旋转不正交
• 协方差「数值爆炸」
• 小角度近似在大回环修正时失效

Bundle Adjustment：视觉后端的核心

3.1 重投影残差

对位姿 T_cw 与路标 p_w：

r = π(K, T_cw · p_w) - u

π 含针孔/鱼眼模型与畸变。BA 同时优化一批关键帧与局部地图点。

3.2 Schur 补：为什么实时系统能跑起来

完整正规方程：

[ H_xx  H_xl ] [Δx] = [b_x]
[ H_lx  H_ll ] [Δl]   [b_l]

路标维度远大于位姿。对 H_ll 做 Schur 消元，得到只含位姿的约化系统：

(H_xx - H_xl H_ll⁻¹ H_lx) Δx = b_x - H_xl H_ll⁻¹ b_l

工程含义：

• H_ll 近似块对角 → 消元便宜
• 实时系统做 Local BA（滑窗），不做全图 Dense BA
• 回环后常做 Pose Graph 优化，再视情况触发 Global BA

ORB-SLAM 系的「局部建图线程 + 回环线程」本质上就是在调度这些不同规模的优化问题。

可观性：精度上限不在调参里

4.1 单目纯视觉

无先验时，单目 SLAM 对全局尺度不可观；对刚体变换的 gauge freedom（世界系任意相似变换）也存在。实际靠：

• 固定第一帧位姿
• 固定某个路标深度 / 基线尺度
• 或引入 IMU / 轮速 / 立体基线

4.2 几何退化

即使算法正确，场景也会让信息矩阵秩亏：

• 纯旋转：三角化失败，深度不可观
• 弱纹理 / 运动模糊：有效残差数量崩溃
• 平面主导：单应与本质矩阵歧义
• 长廊 / 隧道（LiDAR）：纵向平移弱可观

调试时不要只看「丢跟踪」，要问：当前运动激励是否足以让状态可观？

4.3 一个实用判据

在滑窗内检查：

1. 路标平均视差角是否过小
2. 信息矩阵最小特征值 / 条件数
3. 边缘化后先验是否「过度自信」（协方差过小）

条件数恶化时，加大鲁棒核、缩短滑窗、引入绝对观测（GPS、回环、标签）比盲调 ORB 阈值更有效。

回环：改写信息结构，而不是「再匹配一次」

回环检测给出候选 (a, b) 后，必须经过几何验证（PnP + RANSAC / ICP），再添加因子。错误回环的危害是系统性的：一次错误长程约束会把整条轨迹掰断。

工程上的分层防御：

1. 描述子层：BoW / Scan Context 召回
2. 几何层：内点比例、重投影误差、尺度一致性
3. 优化层：开关约束、Cauchy 核、一致性检验（如 Chi2）
4. 地图层：多地图（Atlas）隔离，确认后再 merge

ORB-SLAM3 的 multi-map 思路，本质上是拒绝在不确定时强行把约束焊死。

前端质量如何进入后端

后端再强也救不了系统性偏差。前端必须输出诚实的协方差：

• 特征法：按金字塔层、匹配距离、重投影历史估 Σ
• 直接法：按光度 Hessian / 图像梯度
• LiDAR：按点到面距离与入射角

常见错误：所有因子共用单位阵 I。结果是：

• 好观测与坏观测同等发言权
• LM 阻尼被迫「靠玄学」稳定

评测：用数字说话

7.1 ATE（Absolute Trajectory Error）

对齐估计轨迹 P̂ 与真值 P（Umeyama / SE3 / Sim3）后：

ATE = sqrt( 1/N Σ_i || trans(P_i⁻¹ P̂_i) ||² )

适合评价全局一致性（有回环的系统）。

7.2 RPE（Relative Pose Error）

对固定间隔 Δ 的相对运动：

E_i = (P_i⁻¹ P_{i+Δ})⁻¹ (P̂_i⁻¹ P̂_{i+Δ})

适合评价里程计漂移率。无回环系统应主要看 RPE，用 ATE 会被对齐方式误导。

7.3 除了 evo 曲线还要看什么

• 跟踪丢失次数 / 重定位成功率
• 回环正确率 vs 误触发率
• CPU 耗时分布（跟踪是否抢占建图）
• 地图点存活率（大量地图点秒删说明三角化质量差）

开源系统如何「读源码」

不要从 main 顺序读。按因子图反向读：

1. 状态向量在哪里定义（位姿、速度、bias、路标）
2. 残差函数与雅可比
3. 边缘化 / 滑窗策略
4. 回环如何插入因子
5. 多线程如何保证地图数据结构一致性（读写锁、拷贝局部地图）

推荐阅读顺序：

1. 小型 Pose Graph 示例（g2o / GTSAM tutorials）
2. ORB-SLAM3 的 Optimizer::BundleAdjustment 与 Loop Closing
3. VINS-Fusion 的边缘化（理解先验如何形成）

真机调试协议（可直接照做）

1. 静止 10 秒：位姿抖动是否被协方差解释；若静止仍漂移，查时间戳/标定
2. 纯平移往返：终点闭合误差，验证尺度与轮速/IMU
3. 纯旋转原地：地图点数量应下降，不应凭空「创造深度」
4. 故意遮挡 1–2 秒：恢复靠短时模型还是重定位
5. 走 8 字激发 IMU：为 VIO 初始化做准备
6. 回到起点：有回环时应看到位姿图突变后收敛，而不是慢慢漂回去

每次只改一个变量（曝光、外参、噪声、关键帧策略），并保存 bag + 配置哈希。

小结

深层 SLAM 能力 = 正确的状态定义 + 可观的运动激励 + 诚实的协方差 + 稳健的回环 + 可复现的评测。

模块名词背得再熟，如果不能解释「这个因子把信息矩阵的哪一块填上了、秩差多少」，在真机上仍会反复踩坑。

延伸阅读建议：下一篇专门拆 VIO 的预积分、bias 随机游走与初始化可观性条件。

参考

尺度、外参、时间其中一项不对，后端再漂亮也像在拟合噪声。

• Kaess et al., iSAM / factor graphs
• Triggs et al., Bundle Adjustment — A Modern Synthesis
• Mur-Artal & Tardós, ORB-SLAM / ORB-SLAM2 / ORB-SLAM3
• Sturm et al., A Benchmark for the Evaluation of RGB-D SLAM Systems (ATE/RPE)