尧图网站建设 免费方案咨询
ARTICLE DETAIL

资讯详情

深耕网站建设、视觉设计与SEO优化的一线实战洞察。

首页/资讯中心/资讯详情

Bundle Adjustment：从重投影误差到稀疏优化的视觉SLAM后端核心

发布时间：2026/8/7 2:18:38 尧图网站建设
Bundle Adjustment：从重投影误差到稀疏优化的视觉SLAM后端核心

1. 从“一锅粥”到“一锅好粥”：Bundle Adjustment到底是什么？

如果你玩过摄影，或者用过手机上的全景扫描功能，可能遇到过这样的问题：拍了几张有重叠部分的照片，想把它们拼成一张完整的大图，结果发现边缘对不上，或者拼接处有明显的错位和重影。这背后的核心难题，就是如何让多张照片的视角、位置和三维空间点都“对齐”。在计算机视觉和摄影测量领域，解决这个问题的“终极武器”之一，就是Bundle Adjustment（BA），中文常译为“光束法平差”或“捆绑调整”。

别被这个听起来有点学术的名字吓到。你可以把它想象成一个极其较真的“校对员”。我们有一堆观测数据：比如从不同角度拍摄同一个物体时，物体上的某个特征点（比如一个桌角）在每张照片上的像素坐标。同时，我们还有一堆待确定的“未知数”：每张照片拍摄时相机的位置和朝向（称为相机位姿），以及那个桌角在真实三维空间中的坐标。初始时，我们对这些未知数的估计可能很粗糙——相机位置估得有点偏，三维点坐标也算得不太准。这就导致了一个现象：根据我们当前估计的相机位姿和三维点坐标，理论上“投影”回照片上，得到的像素位置，和实际观测到的像素位置对不上。这个差距，就是“重投影误差”。

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)。计算公式本质上是三维到二维的投影：

  1. 将世界坐标点转换到相机坐标系：P_cam = R1 * P_est + t1
  2. 投影到归一化平面：[x_norm, y_norm] = [P_cam.x / P_cam.z, P_cam.y / P_cam.z]
  3. 考虑相机内参（如焦距、主点）和畸变，得到像素坐标：[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）算法。你可以把它理解为一种“智能梯度下降”。在每一步迭代中：

  1. 线性化：在当前参数估计值（所有相机位姿和所有三维点坐标）处，对误差函数进行一阶泰勒展开，将其近似为一个线性函数。
  2. 构建并求解正规方程：基于线性化模型，构建一个关于参数增量（δ）的大型线性方程组(J^T J + λ I) δ = -J^T e。其中J是巨型雅可比矩阵（包含了误差对所有参数的导数），e是当前的重投影误差向量，λ是LM算法中控制步长的阻尼因子。
  3. 更新参数：求解出增量δ，将其加到当前的参数估计上，得到一组新的参数。
  4. 判断收敛：计算新参数下的总误差。如果误差下降，则接受这次更新，并可能减小λ以加快收敛；如果误差上升，则拒绝更新，增大λ以采取更保守的步长（更接近梯度下降）。重复这个过程，直到总误差不再显著下降或达到最大迭代次数。

这里的关键挑战在于，雅可比矩阵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的调用策略根据精度和效率的权衡，主要分为几种：

  1. 全局BA（Full BA / Global BA）：优化所有关键帧的位姿和所有地图点的位置。这是最彻底、精度最高的优化，但计算量也最大，通常只在系统初始化、闭环检测后，或者作为最终输出前的“抛光”步骤使用。
  2. 局部BA（Local BA）：为了平衡精度和效率，更常用的策略。当系统新增一个关键帧时，并不优化全部历史，而是优化一个局部窗口内的关键帧（例如，新帧、共视关系最强的N个老关键帧）以及这些帧观测到的所有地图点。窗口外的关键帧位姿则保持固定，作为约束。这能有效控制计算规模，保证实时性。
  3. 位姿图优化（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实现一个简单BA

Ceres 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::AutoDiffCostFunction<ReprojectionError, 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的选型考量

另一个常用的库是g2o（General 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严重依赖于初始值。如果初始的相机位姿和三维点坐标离真实值太远，非线性优化很容易陷入局部极小值，或者直接发散。

  • 问题表现：优化后误差反而变大，或者优化器报告“失败”。
  • 解决方案：
    1. 良好的前端：确保特征匹配足够准确，三角化得到的三维点初始值相对可靠。可以使用RANSAC等鲁棒估计方法在BA之前先滤除明显的误匹配。
    2. 渐进式优化：不要一开始就用所有参数和所有观测做Full BA。可以先固定一些参数（比如固定第一个相机位姿和尺度），优化其他参数；或者先进行几次仅优化位姿（固定三维点）或仅优化三维点（固定位姿）的迭代，待结果稳定后再进行联合优化。
    3. 尺度归一化：特别是在单目SLAM中，尺度是模糊的。确保你的三维点坐标和相机平移在一个合理的数量级内（比如1-100之间），避免数值过大或过小导致计算精度问题。

5.2 外点（误匹配）的干扰

这是影响BA精度的头号杀手。即使只有少量外点，如果不加处理，也可能严重扭曲优化结果。

  • 问题表现：优化后的轨迹或模型在局部出现明显的扭曲或跳跃，但整体似乎还能看。
  • 解决方案：
    1. 必用鲁棒核函数：如前所述，在添加残差块时，不要使用默认的平方损失（nullptr），一定要加上鲁棒核函数，如new ceres::HuberLoss(1.0)。这个阈值（1.0）需要根据你重投影误差的像素单位来调整，通常可以设为1-5个像素。
    2. 卡方检验：在优化迭代过程中或优化后，可以计算每个残差的归一化平方（残差^2 / 信息矩阵），如果这个值超过某个基于卡方分布的阈值（例如，95%置信度对应阈值），可以认为该观测是外点，并将其剔除或降低其权重。
    3. 前端把关：最根本的还是在特征匹配阶段尽可能提高内点率，使用更强大的描述子和匹配策略。

5.3 计算效率与规模瓶颈

当相机和点数量增长到数万、数十万时，BA的计算时间和内存消耗会成为瓶颈。

  • 问题表现：优化一次耗时几十秒甚至几分钟，无法满足实时性要求；程序内存占用过高。
  • 解决方案：
    1. 使用正确的线性求解器：在Ceres中，对于大规模BA，必须使用SPARSE_SCHUR求解器。对于超大规模问题，可以考虑使用迭代求解器（如ITERATIVE_SCHUR配合CG或PCG预处理共轭梯度法）。
    2. 采用局部BA策略：这是在线SLAM系统的标准做法。精心设计局部窗口的选取策略，在保证精度的前提下最大化效率。
    3. 边缘化（Marginalization）：当某些旧的关键帧被移出优化窗口时，不能简单地丢弃，否则会丢失它们携带的约束信息。正确的方式是将其“边缘化”，将其对剩余变量的约束信息以先验项的形式保留在优化问题中。这需要操作信息矩阵，是保证SLAM系统一致性的高级技巧。
    4. 并行化：现代BA库（如Ceres）本身支持多线程计算雅可比矩阵和残差。确保在编译时开启OpenMP等支持，并在Solver::Options中设置num_threads。

5.4 参数化与流形上的优化

旋转的非欧几里得特性使得其优化需要特别处理。

  • 问题表现：优化过程中旋转参数出现非法值（不再是旋转矩阵），或者更新步长计算异常。
  • 解决方案：
    1. 使用局部参数化：在Ceres中，对于四元数或旋转矩阵参数块，需要为其设置LocalParameterization。例如，对于四元数，使用ceres::EigenQuaternionParameterization，它保证了在优化过程中，四元数更新始终保持在单位球面上。
    2. 理解“李群-李代数”：对于高级用户，直接使用李代数（so(3), se(3)）作为旋转/位姿的参数化，并在其切空间进行优化，是最规范的做法。这通常需要自己定义参数块和雅可比计算，或者使用库中提供的相应模块。

踩坑实录：曾经遇到一个BUG，BA优化后相机姿态完全错乱。排查了很久，最后发现是在添加四元数参数块时，忘记设置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的改进。

  • 增量式BA（iBA）：在传统的LM算法中，每次迭代都需要重新线性化整个问题并求解一个大线性系统。iBA的核心思想是，当系统新增一些观测（新的相机、新的点）时，利用之前优化结果的信息，只对新增部分相关的方程进行更新和求解，避免全量计算。这可以显著提高优化速度，但实现复杂。
  • 滑动窗口BA：如前所述（局部BA），这是目前视觉里程计中最实用的策略。它维护一个固定大小的关键帧窗口，只优化窗口内的变量。当新关键帧加入时，最老的关键帧被移出窗口。为了不丢失信息，需要对移出的帧进行边缘化。滑动窗口在精度、效率和一致性之间取得了很好的平衡。

6.3 现代BA研究前沿

BA作为一个经典问题，研究并未止步。当前的前沿方向包括：

  • 更快的求解器与硬件加速：利用GPU并行计算雅可比矩阵和求解线性系统；研究更高效的线性求解器（如利用问题特定的先验结构）；甚至使用深度学习来预测优化步长。
  • 联合优化与语义BA：传统的BA只优化几何参数。现代的BA开始尝试联合优化几何、语义、甚至动态物体参数。例如，在优化位姿和点的同时，也优化场景中物体的类别、姿态和尺寸，构建带语义信息的地图。
  • 学习辅助的BA：使用深度学习网络来预测更准确的初始值、更鲁棒的特征、或者直接预测重投影误差的权重，以改善BA的收敛性和鲁棒性，降低对外点手工处理（如RANSAC）的依赖。

BA从本质上讲，是一个关于“如何让多视角观测达成一致”的最优化问题。它的思想不仅限于视觉几何，在机器人学、计量学、甚至其他工程领域，只要涉及多传感器数据融合与状态估计，都能看到类似“捆绑调整”思想的影子。掌握BA，就等于掌握了一把解开多视角几何优化问题的万能钥匙。

网站建设 视觉设计 SEO优化
返回列表