1. 从“一锅粥”到“一锅好粥”Bundle Adjustment到底是什么如果你玩过摄影或者用过手机上的全景扫描功能可能遇到过这样的问题拍了几张有重叠部分的照片想把它们拼成一张完整的大图结果发现边缘对不上或者拼接处有明显的错位和重影。这背后的核心难题就是如何让多张照片的视角、位置和三维空间点都“对齐”。在计算机视觉和摄影测量领域解决这个问题的“终极武器”之一就是Bundle AdjustmentBA中文常译为“光束法平差”或“捆绑调整”。别被这个听起来有点学术的名字吓到。你可以把它想象成一个极其较真的“校对员”。我们有一堆观测数据比如从不同角度拍摄同一个物体时物体上的某个特征点比如一个桌角在每张照片上的像素坐标。同时我们还有一堆待确定的“未知数”每张照片拍摄时相机的位置和朝向称为相机位姿以及那个桌角在真实三维空间中的坐标。初始时我们对这些未知数的估计可能很粗糙——相机位置估得有点偏三维点坐标也算得不太准。这就导致了一个现象根据我们当前估计的相机位姿和三维点坐标理论上“投影”回照片上得到的像素位置和实际观测到的像素位置对不上。这个差距就是“重投影误差”。BA要干的活儿就是同时调整所有的相机位姿和所有的三维点坐标让这个总体的重投影误差变得最小。它不是单独优化相机或者单独优化三维点而是把所有的参数“捆绑”在一起进行全局优化所以才叫“捆绑调整”。这个过程本质上是一个大规模的非线性最小二乘优化问题。通过迭代计算BA能让整个系统达到一个“自洽”的状态所有照片的视角、所有空间点的位置都彼此协调一致就像把一锅食材和调料都放得乱七八糟的粥慢慢调整火候和搅拌最终熬成一锅味道均匀、口感顺滑的好粥。所以BA是三维重建、SLAM同步定位与地图构建、运动恢复结构SfM等技术的基石。没有BA我们得到的三维模型会七扭八歪机器人构建的地图会飘忽不定AR虚拟物体也无法稳稳地“钉”在真实世界里。它虽然不是新鲜技术但因其核心重要性一直是研究和工程中的关键环节。2. BA的核心思想与数学模型拆解要真正理解BA不能停留在比喻层面需要稍微深入其数学模型。放心我们会用最直白的方式把它讲清楚。2.1 重投影误差BA的优化目标BA所有工作的出发点就是最小化重投影误差。我们用一个简单的例子来说明假设我们有一个三维空间点P坐标为[X, Y, Z]被两个相机C1和C2拍摄到。对于相机C1我们观测到点P在其照片上的像素坐标为(u1, v1)对于相机C2观测到的像素坐标是(u2, v2)。现在我们手头有对相机C1位姿的初始估计包括旋转矩阵R1、平移向量t1和相机内参矩阵K1以及对点P三维坐标的初始估计[X_est, Y_est, Z_est]。那么我们可以用这个初始估计将点P的估计坐标按照相机C1的模型“重投影”回C1的像平面上计算出一个估计的像素坐标(u1_est, v1_est)。计算公式本质上是三维到二维的投影将世界坐标点转换到相机坐标系P_cam R1 * P_est t1投影到归一化平面[x_norm, y_norm] [P_cam.x / P_cam.z, P_cam.y / P_cam.z]考虑相机内参如焦距、主点和畸变得到像素坐标[u1_est, v1_est] K1 * distort([x_norm, y_norm])重投影误差就是观测值(u1, v1)和这个估计值(u1_est, v1_est)之间的差值通常用欧氏距离的平方表示。对于相机C2和点P同样可以计算一个误差。BA要优化的是所有这样的误差的总和。假设我们有m个相机和n个三维点那么总误差函数可以写成E Σ Σ || z_ij - proj(C_i, P_j) ||^2其中z_ij是第i个相机观测到第j个三维点的像素坐标如果该点未被该相机看到则此项不计proj(C_i, P_j)就是用第i个相机的参数将第j个三维点投影到像平面的函数。注意这里的双求和符号意味着BA考虑的是所有相机-点对的误差。这是一个典型的“多对多”关系也是其计算复杂性的来源。2.2 非线性优化BA的求解引擎为什么叫“非线性”最小二乘因为投影函数proj(C_i, P_j)本身是非线性的包含了旋转、除法等运算。我们无法直接通过解线性方程组来得到最优解。BA通常采用迭代优化的方法最主流的是列文伯格-马夸尔特Levenberg-Marquardt, LM算法。你可以把它理解为一种“智能梯度下降”。在每一步迭代中线性化在当前参数估计值所有相机位姿和所有三维点坐标处对误差函数进行一阶泰勒展开将其近似为一个线性函数。构建并求解正规方程基于线性化模型构建一个关于参数增量δ的大型线性方程组(J^T J λ I) δ -J^T e。其中J是巨型雅可比矩阵包含了误差对所有参数的导数e是当前的重投影误差向量λ是LM算法中控制步长的阻尼因子。更新参数求解出增量δ将其加到当前的参数估计上得到一组新的参数。判断收敛计算新参数下的总误差。如果误差下降则接受这次更新并可能减小λ以加快收敛如果误差上升则拒绝更新增大λ以采取更保守的步长更接近梯度下降。重复这个过程直到总误差不再显著下降或达到最大迭代次数。这里的关键挑战在于雅可比矩阵J的规模非常庞大。假设有100个相机每个位姿6个参数3个旋转3个平移和1000个三维点每个点3个坐标那么总参数数量就是100*6 1000*3 3600。J将是一个(观测数量*2) x 3600的矩阵。直接存储和求解J^T J一个3600x3600的矩阵是极其耗内存和计算资源的。2.3 稀疏性与舒尔补BA的加速秘诀幸运的是BA问题的雅可比矩阵具有天然的稀疏性。一个特定的重投影误差只对产生该观测的特定相机参数和特定三维点参数有导数对其他相机和点参数的导数为零。这意味着雅可比矩阵J和黑塞矩阵近似J^T J都是稀疏块矩阵。以J^T J为例它可以被排列成如下形式[ B E ] [ E^T C ]其中B是一个对角块矩阵每个对角块对应一个相机参数相对于所有误差的二阶导数块与块之间为零因为不同相机的参数不共同影响同一个误差。C也是一个对角块矩阵每个对角块对应一个三维点参数相对于所有误差的二阶导数。E是相机和三维点之间的耦合矩阵它也是稀疏的只有当相机观测到某个点时对应的块才非零。利用这种稀疏结构LM算法中的核心步骤——求解增量方程(J^T J λ I) δ -J^T e——可以通过舒尔补Schur Complement技巧高效完成。其核心思想是先利用矩阵分块消元法单独解出所有相机参数的增量然后再回代解出三维点参数的增量。由于B块本身也是由许多小对角块组成的对相机参数的求解可以进一步并行化或高效处理。正是这种对稀疏性的极致利用使得BA能够处理成千上万个相机和数百万个三维点的大规模问题从而支撑起现代大规模三维重建和SLAM系统。3. BA在视觉里程计与SLAM中的关键作用在SLAM尤其是视觉SLAM中BA扮演着“后端优化”的核心角色。前端如特征提取与匹配提供粗糙的观测数据后端BA则负责对这些数据进行全局优化得到精确一致的地图和轨迹。3.1 从增量式到全局式BA的调用策略在实际的SLAM系统中BA的调用策略根据精度和效率的权衡主要分为几种全局BAFull BA / Global BA优化所有关键帧的位姿和所有地图点的位置。这是最彻底、精度最高的优化但计算量也最大通常只在系统初始化、闭环检测后或者作为最终输出前的“抛光”步骤使用。局部BALocal BA为了平衡精度和效率更常用的策略。当系统新增一个关键帧时并不优化全部历史而是优化一个局部窗口内的关键帧例如新帧、共视关系最强的N个老关键帧以及这些帧观测到的所有地图点。窗口外的关键帧位姿则保持固定作为约束。这能有效控制计算规模保证实时性。位姿图优化Pose Graph Optimization这是一种更轻量级的后端优化。当地图点数量极其庞大时即使做局部BA优化地图点坐标的开销也很大。位姿图优化“边缘化”掉地图点变量只保留关键帧位姿作为图的节点而节点之间的边则由相对位姿约束来自特征匹配或闭环构成。它优化的是位姿图不再显式优化三维点速度更快常用于大规模场景下的长期运行和闭环校正。但BA尤其是局部BA因其包含地图点优化通常能提供比纯位姿图更高的精度。3.2 工程实践中的关键考量在工程中实现一个高效稳健的BA模块有许多细节需要注意参数化问题相机的旋转如何表示常用的有旋转矩阵9个数有6个约束、四元数4个数有1个约束和旋转向量/李代数3个数无约束。在优化中通常使用李代数so(3)或其对应的切空间增量因为它是无约束的最小参数化方便求导和更新。更新时使用R - R * Exp(δθ)其中δθ是三维旋转增量。鲁棒核函数Robust Kernel特征匹配中难免存在误匹配外点。这些外点会产生巨大的重投影误差如果不加处理会严重扭曲优化结果把正确的参数“拉偏”。鲁棒核函数如Huber核、Cauchy核的作用就是对误差大的项进行抑制降低其对整体优化目标的影响。例如Huber核在误差小于某个阈值时使用平方损失L2范数大于阈值时使用线性损失L1范数从而对外点不那么敏感。信息矩阵与协方差在求解增量方程后我们不仅能得到参数的最优估计还能从黑塞矩阵J^T J的逆或其近似中估计参数的协方差矩阵这反映了估计结果的不确定性。这对于评估重建质量、下一步的决策如选择新的关键帧非常重要。实操心得在调试SLAM系统时如果发现轨迹漂移严重除了检查前端匹配一定要关注后端BA的配置。例如鲁棒核函数的阈值是否设置合理是否因为误匹配太多导致核函数也无力回天局部BA的窗口大小是否合适窗口太小约束不足窗口太大计算超时。通常需要根据场景运动速度和计算资源进行调参。4. 利用开源库快速上手BA实践如今我们无需从零开始实现复杂的LM算法和稀疏矩阵运算。有许多优秀的开源优化库提供了BA或更通用的非线性最小二乘优化的求解器。这里以最著名的Ceres Solver和g2o为例介绍如何快速上手。4.1 使用Ceres Solver实现一个简单BACeres Solver是Google开发的一个用于求解大规模非线性最小二乘问题的开源库API清晰文档完善非常适合BA问题。假设我们有一个非常简单的场景两个相机观测到同一个三维点。我们来构建这个BA问题。首先定义重投影误差的计算模型即代价函数struct ReprojectionError { ReprojectionError(double observed_x, double observed_y) : observed_x(observed_x), observed_y(observed_y) {} template typename T bool operator()(const T* const camera, // 相机参数: [angle_axis[3], translation[3], focal, cx, cy] const T* const point, // 三维点: [x, y, z] T* residuals) const { // 输出残差: [2] // 1. 将点从世界坐标系旋转平移到相机坐标系 // camera[0,1,2] 是旋转向量角轴camera[3,4,5]是平移 T p[3]; ceres::AngleAxisRotatePoint(camera, point, p); p[0] camera[3]; p[1] camera[4]; p[2] camera[5]; // 2. 投影到归一化平面 (这里假设是针孔模型忽略畸变) T xp p[0] / p[2]; T yp p[1] / p[2]; // 3. 应用内参 (camera[6]focal, camera[7]cx, camera[8]cy) T predicted_x camera[6] * xp camera[7]; T predicted_y camera[6] * yp camera[8]; // 4. 计算残差观测值 - 预测值 residuals[0] T(observed_x) - predicted_x; residuals[1] T(observed_y) - predicted_y; return true; } static ceres::CostFunction* Create(double observed_x, double observed_y) { // 残差维度2第一个参数块相机维度9第二个参数块点维度3 return (new ceres::AutoDiffCostFunctionReprojectionError, 2, 9, 3( new ReprojectionError(observed_x, observed_y))); } double observed_x; double observed_y; };然后在主函数中构建并求解问题int main() { // 假设的初始值 double camera1[9] {/* 旋转向量 */ ... , /* 平移 */ ... , /* 焦距, cx, cy */ ...}; double camera2[9] {...}; double point[3] {...}; // 观测数据相机1和相机2观测到的该点像素坐标 double observation1_x ..., observation1_y ...; double observation2_x ..., observation2_y ...; ceres::Problem problem; // 为相机1的观测添加残差块 ceres::CostFunction* cost_function1 ReprojectionError::Create(observation1_x, observation1_y); problem.AddResidualBlock(cost_function1, nullptr, // 损失函数这里用nullptr代表平方损失也可用new ceres::HuberLoss(1.0)添加鲁棒核 camera1, point); // 为相机2的观测添加残差块 ceres::CostFunction* cost_function2 ReprojectionError::Create(observation2_x, observation2_y); problem.AddResidualBlock(cost_function2, nullptr, camera2, point); // 可以设置某些参数块恒定不变例如固定第一个相机作为参考系 // problem.SetParameterBlockConstant(camera1); ceres::Solver::Options options; options.linear_solver_type ceres::DENSE_SCHUR; // 对于小问题可用DENSE_SCHUR大问题用SPARSE_SCHUR options.minimizer_progress_to_stdout true; ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); std::cout summary.BriefReport() \n; // 优化后的 camera1, camera2, point 就存储在原数组里了 return 0; }这个例子虽然简单但清晰地展示了使用Ceres解决BA问题的流程定义误差项、构建问题、添加残差块、配置求解器、执行优化。4.2 g2o与Ceres的选型考量另一个常用的库是g2oGeneral Graph Optimization。与Ceres相比g2o更侧重于“图优化”的建模范式。在g2o中你将优化变量顶点Vertex和误差项边Edge显式地构建成一个图然后由优化器Optimizer对这个图进行优化。g2o的特点图模型非常直观特别适合SLAM中位姿图优化的表达。它提供了更多现成的顶点和边类型如SE3位姿顶点、三维点顶点、各种传感器模型的边在SLAM社区历史更久许多经典SLAM系统如ORB-SLAM使用g2o。Ceres的特点API更通用、更现代化易于上手。其对自动微分AutoDiff的支持非常好如上例所示无需手动推导复杂的雅可比矩阵降低了开发门槛。在性能上两者对于标准BA问题通常相差不大Ceres在某些情况下因其先进的线性求解器接口可能更有优势。选择建议如果你是初学者或者你的问题主要是定义清晰的最小二乘问题如BA推荐从Ceres开始它的学习曲线更平缓。如果你正在实现一个完整的SLAM系统且对位姿图优化有强烈需求或者参考的经典算法基于g2o那么深入学习g2o是必要的。实际上很多开发者会根据模块需求混合使用。5. BA实战中的常见陷阱与调优技巧即使理解了原理使用了强大的库在实际项目中应用BA仍然会遇到各种坑。下面记录一些典型的陷阱和对应的调优技巧。5.1 数值不稳定与初始化问题BA严重依赖于初始值。如果初始的相机位姿和三维点坐标离真实值太远非线性优化很容易陷入局部极小值或者直接发散。问题表现优化后误差反而变大或者优化器报告“失败”。解决方案良好的前端确保特征匹配足够准确三角化得到的三维点初始值相对可靠。可以使用RANSAC等鲁棒估计方法在BA之前先滤除明显的误匹配。渐进式优化不要一开始就用所有参数和所有观测做Full BA。可以先固定一些参数比如固定第一个相机位姿和尺度优化其他参数或者先进行几次仅优化位姿固定三维点或仅优化三维点固定位姿的迭代待结果稳定后再进行联合优化。尺度归一化特别是在单目SLAM中尺度是模糊的。确保你的三维点坐标和相机平移在一个合理的数量级内比如1-100之间避免数值过大或过小导致计算精度问题。5.2 外点误匹配的干扰这是影响BA精度的头号杀手。即使只有少量外点如果不加处理也可能严重扭曲优化结果。问题表现优化后的轨迹或模型在局部出现明显的扭曲或跳跃但整体似乎还能看。解决方案必用鲁棒核函数如前所述在添加残差块时不要使用默认的平方损失nullptr一定要加上鲁棒核函数如new ceres::HuberLoss(1.0)。这个阈值1.0需要根据你重投影误差的像素单位来调整通常可以设为1-5个像素。卡方检验在优化迭代过程中或优化后可以计算每个残差的归一化平方残差^2 / 信息矩阵如果这个值超过某个基于卡方分布的阈值例如95%置信度对应阈值可以认为该观测是外点并将其剔除或降低其权重。前端把关最根本的还是在特征匹配阶段尽可能提高内点率使用更强大的描述子和匹配策略。5.3 计算效率与规模瓶颈当相机和点数量增长到数万、数十万时BA的计算时间和内存消耗会成为瓶颈。问题表现优化一次耗时几十秒甚至几分钟无法满足实时性要求程序内存占用过高。解决方案使用正确的线性求解器在Ceres中对于大规模BA必须使用SPARSE_SCHUR求解器。对于超大规模问题可以考虑使用迭代求解器如ITERATIVE_SCHUR配合CG或PCG预处理共轭梯度法。采用局部BA策略这是在线SLAM系统的标准做法。精心设计局部窗口的选取策略在保证精度的前提下最大化效率。边缘化Marginalization当某些旧的关键帧被移出优化窗口时不能简单地丢弃否则会丢失它们携带的约束信息。正确的方式是将其“边缘化”将其对剩余变量的约束信息以先验项的形式保留在优化问题中。这需要操作信息矩阵是保证SLAM系统一致性的高级技巧。并行化现代BA库如Ceres本身支持多线程计算雅可比矩阵和残差。确保在编译时开启OpenMP等支持并在Solver::Options中设置num_threads。5.4 参数化与流形上的优化旋转的非欧几里得特性使得其优化需要特别处理。问题表现优化过程中旋转参数出现非法值不再是旋转矩阵或者更新步长计算异常。解决方案使用局部参数化在Ceres中对于四元数或旋转矩阵参数块需要为其设置LocalParameterization。例如对于四元数使用ceres::EigenQuaternionParameterization它保证了在优化过程中四元数更新始终保持在单位球面上。理解“李群-李代数”对于高级用户直接使用李代数so(3), se(3)作为旋转/位姿的参数化并在其切空间进行优化是最规范的做法。这通常需要自己定义参数块和雅可比计算或者使用库中提供的相应模块。踩坑实录曾经遇到一个BUGBA优化后相机姿态完全错乱。排查了很久最后发现是在添加四元数参数块时忘记设置LocalParameterization。优化器在欧氏空间中对四元数的四个分量进行无约束更新破坏了单位约束导致整个几何意义失效。这个错误非常隐蔽因为程序不会报错只是结果完全不对。所以对于旋转相关的参数设置正确的参数化是重中之重。6. 深入理解BA与相关概念的对比与演进为了更深刻地理解BA将其与一些相关概念进行对比是很有帮助的。6.1 BA vs. 直接法 vs. 滤波法在视觉SLAM中后端优化主要有三大流派基于滤波的方法、基于关键帧的BA方法、以及直接法。基于滤波的方法如EKF-SLAM将状态位姿和地图点视为随机变量通过贝叶斯滤波进行递推估计。它通常只维护当前时刻的状态估计和协方差计算复杂度与地图点数量的平方相关难以应用于大规模场景。BA相比滤波法是一种批量优化可以利用所有历史信息进行全局调整精度更高且得益于稀疏性能处理更大规模的问题。基于关键帧的BA方法这是目前主流间接法特征点法SLAM的标准后端。它提取特征点在关键帧之间进行匹配构建重投影误差进行BA优化。其优点是精度高、能构建稀疏特征地图但依赖于特征提取与匹配的好坏。直接法如DTAM, DSO它不提取特征点而是直接利用图像的像素灰度信息构建光度误差进行优化。其误差项是“像素亮度误差”而非“几何位置误差”。直接法在纹理缺失、模糊区域可能更鲁棒且能生成半稠密或稠密地图。直接法也可以使用BA的框架只不过误差模型从几何重投影误差变成了光度误差优化变量可能还包括相机的光度参数如增益、偏置。因此有“直接法BA”的说法。两者在优化框架上统一区别在于观测模型和误差定义。6.2 增量式BA与滑动窗口BA这是工程上为了实时性而对经典BA的改进。增量式BAiBA在传统的LM算法中每次迭代都需要重新线性化整个问题并求解一个大线性系统。iBA的核心思想是当系统新增一些观测新的相机、新的点时利用之前优化结果的信息只对新增部分相关的方程进行更新和求解避免全量计算。这可以显著提高优化速度但实现复杂。滑动窗口BA如前所述局部BA这是目前视觉里程计中最实用的策略。它维护一个固定大小的关键帧窗口只优化窗口内的变量。当新关键帧加入时最老的关键帧被移出窗口。为了不丢失信息需要对移出的帧进行边缘化。滑动窗口在精度、效率和一致性之间取得了很好的平衡。6.3 现代BA研究前沿BA作为一个经典问题研究并未止步。当前的前沿方向包括更快的求解器与硬件加速利用GPU并行计算雅可比矩阵和求解线性系统研究更高效的线性求解器如利用问题特定的先验结构甚至使用深度学习来预测优化步长。联合优化与语义BA传统的BA只优化几何参数。现代的BA开始尝试联合优化几何、语义、甚至动态物体参数。例如在优化位姿和点的同时也优化场景中物体的类别、姿态和尺寸构建带语义信息的地图。学习辅助的BA使用深度学习网络来预测更准确的初始值、更鲁棒的特征、或者直接预测重投影误差的权重以改善BA的收敛性和鲁棒性降低对外点手工处理如RANSAC的依赖。BA从本质上讲是一个关于“如何让多视角观测达成一致”的最优化问题。它的思想不仅限于视觉几何在机器人学、计量学、甚至其他工程领域只要涉及多传感器数据融合与状态估计都能看到类似“捆绑调整”思想的影子。掌握BA就等于掌握了一把解开多视角几何优化问题的万能钥匙。