1. Bundle AdjustmentBA是什么从“捆”与“调”说起如果你玩过摄影尤其是尝试过用多张照片重建三维场景那你一定遇到过这样的困境单张照片的相机位置估得不太准三维点云看起来有点飘整体模型总觉得哪里“拧巴”着。Bundle Adjustment中文常译作“光束法平差”或“捆绑调整”就是解决这个“拧巴”问题的终极利器。它不是一个具体的算法而是一个将三维结构场景点和相机参数位置、姿态、内参联合起来进行全局最优化的数学模型与框架。你可以把它想象成一次对所有观测数据的“大清算”和“总校对”。为什么叫“捆绑调整”这里的“Bundle”指的就是从三维空间点发射到各个相机光心所形成的“光束”。在三维重建中一个空间点会被多个相机看到形成多条观测光线光束。BA的核心思想就是调整所有相机的位置姿态和所有三维空间点的位置使得这些重新计算出来的“光束”能够最好地“穿过”图像上实际观测到的像素点。换句话说就是让理论投影和实际观测之间的误差总和降到最低。这个过程就像同时拉扯一张由许多橡皮筋光束连接的网络相机和三维点直到整个网络达到一个整体最协调、受力最均衡的状态。它几乎是所有现代视觉SLAM同步定位与建图、运动恢复结构SfM和摄影测量系统的核心收尾与优化步骤其优化效果直接决定了最终三维模型的精度和一致性。2. BA的数学模型误差从何而来又去向何处要理解BA必须深入其数学模型的核心。BA本质上是一个大规模的非线性最小二乘优化问题。它的目标函数非常直观但背后的计算却相当复杂。2.1 重投影误差优化的基石BA优化的目标是最小化重投影误差。什么是重投影误差我们用一个简单的例子来说明假设我们有一个估计的三维点 ( P_j [X_j, Y_j, Z_j]^T )和一个估计的相机这个相机有自己的旋转矩阵 ( R_i )、平移向量 ( t_i ) 和内参矩阵 ( K_i )。我们用这个相机参数将三维点 ( P_j ) 投影到图像平面上得到一个理论像素坐标 ( \hat{u}{ij} )。同时我们在第 ( i ) 张图像上实际观测到这个点其像素坐标为 ( u{ij} )。那么重投影误差就是这两个坐标之间的欧氏距离[ e_{ij} u_{ij} - \pi(K_i (R_i P_j t_i)) ]其中( \pi ) 表示从相机坐标系到像素坐标系的投影函数包含除法以处理透视投影。对于所有相机 ( i ) 和所有被观测到的三维点 ( j )BA的目标就是找到一组最优的相机参数 ( {R_i, t_i, K_i} ) 和三维点坐标 ( {P_j} )使得所有重投影误差的平方和最小[ \min_{{R_i, t_i, K_i}, {P_j}} \sum_{i1}^{m} \sum_{j1}^{n} | e_{ij} |^2 ]这里 ( m ) 是相机数( n ) 是三维点数。这个求和符号清晰地揭示了BA的规模一个典型的SfM项目可能有成百上千张图像和数百万个三维点这意味着优化变量未知数的规模可以达到数千万甚至上亿而观测方程的数量则是这个规模的数倍。2.2 非线性与线性化从高斯到列文伯格-马夸尔特目标函数 ( | e_{ij} |^2 ) 由于相机投影模型 ( \pi ) 和旋转矩阵 ( R ) 的存在是一个高度非线性的函数。我们无法直接求解其全局最小值。标准的做法是采用迭代优化算法从一组初始估计值开始逐步逼近最优解。最常用的方法是高斯-牛顿法及其稳健变种列文伯格-马夸尔特算法。其核心思想是局部线性化。在每次迭代中我们在当前参数估计值 ( x )这里 ( x ) 是一个庞大的向量包含了所有待优化的相机和三维点参数处对误差函数 ( e(x) ) 进行一阶泰勒展开[ e(x \Delta x) \approx e(x) J(x) \Delta x ]其中 ( J(x) ) 是误差函数 ( e ) 关于所有参数 ( x ) 的雅可比矩阵这是一个极其庞大但稀疏的矩阵。将线性化后的误差代入目标函数优化问题就变成了一个关于增量 ( \Delta x ) 的线性最小二乘问题[ \min_{\Delta x} | e(x) J(x) \Delta x |^2 ]求解这个方程即求解正规方程 ( J^T J \Delta x -J^T e )我们就能得到在当前点使得误差下降最快的参数更新方向 ( \Delta x )然后更新参数( x \leftarrow x \Delta x )。如此反复迭代直到收敛增量 ( \Delta x ) 足够小或误差下降不明显。注意这里的“稀疏”是BA能够高效求解的关键。一个三维点通常只被少数几个相机看到一个相机也只看到一部分三维点。因此雅可比矩阵 ( J ) 中绝大多数元素都是零。利用这种稀疏性可以极大地减少计算和存储开销使得优化大规模问题成为可能。2.3 参数化与流形旋转的特别处理在优化过程中我们需要特别小心地处理相机旋转参数。旋转矩阵 ( R ) 本身是正交矩阵有 ( R^T R I ) 且 ( \det(R) 1 ) 的约束。如果直接在9个矩阵元素上做优化很容易破坏这些约束导致优化后的“旋转矩阵”不再是一个有效的旋转。标准的做法是使用一种更紧凑、无约束的参数化方式例如李代数 so(3)三维向量或四元数。在每次迭代求解出增量 ( \Delta x ) 后对于旋转部分我们实际上求解的是在李代数空间中的增量 ( \delta \phi )一个三维向量。然后通过指数映射 ( R \leftarrow R \cdot \exp(\delta \phi^{\wedge}) ) 来更新旋转矩阵这样可以保证更新后的 ( R ) 始终落在旋转矩阵群 ( SO(3) ) 上。这个过程称为“在流形上进行优化”是处理带约束优化问题的优雅方式。3. BA的完整工作流程与实操要点理解了数学模型我们来看BA在实际的三维重建或SLAM系统中是如何被调用和执行的。它通常不是一个孤立的过程而是嵌入在一个完整的pipeline中。3.1 前端数据关联BA的“原料”准备BA的输入是什么不是原始图像而是已经建立好的观测数据。这包括图像特征点通常是SIFT、SURF、ORB等特征提取算法得到的像素坐标。特征匹配关系知道哪些图像中的哪些特征点对应的是同一个三维物理点。初始的相机位姿和三维点云这通常由前端流程提供例如通过对极几何、PnPPerspective-n-Point或视觉里程计初步计算得到。这些初始值可能误差较大但必须足够“靠谱”以保证BA能收敛到正确的局部最优解全局最优通常难以保证。实操心得前端的匹配质量直接决定了BA的成败。错误的匹配外点会像“毒药”一样污染整个优化过程导致结果完全错误。因此在送入BA之前必须进行严格的外点剔除常用的方法包括交叉验证双向匹配检查。几何验证利用基础矩阵或单应矩阵进行RANSAC剔除不符合几何约束的匹配对。时序或空间一致性检查在SLAM中可以利用运动连续性进行筛选。3.2 BA的构建与求解稀疏线性系统的求解有了初始值和观测数据我们就可以构建BA问题。现代BA库如ceres-solver, g2o, GTSAM为我们封装了大部分繁琐的步骤。我们需要做的是定义参数块告诉优化器哪些变量是相机位姿可能需要自定义李代数局部参数化哪些是三维点坐标哪些是相机内参焦距、主点、畸变系数等。添加残差块对于每一个观测图像i中的点j构建一个重投影误差残差项并将其与对应的相机参数块和三维点参数块关联起来。配置求解器选择优化算法如LM算法设置迭代次数、收敛阈值等。接下来求解器会自动完成我们之前描述的线性化、构建稀疏雅可比矩阵、求解增量方程的过程。求解大规模稀疏线性方程 ( J^T J \Delta x -J^T e ) 是计算的核心。这里通常使用舒尔消元技巧。由于三维点数量远多于相机数量我们可以先消去三维点变量得到一个只关于相机参数的、维度小得多的简化方程称为缩减相机系统求解完相机参数后再回代求解三维点。这能极大提升计算效率。一个简单的ceres-solver示例框架// 假设已有观测数据 observations 初始相机位姿 cameras 初始三维点 points ceres::Problem problem; for (const auto obs : observations) { int camera_id obs.camera_id; int point_id obs.point_id; double observed_x obs.x; double observed_y obs.y; ceres::CostFunction* cost_function SnavelyReprojectionError::Create(observed_x, observed_y); problem.AddResidualBlock(cost_function, nullptr, // 损失函数如Huber用于鲁棒性 cameras[camera_id].rotation, // 旋转参数块如四元数数组指针 cameras[camera_id].translation, // 平移参数块 cameras[camera_id].focal_length, // 内参焦距 cameras[camera_id].k1, cameras[camera_id].k2, // 畸变参数 points[point_id].data()); // 三维点坐标参数块 } // 为旋转参数设置局部参数化李代数 ceres::LocalParameterization* quaternion_local_parameterization ...; for (auto camera : cameras) { problem.SetParameterization(camera.rotation, quaternion_local_parameterization); } ceres::Solver::Options options; options.linear_solver_type ceres::SPARSE_SCHUR; // 使用稀疏舒尔求解器 options.minimizer_progress_to_stdout true; ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); std::cout summary.FullReport() \n;3.3 增量式BA与滑动窗口SLAM中的实时优化在离线SfM中我们可以对所有数据进行全局BA。但在在线SLAM系统中数据源源不断计算资源有限无法每次都做全局BA。因此增量式BA和滑动窗口BA成为关键技术。增量式BA当新的一帧图像到来时只优化与新帧相关的相机位姿和三维点以及与之有较强连接关系的部分旧变量而不是优化全部。这可以显著减少计算量。滑动窗口BA只维护一个固定大小的、最近的若干帧例如10-20帧作为一个局部窗口只对这个窗口内的变量进行BA优化。窗口随着机器人移动而滑动旧的帧被移出窗口其状态被固定或边缘化。边缘化是一种将旧状态的信息以先验的形式保留下来从而不影响后续优化一致性的关键技术。实操心得在滑动窗口BA中边缘化的操作需要格外小心。错误的边缘化例如边缘化了与当前窗口仍有强约束的观测量会导致信息丢失甚至使系统变得不一致产生漂移。通常只边缘化那些不再与滑动窗口内变量有直接视觉观测连接的旧状态。4. BA的挑战、技巧与常见问题排查即使理解了原理在实际实现和调试BA时依然会面临诸多挑战。下面分享一些从实践中得来的经验和常见问题的排查思路。4.1 数值稳定性与初始化BA高度依赖于初始值。如果初始值太差例如相机位姿误差超过几十度三维点深度估计完全错误非线性优化很容易陷入局部极小值或直接发散。技巧渐进式重建从两视图重建开始用三角化得到初始点云然后用PnP估计新相机位姿再用新相机三角化更多点如此循环并不断进行小规模的局部BA逐步扩大重建规模。这比直接用所有数据做全局BA要稳定得多。尺度归一化将三维点坐标和相机平移量控制在合理的数值范围内例如让场景的包围盒大小在1-10个单位内。过大或过小的数值会导致雅可比矩阵条件数变差引发数值计算问题。鲁棒核函数使用Huber、Cauchy等鲁棒核函数代替简单的平方损失。这可以降低外点错误匹配对整体优化目标的负面影响使BA对数据噪声更不敏感。4.2 计算效率与稀疏性利用BA的规模可能非常大直接求解是不现实的。高效利用稀疏性是关键。技巧选择合适的线性求解器SPARSE_SCHUR(Ceres) 或Preconditioned Conjugate Gradient(PCG) 是处理BA类问题的常用选择。对于超大规模问题可能需要使用迭代求解器。参数排序优化器内部对参数块的排序会影响消元效率。通常将点云参数放在相机参数之后有利于舒尔消元。并行化现代BA库支持多线程构建雅可比矩阵和残差能有效利用多核CPU。4.3 常见问题与排查表在实际运行BA时如果结果不理想可以按照以下清单进行排查问题现象可能原因排查与解决思路优化不收敛误差震荡或增大1. 学习率LM算法的阻尼因子设置不当。2. 初始值太差落入“错误”的盆地。3. 存在大量外点模型无法拟合。1. 调整LM算法的初始阻尼因子和缩放因子。2. 检查并改进前端初始化的质量尝试更渐进的重建策略。3. 启用鲁棒核函数如Huber并检查前端匹配的RANSAC阈值是否合理。优化后模型严重扭曲或缩放错误1. 尺度模糊性未解决纯旋转或纯平移序列。2. 优化过程中固定了错误的基准如错误固定了某个相机位姿。1. 确保输入数据有足够的视差变化或者引入一个尺度观测如IMU、已知长度的物体。2. 在BA中通常需要固定第一个相机的位姿和尺度或固定两个相机间的基线长度来锁定坐标系。检查固定策略是否正确。部分区域点云模糊重投影误差仍大1. 该区域纹理匮乏特征点少或匹配不准。2. 相机在该区域视角变化太小观测条件差近退化配置。1. 尝试使用更密集的特征提取或光流追踪或引入其他传感器如深度相机。2. 无法从根本上解决可考虑在BA中降低这些不可靠观测的权重。优化速度极慢1. 问题规模太大内存不足。2. 线性求解器选择不当。3. 参数化或自动求导开销大。1. 采用滑动窗口或关键帧策略控制优化规模。2. 尝试不同的线性求解器如ITERATIVE_SCHUR配合好的预条件子。3. 对于简单模型考虑使用解析导数代替自动求导。内存占用过高雅可比矩阵存储方式低效或问题规模超出内存。1. 确保使用的是稀疏模式。2. 使用SPARSE_NORMAL_CHOLESKY求解器时注意其内存消耗与问题稀疏模式有关有时SPARSE_SCHUR更省内存。4.4 高级话题BA的扩展基础的BA优化的是几何一致性。在实际应用中BA常常被扩展以融合更多信息和约束带IMU的视觉惯性BA在误差函数中不仅包含视觉重投影误差还包含IMU的预积分误差项共同优化视觉和惯性状态。这是视觉惯性SLAM如VINS-Mono, ORB-SLAM3的核心。光度BA不依赖于特征点而是直接最小化图像像素强度的差异光度误差常用于直接法SLAM如LSD-SLAM, DSO。全局位姿图优化在大型场景中先进行局部BA然后将局部BA的结果关键帧位姿作为节点将它们的相对约束作为边构建一个位姿图再进行一次轻量级的全局优化以消除累积漂移这是许多SLAM系统后端的标准流程。BA的魅力在于其框架的通用性和强大的纠错能力。它像一位严谨的校对员通过全局视角审视所有局部测量之间的矛盾并给出一个整体最优的妥协方案。掌握BA不仅是学会调用一个优化库更是理解了多视图几何优化问题的核心思想。在实际项目中耐心地调试前端数据关联、精心设计优化参数块结构、明智地选择求解策略比单纯追求算法理论上的最优更为重要。每一次BA迭代后看到误差曲线稳步下降点云变得清晰紧实那种感觉正是三维视觉工程师的快乐源泉之一。