SLAM 学习笔记（四）后端（KF BA）

一、为什么要有"后端"？（章首引入）

前面你学的前端（VO / 特征跟踪 / PnP / ICP）做的是：

✅ 根据相邻帧图像 ，估计相机的相对运动

特点是：

局部（只关心当前帧 ↔ 上一帧 / 关键帧）

递推（每帧算一次）

噪声会累积 → 漂移（drift）

问题是：

前端一旦有小误差

乘上时间 → 轨迹慢慢歪掉

无法利用历史信息和回环

👉 后端（Backend / Optimization）的作用：

把前端产生的：

相机位姿估计

landmark（三维点）观测

统一放到一个概率 / 优化框架

联合优化 所有位姿 + 所有地图点

使结果：

全局一致

对噪声鲁棒

可消除累积误差（回环）

一句话概括引入段：

前端给出"初值"，后端通过批量优化得到"最优解"

SLAM 后端的三层抽象

这一节本质是帮你建立三个认知层次：
复制代码
(1) 状态估计的概率解释   ← MAP
(2) 线性系统 + KF        ← 特殊情况
(3) 非线性系统 + EKF     ← 过渡
        ↓
(下一节) BA / 图优化      ← 实际用的
下面逐个小节细说。

10.1.1 状态估计的概率解释（⭐最重要）

1️⃣ SLAM 状态定义

通常定义系统状态：
复制代码
xk=[Tk, p1,…,pm]T
Tk：第 k 时刻相机位姿（SE(3)）

pi：第 i 个路标（landmark，3D点）

观测：
复制代码
zk,j=h(Tk,pj)+noise
例如：

h= 相机投影模型

观测 = 像素坐标

2️⃣ 核心问题（贝叶斯视角）

已知所有观测 {z1,...,zn}，求：
复制代码
x^=argxmaxp(x∣z)
用贝叶斯公式：
复制代码
p(x∣z)=p(z)p(z∣x)p(x)
p(z∣x)：似然（Likelihood）

"如果世界是 x，看到 z 有多可能"

p(x)：先验（Prior）

比如运动模型预测

忽略分母（与 x 无关），最大化：
复制代码
x^=argmaxp(z∣x)p(x)
3️⃣ 取负对数 → 最小二乘

假设：

观测噪声 ∼ N(0, Σ)

运动噪声 ∼ N(0, R)

取 −ln，乘法变加法，指数变平方和：
复制代码
x∗=argmin(∥z−h(x)∥Σ2+∥x−x^pred∥R2)
✅ 这就是 SLAM 后端优化的标准形式！

🔑 你要记住的结论（考试 / 面试都爱问）

SLAM 后端 = 最大后验估计（MAP）

MAP 在 Gaussian noise 假设下 = 非线性最小二乘

BA 就是这种形式的具体实例（重投影误差）

10.1.2 线性系统和 KF

现在假设：

运动是线性的：

xk+1=Fkxk+wk

观测是线性的：

zk=Hkxk+vk

噪声均为高斯

→ 得到 卡尔曼滤波（KF）

KF 两大步

预测（Predict）

用运动模型预测状态和协方差

更新（Update）

用新观测修正状态

特点：

✅ 递归、实时

✅ 最优（在线性 + 高斯前提下）

❌ 相机投影 不是线性，不能直接用 KF

👉 所以这节是铺垫理论，不是视觉 SLAM 主流程。

10.1.3 非线性系统和 EKF

现实中：

运动可能是非线性

观测一定是非线性（相机投影 π(T·P)）

EKF 做法：

在当前估计值处做 一阶泰勒展开

局部线性化

套用 KF 框架

即：
复制代码
z=h(x)+J(x−x^)+⋯
EKF SLAM：

状态向量包含：

所有相机位姿

所有 landmark

协方差矩阵：dim × dim（巨大）

EKF 的讨论（高博重点吐槽）

为什么现代视觉 SLAM 几乎不用 EKF？

问题说明

线性化误差大相机模型非线性强，一阶近似差

协方差维度爆炸 O((n+m)²)，无法存、算

强依赖初值线性化点不好 → 发散

难处理回环需频繁增删状态

全局一致性差仍是递推，不是真正的全局 BA

✅ 结论：

EKF 适合 小尺度 / 状态少 / 有 IMU 的 VIO

纯视觉 SLAM（PTAM / ORB-SLAM）→ BA + 图优化

问题	说明
线性化误差大	相机模型非线性强，一阶近似差
协方差维度爆炸	O((n+m)²)，无法存、算
强依赖初值	线性化点不好 → 发散
难处理回环	需频繁增删状态
全局一致性差	仍是递推，不是真正的全局 BA

三、这一块你该怎么学（实操建议）

✅ 理解层面

能口述：

"SLAM 后端是 MAP 估计，在高斯噪声下化成非线性最小二乘"

知道 KF → EKF → BA 的历史演进逻辑

✅ 不要求

不要求你现在能完整推导 KF 五个公式

不要求背 EKF 雅可比形式

✅ 自测问题

为什么前端漂移，后端能减小？

MAP 和最小二乘是什么关系？

EKF 为什么在大规模 SLAM 中被淘汰？

BA 属于上面哪一层抽象？

① 为什么前端会漂移，而后端能减小漂移？

✅ 前端为什么会漂？

前端（VO）通常是增量式/递推式：
复制代码
Tk=Tk−1⋅ΔTk−1→k
每一帧只估计 相对运动

相对运动带有 噪声（特征误配、像素噪声、光照）

误差会 不断相乘累积

👉 结果：

即使每个 ΔT 只偏一点点，长时间下轨迹会越来越歪

→ 这就是 积分漂移（drift）

而且前端：

❌ 只用最近帧/关键帧

❌ 不知道"同一个点之前见过"

✅ 后端为什么能减小漂移？

后端（BA / 图优化）做的是：

把所有帧、所有地图点、所有观测放在一起，统一优化

假设你第 1 帧和第 100 帧看到了 同一个路标：

前端：根本不利用这个信息

后端：

第 1 帧 → 观测该点

第 100 帧 → 也观测该点

BA 会强制：这两个位姿必须同时满足该点的投影

数学上：

前端：链式估计（局部）

后端：闭回路约束（全局）

如果还有 回环（Loop Closure）：

后端直接"把错掉的轨迹掰回来"

前端永远做不到这一点

📌 一句话总结：

**前端漂移是因为"只看局部、递推累加误差"；

后端减小漂移是因为"利用所有观测 + 回路约束，做全局最小二乘拟合"。**

② MAP 和最小二乘是什么关系？

这是 10.1.1 的核心结论。

1️⃣ MAP（最大后验估计）
复制代码
x^=argxmaxp(x∣z)=argxmaxp(z∣x)p(x)
2️⃣ 假设高斯噪声

观测噪声：

z=h(x)+ε, ε∼N(0,Σ)

运动/先验噪声：N(0,R)

那么：
复制代码
p(z∣x)∝exp(−21∥z−h(x)∥Σ2)
复制代码
p(x)∝exp(−21∥x−xpred∥R2)
3️⃣ 取负对数
复制代码
x^=argmin(∥z−h(x)∥Σ2+∥x−xpred∥R2)
✅ 这就是加权非线性最小二乘！

📌 结论：

MAP 是概率意义上的"最优估计"

在高斯噪声假设下，MAP ⇔ 非线性最小二乘

SLAM 后端（BA）就是在解这个最小二乘问题

（如果无先验、只优化观测，就是普通 LS；加先验 = MAP）

③ EKF 为什么在大规模视觉 SLAM 中被淘汰？

再给你一个结构化、面试可直接背版的回答：

❌ 问题 1：状态维数爆炸

EKF SLAM 状态：
复制代码
x=[相机位姿1...n, landmark1...m]
协方差矩阵：
复制代码
Σ∈R(6n+3m)×(6n+3m)
存储：O((n+m)²)

更新：O((n+m)³)

实际根本存不下、算不动

❌ 问题 2：线性化误差大

EKF 在相机状态处对 投影函数做一阶泰勒

相机模型强非线性

线性化点稍偏 → 误差大 → 滤波发散

❌ 问题 3：强依赖初值

EKF 是递推的

若早期线性化不准 → 后续修正困难

BA 可以多次迭代、重新线性化

❌ 问题 4：难处理回环 & 动态结构

增加 / 删除 landmark 要改整个协方差

回环需要全局约束，EKF 只能"局部更新"

✅ 现代视觉 SLAM（PTAM / ORB-SLAM）：

用 关键frame + BA + 滑窗 / 位姿图

EKF 主要留在 VIO（IMU+视觉），不在大规模纯视觉 BA

📌 一句话：

EKF 在小型/状态少时尚可；在大规模、非线性强、需回环的视觉 SLAM 中，效率和精度都不如 BA。

④ BA 属于上面哪一层抽象？

回顾 10.1 的三层：

✅ 状态估计的概率解释（MAP）

➖ 线性系统 + KF（特例）

➖ 非线性 + EKF（近似方法）

✅ BA（Bundle Adjustment）属于第 1 层的具体实现：
把 MAP 写成：
复制代码
Ti,Pjmin∑∥uij−π(Ti,Pj)∥2
假设高斯噪声 → MAP = 非线性最小二乘

用 图优化 + LM / Gauss--Newton 求解
📌 标准说法：

BA 是在 MAP（最大后验估计）框架下，以重投影误差为残差的非线性最小二乘问题，是 SLAM 后端的主流实现方式。

✅ 超简总结版（你可以默念这句）

前端漂：递推累加噪声，只看局部

后端减漂：全局约束 + 回环，统一优化

MAP = 最小二乘（高斯噪声下取负对数）

EKF 淘汰原因：维数爆炸 + 线性化误差 + 无全局回环

BA = MAP 框架下的具体非线性最小二乘问题

10.2 BA 与图优化

先给一张全局地图，你看完再回头对照：
复制代码
BA 问题来源（重投影误差）
        ↓
构建最小二乘 ⇒ 图模型（节点 + 边）
        ↓
求解：Gauss--Newton / LM
        ↓
Hessian 矩阵具有稀疏性
        ↓
用 Schur 补边缘化地图点 ⇒ 加速
        ↓
加 Robust Kernel ⇒ 抗外点
10.2.1 投影模型与 BA 代价函数

1️⃣ BA 要解决什么问题？

前端给你：

若干相机位姿 T1,...,Tm

若干地图点 P1,...,Pn

观测：第 i 个相机看到第 j 个点 → 像素坐标 uij

BA（Bundle Adjustment）同时优化：

所有相机位姿 + 所有三维点

使它们最好地解释所有观测

2️⃣ 相机投影模型

对第 i 个相机、第 j 个地图点：
复制代码
u^ij=π(TiPj)=K⋅[Ri∣ti]⋅Pj(齐次→像素)
Ti∈SE(3)

Pj∈R3

3️⃣ 重投影误差（核心）

观测像素 uij与预测像素 u^ij之差：
复制代码
eij(Ti,Pj)=uij−π(TiPj)
BA 代价函数：
复制代码
Ti,Pjmin(i,j)∈O∑eij(Ti,Pj)Σij2
O：有效观测集合

通常 Σ=I（等权）→ 普通欧氏距离平方

🔑 你要建立的认知

✅ BA = 非线性最小二乘

✅ 残差 = 2D 重投影误差

✅ 待优化变量：

位姿（6DoF each）

点（3DoF each）

👉 这和 10.1.1 说的 MAP → 最小二乘 完全对应

10.2.2 BA 的求解（非线性最小二乘）

因为 π(T,P)是非线性函数 ⇒ 不能闭式求解

常用方法：

Gauss--Newton（GN）

Levenberg--Marquardt（LM，更常用）

迭代形式（简化）

对当前估计 x：
复制代码
(JTJ+λI)Δx=−JTe(x)
J：整体雅可比（对位姿 + 点对误差）

Δx：更新量

LM 加阻尼 λ 保证稳定收敛

雅可比说明（很重要但先不深推）

JTi：误差对 李代数 ξ（位姿扰动）

JPj：误差对 三维点坐标

后面 g2o / Ceres 会自动求，但你要知道：

每条边只连接 1 个位姿节点 + 1 个点节点

这点直接决定下一个小节的稀疏性。

稀疏性和边缘化（⭐整章最关键）

这是 为什么 BA 能在万级变量下实时跑 的根本原因。

1️⃣ Hessian 矩阵的稀疏结构

BA 的二次型近似：
复制代码
F(x)≈21ΔxTHΔx+gTΔx
其中：
H=JTJ
变量顺序：
复制代码
x=[位姿T1,…,Tm,点P1,…,Pn]
2️⃣ 为什么 H 是稀疏的？

每条重投影误差：

只依赖 一个相机 Ti

只依赖 一个点 Pj

⇒ 每个误差项只在 H 中产生：

HTi,Ti

HPj,Pj

HTi,Pj和 HPj,Ti

➡️ 没有 Ti--Tk（k≠i）直接耦合

➡️ 没有 Pi--Pj（j≠i）直接耦合

画成图：

位姿--位姿之间：无直接边

点--点之间：无直接边

位姿--点之间：有边（当且仅当被观测）

👉 这就是 稀疏性（Sparsity）

3️⃣ 分块写 H
复制代码
H=[HccHpcHcpHpp]
Hcc：相机--相机（块对角 / 稀疏）

Hpp：点--点（对角！因为每个点只被若干相机看）

4️⃣ Schur 补（边缘化地图点）

对线性系统：
复制代码
[HccHpcHcpHpp][ΔTΔP]=[bcbp]
先消元 ΔP：
复制代码
(Hcc−HcpHpp−1Hpc)ΔT=bc−HcpHpp−1bp
记：
复制代码
H~=Hcc−HcpHpp−1Hpc
Hpp是对角 ⇒ 逆很容易求

先解 只有位姿的缩简系统

再回代求 ΔP

✅ 好处：

未知数从 (6m+3n)→ 6m

大幅降低计算量

现代 BA（g2o / Ceres / gtsam）都这么干

🔑 一句人话总结

因为 BA 图中每个误差只连一个位姿和一个点，Hessian 高度稀疏；通过舒尔补边缘化掉地图点，只需先解一个更小的位姿系统，从而高效求解。

（ORB-SLAM / PTAM 的 BA 能实时，全靠这个）

① BA 的残差物理意义是什么？

BA 残差是：观测像素 ↔ 当前模型预测像素之间的距离
复制代码
eij=uijmeasured−π(Ti,Pj)
物理意义（人话）：

**假如世界中有这个点 Pj、相机在位姿 Ti，它"应该"投到像素 u^；

实际图像中检测到它在 u；

两者差多少------这个差就是重投影误差。**

若 BA 优化完美 ⇒ 所有重投影误差 ≈ 0

→ 同一三维点在不同帧中严丝合缝地解释所有观测

残差单位：像素

📌 一句话背诵：

BA 残差是"当前位姿+点预测出的像素位置"与"真实图像观测像素"之间的偏差（重投影误差）。

② 为什么 BA 的 Hessian 矩阵是稀疏的？

核心原因只有一个：

每条重投影误差只依赖于【一个相机位姿 + 一个地图点】

推导直觉

BA 的误差项：
复制代码
eij(Ti,Pj)
不含 Tk (k=i)

不含 Pl (l=j)

对 Hessian H=JTJ：

∂Tk∂eij=0仅当 k=i

∂Pl∂eij=0仅当 l=j

⇒ 每个误差项只在 H 中填充：

HTi,Ti

HPj,Pj

HTi,Pj与 HPj,Ti

因此：

不同位姿之间无直接耦合

不同点之间无直接耦合

只有"看到该点的相机 ↔ 该点"之间有非零块

画成图：
复制代码
[相机] ------ [点] ------ [相机]
    \            /
     (不直接相连)
📌 一句话：

BA 图的每个边只连接一个位姿节点和一个路标点节点，导致 Jacobian 列不重叠，Hessian 呈块稀疏结构。

③ Schur 补边缘化掉的是什么？为什么要这么做？

✅ 边缘化掉的是：地图点（landmark）变量

BA 变量：
复制代码
x=[T1…Tm, P1…Pn]
线性化系统：
复制代码
[HccHpcHcpHpp][ΔTΔP]=[bcbp]
通过 Schur 补：

先消去 ΔP

得到仅关于位姿的系统：
复制代码
(Hcc−HcpHpp−1Hpc)ΔT=bc−HcpHpp−1bp
✅ 为什么要这么做？

降维

原变量：6m+3n（n 常几千～几万）

消元后：6m（m 远小于 n）

Hpp是对角阵

每个地图点只被少数相机观测

求逆极快（逐元素取倒数）

数值稳定 + 快

避免直接解超大稠密方程组

是现代 BA 能实时的根本原因

📌 一句话：

Schur 补边缘化掉地图点变量，把 BA 化为一个规模更小、只关于位姿的线性系统；因 Hpp 对角，消元高效，从而大幅加速求解。

（ORB‑SLAM / PTAM / g2o / Ceres 全靠这一步）

④ 鲁棒核解决的是前端还是后端的问题？

✅ 它是后端优化中的机制，用来弥补前端不可避免的缺陷

更精确地说：

问题来源：前端

特征误匹配

动态物体

重复纹理 / 遮挡

→ 产生 外点（Outlier）观测

解决位置：后端 BA

不用原始 ∥e∥2

改用 ρ(e)（Huber / Cauchy）

抑制大残差对整体优化的过度影响

所以严谨说法是：

鲁棒核函数在后端使用，用来抵抗前端引入的外点（误匹配）对非线性优化的破坏。

📌 面试最简版：

外点来自前端，鲁棒核作用在后端。

它们解决的是什么问题？

你刚学的 BA 最终变成这个要求：

**给我一堆未知量（位姿、点），

给我一堆误差函数（重投影误差），

帮我找到让所有误差平方和最小的那个解**

这个问题叫：

👉 非线性最小二乘优化（Nonlinear Least Squares）

g2o 和 Ceres 就是帮你解这个问题的优化库。

你自己写 Gauss--Newton / LM 很麻烦：

求雅可比

组装 H 矩阵

处理稀疏性

迭代、收敛判断

→ 这些库全帮你做了，你只负责"描述问题"。

二、用一个生活例子类比------"调家具位置"

想象你在布置客厅：

有几把椅子（地图点）

有相机/你站的位置（位姿）

你从不同角度"看"椅子，记下看到的角度（观测）

但所有椅子和你站的位置都稍微不对劲，

你想：

微调椅子和你的站位，让所有'看起来的角度'最符合记录

这就是 BA。

g2o / Ceres = 一个专业装修调整团队：

你告诉它：

有哪些东西可动（椅子、站位）

每条"我看它应该长这样"的规则（误差）

它自动帮你一点点拧、推、转，直到整体最协调

三、g2o 是什么？（Graph Optimization）

g2o = General Graph Optimization

🔹 它怎么看问题？

**节点（Vertex）** → 待优化的东西

相机位姿

三维点

**边（Edge）** → 误差/约束

重投影误差（一个相机看到一个点的观测）

图结构：
复制代码
[位姿节点] ──边── [点节点]
你说：

"这个边连这两个节点，误差函数长这样"

g2o 就帮你：

建图

算雅可比

利用稀疏性（Schur 补）

LM 迭代求解

🔹 g2o 适合干啥？

✅ SLAM / BA / 位姿图（Pose Graph）

✅ 问题天然是"图"结构（节点 + 边）

📌 一句话：

g2o 是你告诉它"图里有啥节点、啥边、误差怎么算"，它帮你把整张图调到最优。

四、Ceres Solver 是什么？（通用非线性最小二乘）

Ceres 是 Google 出的通用数值优化库。

🔹 它怎么看问题？

不强调"图"，而是：

ParameterBlock → 待优化变量（位姿、点）

ResidualBlock → 残差函数（重投影误差）

LossFunction → 鲁棒核（Huber 等）

你说：
复制代码
我有这些变量
每个残差 = f(变量)
我想最小化 Σ‖残差‖²
Ceres 自动：

自动求导 / 数值求导 / 你给解析导

Trust Region（LM）

稀疏求解（可配）

🔹 Ceres 适合干啥？

✅ BA

✅ 标定（相机 / IMU / 手眼）

✅ 任意自定义非线性最小二乘问题

📌 一句话：

Ceres 是你定义"变量 + 残差函数"，它帮你最小化残差平方和，不局限于图结构。

五、g2o vs Ceres ------ 一张表看懂区别

维度 g2o Ceres

设计理念图优化框架通用非线性最小二乘

建模方式 Vertex + Edge（图） ParameterBlock + ResidualBlock

SLAM 贴合度 ⭐⭐⭐⭐（BA / Pose Graph 天然） ⭐⭐⭐⭐（也很常用）

自动求导 ❌（一般手写/数值） ✅（非常强）

学习曲线稍陡（要懂顶点/边继承）相对平滑

工业使用学术界/早期 SLAM Google / 工业界广泛

👉 视觉 SLAM 十四讲示例两个都给了：

g2o → 强调图模型、稀疏 BA

Ceres → 强调自动求导、更现代写法

六、结合你正在看的 BA，一句话定位

**BA 是数学问题（最小化重投影误差）**

g2o / Ceres 是帮你解 BA 的工具

你只要告诉它们：

✅ 哪些量是待优化的

✅ 每个误差怎么算

✅ 是否加鲁棒核

它们负责 Jacobian、Hessian、稀疏求解、迭代

维度	g2o	Ceres
设计理念	图优化框架	通用非线性最小二乘
建模方式	Vertex + Edge（图）	ParameterBlock + ResidualBlock
SLAM 贴合度	⭐⭐⭐⭐（BA / Pose Graph 天然）	⭐⭐⭐⭐（也很常用）
自动求导	❌（一般手写/数值）	✅（非常强）
学习曲线	稍陡（要懂顶点/边继承）	相对平滑
工业使用	学术界/早期 SLAM	Google / 工业界广泛

SLAM 学习笔记（四）后端（KF BA）

一、为什么要有"后端"？（章首引入）

SLAM 后端的三层抽象

10.1.1 状态估计的概率解释（⭐最重要）

1️⃣ SLAM 状态定义

2️⃣ 核心问题（贝叶斯视角）

3️⃣ 取负对数 → 最小二乘

🔑 你要记住的结论（考试 / 面试都爱问）

10.1.2 线性系统和 KF

KF 两大步

10.1.3 非线性系统和 EKF

EKF 的讨论（高博重点吐槽）

三、这一块你该怎么学（实操建议）

① 为什么前端会漂移，而后端能减小漂移？

✅ 前端为什么会漂？

✅ 后端为什么能减小漂移？

② MAP 和最小二乘是什么关系？

1️⃣ MAP（最大后验估计）

2️⃣ 假设高斯噪声

3️⃣ 取负对数

③ EKF 为什么在大规模视觉 SLAM 中被淘汰？

❌ 问题 1：状态维数爆炸

❌ 问题 2：线性化误差大

❌ 问题 3：强依赖初值

❌ 问题 4：难处理回环 & 动态结构

④ BA 属于上面哪一层抽象？

✅ 超简总结版（你可以默念这句）

10.2 BA 与图优化

10.2.1 投影模型与 BA 代价函数

1️⃣ BA 要解决什么问题？

2️⃣ 相机投影模型

3️⃣ 重投影误差（核心）

🔑 你要建立的认知

10.2.2 BA 的求解（非线性最小二乘）

迭代形式（简化）

雅可比说明（很重要但先不深推）

稀疏性和边缘化（⭐整章最关键）

1️⃣ Hessian 矩阵的稀疏结构

2️⃣ 为什么 H 是稀疏的？

3️⃣ 分块写 H

4️⃣ Schur 补（边缘化地图点）

🔑 一句人话总结

① BA 的残差物理意义是什么？

物理意义（人话）：

② 为什么 BA 的 Hessian 矩阵是稀疏的？

推导直觉

③ Schur 补边缘化掉的是什么？为什么要这么做？

✅ 边缘化掉的是：地图点（landmark）变量

✅ 为什么要这么做？

④ 鲁棒核解决的是前端还是后端的问题？

它们解决的是什么问题？

二、用一个生活例子类比------"调家具位置"

三、g2o 是什么？（Graph Optimization）

🔹 它怎么看问题？

🔹 g2o 适合干啥？

四、Ceres Solver 是什么？（通用非线性最小二乘）

🔹 它怎么看问题？

🔹 Ceres 适合干啥？

五、g2o vs Ceres ------ 一张表看懂区别

六、结合你正在看的 BA，一句话定位