1. 从“拍歪了”到“算准了”束平差到底在解决什么问题如果你玩过摄影尤其是用手机拍过全景照片可能会遇到一个尴尬的情况明明想拍一张平滑衔接的广角图结果拼接出来的照片里电线杆是弯的地平线出现了断层。或者在三维重建、机器人SLAM同步定位与建图领域我们通过多张照片计算出了相机的位置和一堆三维点但把这些结果放在一起看总觉得有些“别扭”——某些点的投影和它在照片上的实际位置对不上误差比预想的要大。这种“别扭”感其根源往往在于我们之前每一步的计算都是基于局部、带有噪声的观测这些误差会像滚雪球一样累积和传播。“束平差”Bundle Adjustment 简称BA要干的就是解决这个“别扭”问题的终极优化手段。你可以把它想象成一次全局的“大扫除”和“总校对”。我们手头有一堆零散的“证据”很多张从不同角度拍摄同一场景的照片图像以及从这些照片中提取出的、代表同一个物理点的像素位置特征点对应关系。最初我们可能通过SFM运动恢复结构等方法粗略地估计出了每张照片拍摄时相机的位姿位置和朝向以及场景中一系列三维点的坐标。但这个粗略估计的“世界”是松散的、自相矛盾的。束平差的核心思想非常直观它同时调整所有相机的位姿参数和所有三维空间点的坐标使得调整后的参数能够最好地“解释”我们观测到的所有图像像素点。什么叫“最好地解释”就是让根据当前三维点和相机位姿“预测”出来的像素位置称为重投影与我们在图像上实际“看到”的像素位置之间的差距总和达到最小。这个“差距总和最小化”的问题在数学上正是一个典型的非线性最小二乘问题。所以当我们谈论“最小二乘问题详解15束平差原理与基础实现”时我们实际上是在深入一个计算机视觉、摄影测量、机器人等领域最核心的优化引擎的内部。它不只是一个数学玩具而是诸如Google街景、无人机三维建模、AR/VR定位、自动驾驶环境感知等众多实际系统的基石。接下来我将抛开复杂的教科书定义带你从第一性原理出发拆解BA的每一个环节并用最基础的代码展示其内核让你真正理解这个“全局优化大师”是如何工作的。2. 拆解BA的数学模型误差从何而来又去向何处要理解BA必须先看清它要优化的目标是什么。我们暂时忘掉“束”和“平差”这些术语从最基本的元素开始构建。想象一个简单的场景我们在空间中有一个真实的三维点P坐标为[X, Y, Z]用一台相机从某个角度拍下了它在照片上我们看到了这个点其像素坐标为[u, v]。相机本身有它的位置和朝向外参包括旋转矩阵R和平移向量t以及内部的成像特性内参比如焦距f、主点[cx, cy]、畸变系数等。从三维世界到二维图像的转换过程就是一个投影函数π[u, v]^T π( R * P t, K, D)其中K是内参矩阵D是畸变参数。这个函数描述了完美的、无噪声的成像过程。但现实是骨感的。我们观测到的像素坐标[u_obs, v_obs]是有噪声的特征点提取不精准、图像模糊等。同时我们最初估计的相机位姿[R, t]和三维点坐标P也是不准确的。这就产生了一个重投影误差e [u_obs, v_obs]^T - π( R * P t, K, D)这个e是一个二维向量代表了预测与观测在图像平面上的偏移。现在把场景复杂化。我们有m个三维点P_j (j1...m)和n个相机位姿C_i (i1...n)。并不是每个点都被每个相机看到我们有一个观测集合对于第i个相机和第j个点如果该点在该相机图像中可见我们就有一个观测到的像素坐标z_{ij}。那么整体的目标就是找到所有相机参数包括内参、外参和所有三维点坐标使得所有可见点对应的重投影误差的平方和最小。这就是BA的代价函数F Σ_{i1}^{n} Σ_{j1}^{m} ρ( || z_{ij} - π( C_i, P_j ) ||^2 )这里的ρ是一个鲁棒核函数如Huber核用于抑制误匹配外点对优化过程的破坏性影响这是工程实践中至关重要的一环后面会详细讲。为什么叫“束”平差“束”Bundle这个说法非常形象。对于同一个三维点P_j从不同相机C_i发出的光线观测射线都应该交汇于这个点上。由于初始估计不准和观测噪声这些光线像一把散开的“光束”无法精确交汇。BA的过程就是同时调整所有相机的位置和所有三维点的位置让每一束“光线”都尽可能收束到对应的点上故名“束调整”或“束平差”。这个优化问题的参数空间巨大。假设有100个相机每个有6个外参自由度如果也优化内参则更多和10000个三维点每个点3个自由度那么待优化参数的总维数可能达到数万甚至数十万。同时这个代价函数F是关于相机参数和三维点参数的非线性函数因为投影函数π包含旋转和透视除法是非线性的。因此BA是一个大规模的非线性最小二乘问题。3. 非线性最小二乘求解器BA的发动机是如何工作的面对F这样一个复杂的非线性函数我们无法直接找到令其全局最小的解。通用的方法是迭代优化从一个初始猜测比如SFM的结果开始反复进行微小的调整使代价函数值不断下降直到收敛。最常用、最有效的方法是基于高斯-牛顿法或其变种如列文伯格-马夸尔特法LM算法。其核心步骤如下线性化在当前参数估计值xx是一个巨大的向量包含了所有待优化的相机和三维点参数处对每个重投影误差函数e_{ij}(x)进行一阶泰勒展开e_{ij}(x Δx) ≈ e_{ij}(x) J_{ij} Δx其中J_{ij}是误差e_{ij}对参数增量Δx的雅可比矩阵它描述了误差随参数微小变化的敏感度。构建正规方程将线性化后的误差代入代价函数F原来的非线性最小二乘问题就近似变成了一个关于参数增量Δx的线性最小二乘问题min_Δx || J Δx e ||^2这里J是所有误差项雅可比矩阵堆叠起来的大雅可比矩阵e是所有当前误差值堆叠起来的残差向量。这个线性最小二乘问题的解由正规方程给出(J^T J) Δx -J^T e求解增量并更新解这个线性方程H Δx b其中H J^T J称为海森矩阵或信息矩阵b -J^T e。得到参数增量Δx后更新参数x_{new} x Δx。迭代与判断用新的参数x_{new}重新计算误差判断代价函数是否下降、是否满足收敛条件如Δx足够小、误差下降量足够小等。如果不满足则以x_{new}为新的起点重复步骤1-3。注意这里有一个关键的工程取舍。直接构建和求解(J^T J) Δx -J^T e需要操作一个N×N的矩阵N是参数总数对于大规模BAN可能高达数万直接求逆的计算复杂度是O(N^3)完全不可行。幸运的是BA问题的雅可比矩阵J具有特殊的稀疏结构。一个特定的重投影误差e_{ij}只依赖于第i个相机和第j个点因此它的雅可比矩阵J_{ij}只在对应相机参数块和点参数块的位置有非零值。这导致大雅可比矩阵J和大海森矩阵H J^T J都是分块稀疏的。利用这种稀疏性正规方程H Δx b可以写成如下分块形式[ U W ] [ Δc ] [ b_c ] [ W^T V ] [ Δp ] [ b_p ]其中Δc是所有相机参数的增量向量Δp是所有三维点参数的增量向量。U和V分别是只关于相机和只关于点的对角块矩阵更准确地说U是块对角矩阵每个块对应一个相机V也是块对角矩阵每个块对应一个点。W是相机-点关联的矩阵也非常稀疏。这种结构允许我们使用舒尔补消元Schur Complement Trick进行高效求解。具体步骤是先消去点参数增量Δp得到一个只关于相机参数增量Δc的、规模小得多的方程S Δc b_s其中S U - W V^{-1} W^T称为舒尔补矩阵。求解这个“缩减的相机系统”得到Δc。再回代求解Δp。由于相机数量通常远少于三维点数量S矩阵的规模大大减小且它通常也是稀疏、正定的可以用稀疏乔列斯基分解等高效方法求解。这是现代BA求解器如Ceres Solver, g2o能够处理成千上万个相机和数百万个点的关键。4. 雅可比矩阵计算BA精度与效率的基石雅可比矩阵J的计算是BA迭代中的核心计算任务它直接决定了优化的方向和效率。我们需要计算每个重投影误差e_{ij} [u_obs - u_pred, v_obs - v_pred]^T对相机参数和三维点参数的导数。假设我们使用针孔相机模型并考虑径向畸变。投影过程可以分解为几步将世界点P_w变换到相机坐标系P_c R * P_w t。归一化到归一化平面p_n [X_c/Z_c, Y_c/Z_c]^T。应用畸变r^2 x_n^2 y_n^2畸变后坐标p_d (1 k1*r^2 k2*r^4) * p_n这里简化了畸变模型。通过内参投影到像素平面u f_x * x_d c_x,v f_y * y_d c_y。我们需要链式求导计算∂e/∂(相机参数)和∂e/∂(三维点)。这涉及到对旋转矩阵R通常用李代数 so(3) 的扰动模型来求导避免万向节锁和冗余参数、平移向量t、内参[f_x, f_y, c_x, c_y, k1, k2...]以及三维点坐标[X_w, Y_w, Z_w]的偏导数。手动推导这些导数非常繁琐但理解其模式至关重要对三维点P_w的导数本质上反映了三维点位置微小变化时其在图像上投影点的移动方向。对相机旋转ω李代数的导数反映了相机朝向微小变化时投影点的移动方向。对相机平移t的导数反映了相机位置微小变化时投影点的移动方向。对内参的导数反映了相机成像模型本身参数变化的影响。在实际的BA库中这些导数通常通过自动微分Automatic Differentiation技术来计算例如Ceres Solver就内置了强大的自动微分能力。开发者只需要编写计算重投影误差的仿函数Functor描述e f(相机参数, 点参数)的计算过程库就能自动、精确地计算出雅可比矩阵这大大降低了实现难度并保证了数值稳定性。实操心得虽然自动微分很方便但在性能至关重要的场景如嵌入式SLAM手动推导并优化雅可比矩阵的计算代码仍然是必要的。手动实现时要特别注意旋转参数化的求导正确性推荐使用李代数并充分利用中间变量的复用避免重复计算。5. 鲁棒核函数与异常值处理让BA不被“坏点”带偏在真实的图像数据中特征匹配不可能完美。总会有一些错误的匹配点外点混入观测集合。这些外点产生的重投影误差可能非常大。如果使用标准的平方误差L2范数这些巨大的误差项在平方后会占据主导地位优化算法会为了减小这些“不可能完成的任务”而过度调整参数反而破坏了那些正确匹配点内点的几何关系导致优化结果完全失真。这就是为什么在BA的代价函数中需要引入鲁棒核函数ρ(·)。它的作用是对误差的平方进行“重新加权”抑制那些误差过大的项的影响。最常用的鲁棒核函数是Huber核ρ(s) { s, if s δ^2 { 2δ√s - δ^2, if s δ^2其中s ||e||^2是误差的平方δ是一个阈值参数。当误差较小时Huber核的行为和平方误差一样线性当误差超过阈值δ时它的增长变为线性增长而不是平方增长从而削弱了大误差项的影响力。另一个更激进的核函数是Cauchy核ρ(s) c^2 * log(1 s/c^2)它对大误差的抑制能力更强。在优化框架中使用鲁棒核函数等价于对每个残差项引入一个权重。在迭代求解时这个权重会根据当前残差的大小动态调整。实现上这通常通过“重加权最小二乘”的方式融入求解过程。注意事项阈值δ或Cauchy核中的c的选择需要根据具体问题的噪声水平来定。一个经验法则是δ可以设置为特征点提取/匹配的预期标准差例如0.5到2个像素。设置过小会误伤内点设置过大则起不到抑制外点的作用。在实际系统中常常会结合诸如RANSAC的前端几何验证来预先剔除大部分外点BA中的鲁棒核则作为最后一道防线处理漏网之鱼和噪声较大的点。6. 基础实现用Ceres Solver手写一个简易BA理论说了这么多是时候动手实践了。我们将使用C和Ceres Solver库来实现一个最基础的BA优化若干相机位姿和三维点。这里假设相机内参已知且固定只优化相机外参旋转和平移用李代数so(3)表示和三维点坐标。第一步定义重投影误差结构体我们需要定义一个仿函数用于计算误差。Ceres要求我们重载operator()并使用模板参数以便自动微分。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)] const T* const point, // 三维点: [X, Y, Z] T* residuals) const { // 1. 将点从世界坐标系转换到相机坐标系 // camera[0,1,2] 是旋转的角轴向量李代数需要转换为旋转矩阵 T p[3]; ceres::AngleAxisRotatePoint(camera, point, p); // p R * point p[0] camera[3]; // tx p[1] camera[4]; // ty p[2] camera[5]; // tz // 2. 投影到归一化平面 (针孔模型) T xp p[0] / p[2]; T yp p[1] / p[2]; // 3. 应用已知的内参 (这里假设焦距f520, 主点cx325, cy253) const T focal T(520.0); const T cx T(325.0); const T cy T(253.0); T predicted_x focal * xp cx; T predicted_y focal * yp cy; // 4. 计算残差 (观测值 - 预测值) residuals[0] T(observed_x) - predicted_x; residuals[1] T(observed_y) - predicted_y; return true; } static ceres::CostFunction* Create(const double observed_x, const double observed_y) { // 残差维度2相机参数块维度6点参数块维度3 return (new ceres::AutoDiffCostFunctionReprojectionError, 2, 6, 3( new ReprojectionError(observed_x, observed_y))); } double observed_x; double observed_y; };第二步构建问题并添加参数块与残差块假设我们已经有了初始的相机位姿数组cameras和三维点数组points以及观测数据列表observations每个观测记录了相机索引、点索引和观测到的像素坐标。ceres::Problem problem; // 添加相机参数块使用李代数角轴表示旋转因此不需要额外的局部参数化 for (int i 0; i num_cameras; i) { problem.AddParameterBlock(cameras[i], 6); } // 添加三维点参数块 for (int j 0; j num_points; j) { problem.AddParameterBlock(points[j], 3); } // 添加残差块即观测约束 for (const auto obs : observations) { int camera_id obs.camera_index; int point_id obs.point_index; double observed_x obs.x; double observed_y obs.y; ceres::CostFunction* cost_function ReprojectionError::Create(observed_x, observed_y); problem.AddResidualBlock(cost_function, nullptr, // 这里可以传入鲁棒核函数例如 new ceres::HuberLoss(1.0) cameras[camera_id], points[point_id]); }第三步配置求解器并执行优化ceres::Solver::Options options; options.linear_solver_type ceres::SPARSE_SCHUR; // 关键使用舒尔补消元法 options.minimizer_progress_to_stdout true; // 输出迭代信息 options.max_num_iterations 100; // 最大迭代次数 options.function_tolerance 1e-6; // 函数值变化容忍度 options.gradient_tolerance 1e-10; // 梯度容忍度 options.parameter_tolerance 1e-8; // 参数变化容忍度 ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); std::cout summary.FullReport() \n;第四步分析结果summary对象包含了优化的详细报告如初始代价、最终代价、迭代次数、收敛情况等。优化后的相机位姿和三维点坐标就存储在cameras和points数组中了。踩坑实录在第一次运行这个代码时你可能会遇到优化发散或结果怪异的情况。除了数据本身的问题常见原因有1) 初始值太差远离真值导致线性化近似失效。解决方法是提供更好的初始值例如从对极几何或PnP得到。2) 尺度模糊。如果所有相机和点一起做刚体变换重投影误差不变。通常需要固定第一个相机的位姿或固定某个三维点的深度来锁定尺度。3) 数值问题。确保角轴向量的模不要太大旋转角度最好在π以内必要时对平移量进行归一化。7. 稀疏性与求解器选型为什么BA能跑得动我们之前提到直接求解海森矩阵的逆是O(N^3)的复杂度。通过舒尔补消元我们将求解一个巨大的(6m3n) x (6m3n)的稠密矩阵问题转化为了先求解一个相对较小的6m x 6m的稀疏矩阵S舒尔补矩阵再回代求解点参数。S矩阵的稀疏结构由相机之间的可见关系决定。如果两个相机没有共同观测的三维点那么它们在S矩阵中对应的块就是零。在大多数SLAM或SFM问题中相机通常只与邻近时间或空间的相机有共同观测因此S矩阵通常是带状或近似块对角化的稀疏矩阵。对于这种稀疏正定矩阵有高效的求解算法稀疏乔列斯基分解Sparse Cholesky Factorization如SuiteSparse中的CHOLMOD库。这是最直接稳定的方法但对于超大规模问题分解的填充fill-in可能很高消耗大量内存。共轭梯度法Conjugate Gradient, CG一种迭代法特别适合求解大型稀疏线性系统。为了加速收敛需要好的预条件子Preconditioner如雅可比预条件子、不完全乔列斯基分解预条件子等。基于图的求解器像g2o、GTSAM等库有时会利用问题的图模型结构使用诸如树状网络Tree-based或子图预积分等技巧来加速求解。在Ceres Solver中options.linear_solver_type ceres::SPARSE_SCHUR就是指示其使用舒尔补技巧并调用稀疏线性代数库通常是Eigen或SuiteSparse来求解缩减后的相机系统。对于超大规模问题可以尝试ITERATIVE_SCHUR并结合CG迭代法。选型建议中小规模问题相机数1000SPARSE_SCHUR SuiteSparse 通常是最快最稳定的选择。大规模问题相机数1000ITERATIVE_SCHUR 好的预条件子如SCHUR_JACOBI或CLUSTER_JACOBI可能内存效率更高。特定结构问题如果相机位姿图具有特殊的时序链式结构如视觉里程计使用DENSE_SCHUR甚至DENSE_NORMAL_CHOLESKY并利用滑动窗口法可能是更优解。8. 工程实践中的关键技巧与扩展方向一个能工作的基础BA只是起点。要让它在实际系统中稳定、高效、精确地运行还需要考虑很多工程细节。1. 参数化与流形旋转不能用普通的3x3矩阵或欧拉角直接作为优化变量。我们使用李代数 so(3)角轴向量或四元数配合局部参数化来参数化旋转。在Ceres中对于四元数我们需要调用problem.SetParameterization()来告诉求解器其在流形上的更新规则。错误的参数化会导致优化失败或数值不稳定。2. 先验信息与固定参数尺度固定单目BA存在尺度模糊性。通常固定第一个相机的位姿旋转设为单位阵平移设为0和第一个三维点的深度或固定某两点间的距离来锁定尺度。内参标定如果内参已知且可信可以固定它们不优化。如果内参也需要优化要小心处理因为焦距和主点等参数与旋转平移存在耦合可能需要更强的先验或更多的观测数据。闭环检测在SLAM中当机器人回到之前到过的地方识别出闭环并添加位姿约束能为BA提供全局一致性约束极大减少累积漂移。3. 外点剔除策略前端滤波使用RANSAC进行基础矩阵或单应矩阵估计在特征匹配阶段就剔除大部分外点。后端鲁棒核如前述使用Huber或Cauchy损失函数。后优化剔除在BA优化几轮后检查每个残差的大小。将重投影误差大于某个阈值如5-10个像素的观测视为外点从问题中移除然后重新优化。4. 加速技巧多线程雅可比矩阵和残差的计算可以完全并行。Ceres Solver通过设置options.num_threads来支持。问题简化局部BA只优化最近几帧的相机位姿和它们观测到的点保持远处点和其他相机固定。常用于视觉里程计。位姿图优化在BA之后将三维点边缘化掉只保留相机位姿以及它们之间的相对约束形成一个更轻量的位姿图进行优化用于全局闭环校正。增量式求解对于实时SLAM可以使用iSAM增量平滑与建图等增量求解器避免每次全量优化。5. 与现代深度学习的结合这是一个非常活跃的方向。传统BA依赖于手工设计的特征点如SIFT ORB。现在研究者们正在探索用深度特征替换手工特征提高匹配鲁棒性和精度。学习型的BA用神经网络直接预测几何参数或学习优化过程中的更新步长、权重。联合优化将BA与语义分割、深度估计等网络一起进行端到端优化实现更高级别的场景理解。从原理到实现束平差是一个将优美的数学理论与扎实的工程实践紧密结合的典范。理解其每一步背后的“为什么”能帮助我们在面对实际系统中千奇百怪的问题时不再是一个调参的“黑盒”使用者而是一个能够洞察根源、精准施策的工程师。当你下次看到AR应用稳定地锚定虚拟物体或者无人机生成精确的三维地图时你会知道背后正是这套名为“束平差”的精密机器在无声地运转。
