前端跑完了，我们有了相机的初步位姿估计和一批3D路标点。但这些估计都有误差，而且误差会随着时间一点点累积。后端的工作就是把这些误差消除掉，得到全局一致的位姿和地图。这个过程就像是你画了一张地图，发现有些地方对不上，需要回头修正一样。
后端最核心的工具就是Bundle Adjustment（BA），中文叫"光束法平差"。这个名字听起来很学术，来源于摄影测量学——调整光束（光线）的方向和位置使得投影误差最小。但原理其实很直观。面试的时候BA是必考的后端知识，你得能讲清楚它的数学原理和工程实现。
BA在做什么
BA做的事情可以用一句话概括：同时优化所有相机的位姿和所有路标点的位置，使得所有观测的重投影误差之和最小。这里的"同时"很关键——不是先优化位姿再优化路标点，而是两者一起优化，互相约束。
什么是重投影误差？假设一个3D路标点的真实位置是P，相机在某时刻的位姿是T。根据T把P投影到图像上，得到一个预测的像素位置p_pred。而实际观测到的像素位置是p_obs。两者之间的差异就是重投影误差。
# 重投影误差的计算 def reprojection_error(T, P, p_obs, K): p_pred = K @ T @ P # 投影到图像 p_pred = p_pred[:2] / p_pred[2] # 归一化 return np.linalg.norm(p_pred - p_obs)BA的目标函数就是所有观测的重投影误差的平方和：
min Σᵢⱼ ||π(Tᵢ, Pⱼ) - pᵢⱼ||²
Tᵢ是第i个相机的位姿，Pⱼ是第j个路标点，pᵢⱼ是第i帧观测到第j个路标点的像素坐标，π是投影函数。
BA的数学结构
BA是一个非线性最小二乘问题。求解方法通常是Gauss-Newton法或Levenberg-Marquardt法。
Gauss-Newton法的思路是：在当前估计值处做一阶泰勒展开，把非线性问题近似成线性最小二乘问题，求解增量，然后更新估计值。反复迭代直到收敛。GN法的优点是收敛速度快（二阶收敛），缺点是需要海塞矩阵正定，否则增量方向可能不对。
Levenberg-Marquardt法（LM法）是GN法的改进版。它在海塞矩阵的对角线上加一个阻尼因子λ：(H + λI)Δx = b。λ大的时候，LM接近梯度下降法（稳定但慢）；λ小的时候，LM接近GN法（快但可能不稳定）。LM法在迭代过程中自动调整λ，兼顾了稳定性和速度。实际工程中BA几乎都用LM法。
# LM法的核心逻辑 lam = 0.001 # 初始阻尼因子 for iteration in range(max_iter): J = compute_jacobian(poses, landmarks) r = compute_residuals(poses, landmarks) H = J.T @ J b = J.T @ r delta = solve(H + lam * np.eye(len(b)), -b) new_cost = compute_cost(poses + delta) if new_cost < current_cost: poses += delta # 接受更新 lam *= 0.5 # 减小阻尼 else: lam *= 2.0 # 增大阻尼，重试关键的一点是BA的雅可比矩阵有特殊的稀疏结构。每个观测只涉及一个位姿和一个路标点，所以雅可比矩阵的大部分元素是零。对应的海塞矩阵也是稀疏的，而且有一种特殊的块结构。
利用这个稀疏结构，可以用Schur补来加速求解。具体来说，先把路标点的增量用位姿的增量表示出来（因为路标点之间没有直接约束），然后代入位姿的方程。这样就把一个大的线性系统简化为只包含位姿的小系统。
# Schur补的直觉 # 海塞矩阵的块结构： # H = [B E] B: 位姿-位姿块 # [Eᵀ C] C: 路标-路标块 # Schur补后：(B - E C⁻¹ Eᵀ) Δx_pose = ... # 只需要求解位姿的增量，路标点的增量可以回代得到Schur补是BA能高效求解的关键。没有Schur补技术，BA的计算量会大得多。
全量BA vs 增量BA
全量BA优化所有历史帧的位姿和所有路标点。精度最高，但计算量随时间增长。地图大了之后，全量BA跑不动。
增量BA只优化最近几帧的位姿和它们观测到的路标点。计算量恒定，可以实时运行。ORB-SLAM2的局部BA就是增量BA，只优化当前关键帧和共视关键帧。
实际中通常的做法是：前端用PnP做实时跟踪，后端定期做增量BA来消除局部误差，回环检测触发后做全量BA（或位姿图优化）来消除全局累积误差。
面试中的高频追问
"BA和PnP有什么区别？" PnP只优化当前帧的位姿，路标点位置固定。BA同时优化位姿和路标点。PnP是单帧优化，BA是多帧联合优化。BA的精度更高，但计算量也大得多。
"BA的稀疏性怎么利用？" 每个观测只涉及一个位姿和一个路标点，雅可比矩阵是稀疏的。用Schur补消去路标点变量后，剩下的位姿方程也是稀疏的（因为每个位姿只和少数几个其他位姿有共视关系）。用稀疏矩阵的Cholesky分解可以高效求解。
"BA一定收敛到全局最优吗？" 不一定。BA本质上是非凸优化，可能收敛到局部最优。但如果初始值足够好（前端给出的位姿估计不太差），通常能收敛到全局最优附近。这就是为什么前端的质量对后端很重要。
"BA的复杂度是多少？" 全量BA的复杂度大约是O(n²m)，n是位姿数，m是路标点数。用Schur补和稀疏技巧之后可以降低到接近O(n)。增量BA的复杂度是常数级别的。
"自动求导和手动求导有什么区别？" 自动求导（Ceres用的方式）通过计算图来自动算雅可比矩阵，不需要你手动推公式。优点是方便、不容易出错。缺点是计算效率比手动推导的解析雅可比稍低。在SLAM里，如果追求极致性能，通常会手动推导雅可比矩阵。Ceres也支持手动指定雅可比函数。
"BA的鲁棒核函数是什么？" 重投影误差中可能有外点（错误的匹配），外点的误差会很大，影响优化结果。鲁棒核函数（如Huber核、Cauchy核）会限制大误差的影响，让BA对外点不那么敏感。Huber核在误差小于阈值时是平方损失，大于阈值时变成线性损失，这样大误差不会被过度放大。
工程实现
实际做BA不需要从零写。常用的工具：
Ceres Solver：Google开源的非线性优化库，功能强大，文档齐全。支持自动求导，你只需要定义残差函数，雅可比矩阵它帮你算。
g2o：专门为SLAM设计的图优化框架。用图的方式建模优化问题，支持多种BA和位姿图优化的实现。
gtsam：Georgia Tech开发的因子图优化库。用因子图来描述优化问题，API设计优雅。在机器人领域越来越流行。
这三个工具的选择取决于你的需求。Ceres求解器最通用，适合各种非线性优化问题。g2o在SLAM社区用得最多，教程也最多。gtsam的因子图建模方式则更灵活，适合复杂的多传感器融合场景。
# Ceres Solver做BA的简化示例 problem = ceres.Problem() for obs in observations: cost = ReprojectionError(obs.point2d, obs.camera_K) problem.AddResidualBlock(cost, None, pose, landmark) options = ceres.SolverOptions() options.linear_solver_type = ceres.SCHUR ceres.Solve(options, problem, summary)实际项目中，BA的调用频率要根据计算资源来定。ORB-SLAM2每插入一个关键帧就做一次局部BA，每次优化20-30帧。如果你的嵌入式平台算力有限，可以降低BA的频率，比如每5个关键帧做一次。总之要在精度和实时性之间找到平衡点。
BA是SLAM后端的基石，也是面试中最常被问到的后端知识。从目标函数的定义到稀疏性的利用，从GN法到LM法，从全量BA到增量BA，每个环节你都要理解清楚。理解了BA的原理，后面学g2o和GTSAM就水到渠成了。
上一篇：第243篇 视觉SLAM前端之直接法——不用特征点的另一条路
下一篇我们聊g2o框架——图优化的工程实现，看看怎么把BA问题建模成图优化问题并高效求解。
如果这篇文章对你有帮助，欢迎点赞支持一下，你的鼓励是我持续更新的动力！
