最小二乘问题详解15：束平差原理与基础实现

初始两帧的 E 矩阵分解可能存在错误解或尺度模糊三角化结果受位姿误差和图像噪声影响PnP 的位姿估计会继承并放大前期误差。随着图像数量增加这些局部误差会不断累积导致最终重建结果出现尺度漂移、结构扭曲甚至拓扑错误。要解决这一问题我们需要一种全局优化机制能够同时调整所有相机位姿和所有 3D 点使得整个观测系统在统一的几何模型下达到最优自洽。这就是束平差Bundle Adjustment, BA一种非线性最小二乘优化方法它将所有相机位姿和所有 3D 点作为联合变量最小化所有观测点的重投影误差总和从而实现全局一致的高精度重建。BA 并不取代上述局部方法而是作为 SFM 流程的最终全局优化阶段对由 PnP 与三角化逐步构建的初始重建结果进行一致性精修。在本文中我们将建立 BA 的完整数学模型分析其稀疏结构并基于 Ceres Solver 实现一个基础但可运行的 BA 系统。2 几何模型束平差建立在针孔相机成像模型基础之上。考虑一个由 N 个相机和 M 个 3D 空间点构成的多视图系统。对于任意一个被第 i 个相机观测到的第 j 个 3D 点其成像过程可描述如下世界坐标系中的 3D 点记为 Xj∈R3∈3变换至第 i 个相机坐标系X(i)jRiXjti()其中 Ri∈SO(3)∈(3) 和 ti∈R3∈3 分别表示第 i 个相机的旋转矩阵和平移向量即外参透视投影至归一化平面xnormij1Z(i)j⎡⎣X(i)jY(i)j⎤⎦norm1()[()()]应用内参得到像素坐标假设相机内参 K 已知且固定^xijK[xnormij1]π(K[Ri∣ti][Xj1])^[norm1]([∣][1])其中 π(⋅)(⋅) 表示齐次坐标到非齐次坐标的转换操作。在理想无噪声情况下重投影点 ^xij^ 应与图像中实际检测到的特征点 xij 完全重合。然而由于特征提取误差、初始位姿不准、三角化偏差等因素二者通常存在偏差。BA 的目标正是通过调整所有 {Ri,ti}{,} 与 {Xj}{}使这一偏差全局最小化。在几何上BA 可理解为调整所有相机光心位置与朝向以及所有 3D 点的空间位置使得从各相机出发、穿过对应图像点的“光线束”bundle of rays尽可能交汇于同一点。3 问题建模束平差本质上是一个大规模非线性最小二乘问题。我们首先定义可见性集合Ω{(i,j)∣第 i 个相机观测到了第 j 个 3D 点}Ω{(,)∣第 个相机观测到了第 个 3D 点}其大小 K|Ω||Ω| 表示有效观测总数。3.1 优化变量相机位姿共 N 个每个用李代数 ξi[ϕ⊤i,t⊤i]⊤∈R6[⊤,⊤]⊤∈6 表示ϕi 为旋转向量避免直接优化带约束的旋转矩阵3D 点坐标共 M 个Xj∈R3∈3。将所有变量拼接为全局参数向量p[ξ⊤1,…,ξ⊤N,X⊤1,…,X⊤M]⊤∈R6N3M[1⊤,…,⊤,1⊤,…,⊤]⊤∈633.2 残差函数对每一对观测 (i,j)∈Ω(,)∈Ω定义重投影残差rij(ξi,Xj)xij−π(K[exp([ϕi]×)|ti][Xj1])∈R2(,)−([exp⁡([]×)|][1])∈2其中 exp([ϕi]×)exp⁡([]×) 将旋转向量映射为旋转矩阵。3.3 目标函数将所有残差堆叠为总残差向量 rtotal∈R2Ktotal∈2束平差的目标为minp ∥rtotal(p)∥2∑(i,j)∈Ω∥∥rij(ξi,Xj)∥∥2min ‖total()‖2∑(,)∈Ω‖(,)‖2该问题具有以下关键特性非线性因透视除法与旋转指数映射引入强非线性稀疏性每个残差 rij 仅依赖两个变量块ξi 与 Xj导致雅可比矩阵呈块状稀疏结构大规模当 N,M, 较大时如 N100,M104100,104变量维数可达数万需专用稀疏求解器。4 理论推导在《最小二乘问题详解4非线性最小二乘》与《最小二乘问题详解8Levenberg-Marquardt方法》中我们已系统介绍了 Gauss-NewtonGN与 Levenberg-MarquardtLM方法求解非线性最小二乘问题的一般框架。束平差BA作为其典型应用场景同样通过迭代求解如下线性化系统(J⊤JλI)Δp−J⊤r,(⊤)Δ−⊤,其中 J∂rtotal/∂p∂total/∂ 为总残差对所有参数的雅可比矩阵ΔpΔ 为参数更新量。BA 的高效求解关键在于 J 的特殊稀疏结构。下面我们将具体推导单个残差块的雅可比并揭示其全局稀疏模式。4.1 单个残差的雅可比分解考虑一个观测 (i,j)∈Ω(,)∈Ω其残差为rijxij−^xij,^xijπ(K[Ri∣ti][Xj1]).−^,^([∣][1]).该残差仅依赖两个变量相机 i 的位姿 ξi 和 3D 点 j 的坐标 Xj。因此其雅可比可分解为两部分∂rij∂p⎡⎢ ⎢ ⎢⎣0⋯J(ij)ξi⋯0关于相机位姿,0⋯J(ij)Xj⋯0关于3D点⎤⎥ ⎥ ⎥⎦,∂∂[0⋯()⋯0⏟关于相机位姿,0⋯()⋯0⏟关于3D点],其中J(ij)ξi∂rij/∂ξi∈R2×6()∂/∂∈2×6J(ij)Xj∂rij/∂Xj∈R2×3()∂/∂∈2×3。这两部分可通过链式法则推导其核心思想是J(ij)ξi() 描述相机微小运动旋转平移对重投影的影响涉及李代数扰动模型。J(ij)Xj() 描述 3D 点微小移动对重投影位置的影响4.2 全局雅可比的稀疏结构从以上雅可比分解的推导可以看到每个残差块仅贡献两个非零子块其余位置全为零。当所有 K 个残差的雅可比垂直堆叠得到总雅可比矩阵 J∈R2K×(6N3M)∈2×(63)。其结构如下图所示Bundle Adjustment Jacobian Sparsity Columns (Optimization Variables) --------------------------------------------------------- | Cameras (6N) | Points (3M) | --------------------------------------------------------- Rows (Residuals) r11 | XXXX | XXX r12 | XXXX | XXX r13 | XXXX | XXX r21 | XXXX | XXX r22 | XXXX | XXX r31 | XXXX | XXX r41 | XXXX| XXX 每一行只有两个非零块 camera_i point_j每2 行对应一个观测 (i,j)(,)每行中仅有两个非零块一个在相机 i 对应的 6 列一个在 3D 点 j 对应的 3 列非零元素占比通常低于0.1%属于典型的块状稀疏矩阵Block-Sparse Matrix。4.3 稀疏求解优势尽管束平差BA的目标函数是非线性的但在 Gauss-Newton 或 Levenberg-Marquardt 框架下每一步迭代都归结为求解一个大型线性方程组HΔp−g,其中 HJ⊤J, gJ⊤r.Δ−,其中 ⊤, ⊤.这里 H∈R(6N3M)×(6N3M)∈(63)×(63) 是所谓的信息矩阵或 Hessian 近似g 是梯度向量。虽然原始雅可比矩阵 J 是高度稀疏的但其信息矩阵 HJ⊤J⊤ 通常是稠密的——因为两个相机若共视同一个 3D 点它们的参数块就会在 H 中产生非零耦合项。直接对 H 进行 Cholesky 分解的复杂度为 O((6N3M)3)((63)3)在大规模问题中不可接受。然而BA 的特殊结构——每个残差仅连接一个相机和一个 3D 点——使得 H 具有可利用的块结构。我们可以通过Schur Complement舒尔补技巧将原系统分解并消元从而大幅降低计算量。4.3.1 按变量类型重排系统我们将优化变量分为两类相机参数c[ξ⊤1,…,ξ⊤N]⊤∈R6N[1⊤,…,⊤]⊤∈63D点参数x[X⊤1,…,X⊤M]⊤ ∈R3M[1⊤,…,⊤]⊤ ∈3相应地将信息矩阵 H 和梯度 g 按此分块H[BEE⊤C],g[vw],[⊤],[],其中B∈R6N×6N∈6×6相机-相机块表示相机之间的耦合通过共视点间接产生C∈R3M×3M∈3×3点-点块E∈R6N×3M∈6×3相机-点交叉块。由于每个 3D 点仅被一组相机观测且不同 3D 点之间无共享残差因此 C 是一个块对角矩阵Cdiag(C1,C2,…,CM),diag⁡(1,2,…,),其中每个 Cj∈R3×3∈3×3 仅与观测到点 j 的相机有关。这意味着 C−1−1 可以并行、高效地计算只需对每个 3×3 块求逆。4.3.2消去 3D 点变量原线性系统为{BΔcEΔx−v(1)E⊤ΔcCΔx−w(2){ΔΔ−(1)⊤ΔΔ−(2)由方程 (2) 解出 ΔxΔΔx−C−1(E⊤Δcw).Δ−−1(⊤Δ).代入方程 (1)得到仅关于 ΔcΔ 的方程BΔc−EC−1(E⊤Δcw)−v,Δ−−1(⊤Δ)−,整理后即为 Schur Complement 系统(B−EC−1E⊤)Δc−(v−EC−1w)(−−1⊤)Δ−(−−1)4.3.3求解与回代求解简化系统新系统的规模仅为 6N×6N6×6。虽然 B−EC−1E⊤−−1⊤ 仍是稠密的称为“填充” fill-in但当 N≪M≪通常如此如 100 相机 vs 10,000 点其规模远小于原始 (6N3M)2(63)2。回代求 ΔxΔ一旦得到 ΔcΔ即可通过Δx−C−1(E⊤Δcw)Δ−−1(⊤Δ)快速计算所有 3D 点的更新量。由于 C 是块对角的这一步可完全并行化。计算复杂度对比与常规的 Cholesky 分解算法相比Schur Complement 方法的复杂度从 O((6N3M)3)((63)3) 下降到 O((6N)3M)((6)3) 可带来数个数量级的加速。5. 实际案例基于以上的理论推导这里设计了一个完整的束平差数值优化实现。为了能验证初始估计存在显著偏差全局优化仍能恢复高精度重建按照如下设计思路生成了实验数据构建一个由 5 个相机和 100 个 3D 点组成的简单航拍场景相机沿 Y 轴匀速旋转并向前平移模拟无人机飞行3D 点随机分布在相机前方一定深度范围内对真实重投影点添加高斯噪声标准差 1 像素模拟特征检测误差初始估计在真值基础上叠加小量噪声平移 ±0.1m点位置 ±0.1m模拟 SFM 前端输出。完整代码如下所示#include ceres/ceres.h #include ceres/rotation.h #include Eigen/Core #include Eigen/Geometry #include iostream #include random #include vector using namespace std; struct Camera { double q[4]; // quaternion double t[3]; // translation }; struct Point3D { double xyz[3]; }; struct Observation { int cam_id; int pt_id; double x; double y; }; struct ReprojectionError { ReprojectionError(double x, double y, double fx, double fy, double cx, double cy) : x_(x), y_(y), fx_(fx), fy_(fy), cx_(cx), cy_(cy) {} template typename T bool operator()(const T* const q, const T* const t, const T* const point, T* residuals) const { T p[3]; ceres::QuaternionRotatePoint(q, point, p); p[0] t[0]; p[1] t[1]; p[2] t[2]; T xp p[0] / p[2]; T yp p[1] / p[2]; T u T(fx_) * xp T(cx_); T v T(fy_) * yp T(cy_); residuals[0] u - T(x_); residuals[1] v - T(y_); return true; } static ceres::CostFunction* Create(double x, double y, double fx, double fy, double cx, double cy) { return new ceres::AutoDiffCostFunctionReprojectionError, 2, 4, 3, 3( new ReprojectionError(x, y, fx, fy, cx, cy)); } double x_, y_; double fx_, fy_, cx_, cy_; }; double ComputeRMSE(const vectorCamera cams, const vectorPoint3D pts, const vectorObservation obs, double fx, double fy, double cx, double cy) { double err 0; int n 0; for (auto o : obs) { const Camera c cams[o.cam_id]; const Point3D p pts[o.pt_id]; Eigen::Quaterniond q(c.q[0], c.q[1], c.q[2], c.q[3]); Eigen::Vector3d t(c.t[0], c.t[1], c.t[2]); Eigen::Vector3d P(p.xyz[0], p.xyz[1], p.xyz[2]); Eigen::Vector3d Pc q * P t; double u fx * Pc.x() / Pc.z() cx; double v fy * Pc.y() / Pc.z() cy; double du u - o.x; double dv v - o.y; err du * du dv * dv; n 2; } return sqrt(err / n); } double PoseError(const Camera a, const Camera b) { Eigen::Quaterniond qa(a.q[0], a.q[1], a.q[2], a.q[3]); Eigen::Quaterniond qb(b.q[0], b.q[1], b.q[2], b.q[3]); double rot qa.angularDistance(qb); Eigen::Vector3d ta(a.t[0], a.t[1], a.t[2]); Eigen::Vector3d tb(b.t[0], b.t[1], b.t[2]); double trans (ta - tb).norm(); return rot trans; } int main() { int num_cams 5; int num_pts 100; double fx 800, fy 800, cx 320, cy 240; vectorCamera gt_cams(num_cams); vectorPoint3D gt_pts(num_pts); vectorObservation obs; mt19937 rng(0); normal_distributiondouble noise(0, 1); // 生成真实相机 for (int i 0; i num_cams; i) { double ang i * 0.2; Eigen::Quaterniond q(Eigen::AngleAxisd(ang, Eigen::Vector3d::UnitY())); gt_cams[i].q[0] q.w(); gt_cams[i].q[1] q.x(); gt_cams[i].q[2] q.y(); gt_cams[i].q[3] q.z(); gt_cams[i].t[0] i * 0.5; gt_cams[i].t[1] 0; gt_cams[i].t[2] 0; } // 生成3D点 for (int i 0; i num_pts; i) { gt_pts[i].xyz[0] (rand() % 100) / 50.0 - 1; gt_pts[i].xyz[1] (rand() % 100) / 50.0 - 1; gt_pts[i].xyz[2] 4 (rand() % 100) / 50.0; } // 生成观测 for (int i 0; i num_cams; i) { Eigen::Quaterniond q(gt_cams[i].q[0], gt_cams[i].q[1], gt_cams[i].q[2], gt_cams[i].q[3]); Eigen::Vector3d t(gt_cams[i].t[0], gt_cams[i].t[1], gt_cams[i].t[2]); for (int j 0; j num_pts; j) { Eigen::Vector3d P(gt_pts[j].xyz[0], gt_pts[j].xyz[1], gt_pts[j].xyz[2]); Eigen::Vector3d Pc q * P t; double u fx * Pc.x() / Pc.z() cx; double v fy * Pc.y() / Pc.z() cy; Observation o; o.cam_id i; o.pt_id j; o.x u noise(rng); o.y v noise(rng); obs.push_back(o); } } vectorCamera est_cams gt_cams; vectorPoint3D est_pts gt_pts; // 添加初始噪声 for (auto c : est_cams) { for (int k 0; k 3; k) c.t[k] noise(rng) * 0.1; } for (auto p : est_pts) { for (int k 0; k 3; k) p.xyz[k] noise(rng) * 0.1; } cout Initial RMSE: ComputeRMSE(est_cams, est_pts, obs, fx, fy, cx, cy) endl; ceres::Problem problem; ceres::Manifold* q_manifold new ceres::QuaternionManifold(); for (int i 0; i num_cams; i) { problem.AddParameterBlock(est_cams[i].q, 4, q_manifold); problem.AddParameterBlock(est_cams[i].t, 3); } for (auto o : obs) { ceres::CostFunction* cost ReprojectionError::Create(o.x, o.y, fx, fy, cx, cy); problem.AddResidualBlock(cost, nullptr, est_cams[o.cam_id].q, est_cams[o.cam_id].t, est_pts[o.pt_id].xyz); } // Fix first camera problem.SetParameterBlockConstant(est_cams[0].q); problem.SetParameterBlockConstant(est_cams[0].t); ceres::Solver::Options options; options.linear_solver_type ceres::SPARSE_SCHUR; options.minimizer_progress_to_stdout true; options.max_num_iterations 50; ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); cout summary.BriefReport() endl; cout Final RMSE: ComputeRMSE(est_cams, est_pts, obs, fx, fy, cx, cy) endl; cout \nPose error\n; for (int i 0; i num_cams; i) { cout cam i : PoseError(gt_cams[i], est_cams[i]) endl; } return 0; }在这段代码实现中需要注意如下关键机制参数化与流形注册使用 AddParameterBlock 指定参数块的更新规则如绑定流形、设为常量等使用 AddResidualBlock 则将所引用的参数内存注册为优化变量默认采用欧氏流形无需额外声明本文理论推导采用李代数参数化以简化扰动模型实际代码实现中我们使用四元数平移并通过ceres::QuaternionManifold处理旋转约束二者在优化效果上等价。相机旋转以四元数q[4]存储并通过ceres::QuaternionManifold约束其单位性平移t[3]和 3D 点xyz[3]属于欧氏空间无需特殊流形虽然 3D 点未显式调用AddParameterBlock但 Ceres 会在AddResidualBlock中自动将其识别为优化变量默认欧氏流形。仅当需要覆盖默认行为如固定、加边界、非欧结构时才需显式注册。残差函数设计旋转变换使用了 Ceres 提供的ceres::QuaternionRotatePoint(q, point, p)而非直接调用 Eigen 的q * point。需要说明的是从自动微分的角度看Eigen 的四元数乘法本身是可导的Ceres 的AutoDiffCostFunction完全能够正确处理它——因此并非“不能用矩阵乘法”。使用 Ceres 专门提供的QuaternionRotatePoint这类底层几何操作函数主要出于以下工程考量数值鲁棒性该函数内部会对输入四元数进行归一化处理即使在优化过程中因浮点误差导致四元数轻微失范|q| ≠ 1仍能保证旋转结果有效求导链路优化作为标量函数实现其雅可比推导路径更短、更稳定避免了 Eigen 模板在Jet类型上实例化带来的潜在数值噪声或性能开销编译效率不依赖 Eigen 的泛型模板减少编译时间与二进制体积接口一致性与ceres::QuaternionManifold配套使用确保参数更新与几何操作遵循相同的四元数约定w, x, y, z。消除零空间自由度调用SetParameterBlockConstant固定第一帧相机的位姿q和t。这是 BA 能收敛的必要条件由于整个场景在 3D 空间中可任意刚体变换而不改变重投影误差Hessian 矩阵天然奇异。固定一个相机相当于设定世界坐标系原点消除 6 个不可观测自由度3 旋转 3 平移。稀疏求解器选择设置options.linear_solver_type ceres::SPARSE_SCHUR启用基于 Schur Complement 的稀疏求解策略。Ceres 会自动识别“相机-点”二分图结构构建并高效求解约简系统规模仅 6N×6N6×6大幅加速大规模问题求解。程序输出结果如下Initial RMSE: 30.1646 iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time 0 4.549507e05 0.00e00 1.87e06 0.00e00 0.00e00 1.00e04 0 6.63e-04 3.06e-03 1 8.934163e02 4.54e05 6.91e04 0.00e00 9.99e-01 3.00e04 1 3.16e-03 6.44e-03 2 3.666374e02 5.27e02 5.55e02 2.76e-01 1.00e00 9.00e04 1 6.72e-04 7.77e-03 3 3.664255e02 2.12e-01 1.75e02 8.00e-02 1.00e00 2.70e05 1 6.67e-04 8.90e-03 4 3.664170e02 8.57e-03 2.01e01 2.69e-02 1.00e00 8.10e05 1 6.15e-04 9.82e-03 Ceres Solver Report: Iterations: 5, Initial cost: 4.549507e05, Final cost: 3.664170e02, Termination: CONVERGENCE Final RMSE: 0.856057 Pose error cam 0 : 0.081944 cam 1 : 0.0811149 cam 2 : 0.0821392 cam 3 : 0.083811 cam 4 : 0.0855873从这个输出结果可以看到重投影误差显著下降初始 RMSE 高达30.16 像素严重失准经 5 次迭代后降至0.86 像素达到亚像素级精度表明 BA 成功校正了初始估计中的系统性偏差。位姿恢复准确各相机的位姿误差旋转角距离 平移范数稳定在 0.08 量级。注意第 0 帧误差非零是因为我们固定了其初始含噪估计值作为世界坐标系而非真实值其余帧均相对于此参考系优化因此误差具有可比性。