前端跑完了我们有了相机的初步位姿估计和一批3D路标点。但这些估计都有误差而且误差会随着时间一点点累积。后端的工作就是把这些误差消除掉得到全局一致的位姿和地图。这个过程就像是你画了一张地图发现有些地方对不上需要回头修正一样。后端最核心的工具就是Bundle AdjustmentBA中文叫光束法平差。这个名字听起来很学术来源于摄影测量学——调整光束光线的方向和位置使得投影误差最小。但原理其实很直观。面试的时候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 SolverGoogle开源的非线性优化库功能强大文档齐全。支持自动求导你只需要定义残差函数雅可比矩阵它帮你算。g2o专门为SLAM设计的图优化框架。用图的方式建模优化问题支持多种BA和位姿图优化的实现。gtsamGeorgia 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问题建模成图优化问题并高效求解。如果这篇文章对你有帮助欢迎点赞支持一下你的鼓励是我持续更新的动力
`)
