1. 项目概述从“最小二乘”到“束平差”如果你在三维重建、机器人SLAM或者计算机视觉领域摸爬滚打过一阵子那么“束平差”这个词对你来说一定不陌生。它听起来有点学术甚至有点吓人但说白了它就是解决一个核心问题的“终极武器”当我们用多张照片或者多帧传感器数据来重建一个三维场景时如何让所有观测到的二维点像素坐标和我们估计的三维点、相机姿态之间达到全局最优的匹配这个“最优”在数学上通常就体现为一个非线性最小二乘问题。所以今天我们不谈那些空中楼阁的理论就从一个资深从业者的视角把“束平差”这个听起来高大上的东西掰开了、揉碎了从最根本的最小二乘原理讲起一直聊到如何用Ceres Solver这样的工业级工具手把手实现一个基础但完整的束平差模块。你会发现它的核心思想其实非常直观而实现起来关键在于理解其“稀疏性”并善加利用。2. 核心原理最小二乘的几何与概率视角在深入束平差之前我们必须把它的基石——最小二乘——彻底搞明白。很多人一上来就套公式但如果不理解其背后的“为什么”调参和debug时会非常痛苦。2.1 从直线拟合到投影降维让我们从一个最简单的例子开始用一堆散点拟合一条直线y ax b。最小二乘的目标是找到参数a和b使得所有数据点到这条直线的垂直距离残差的平方和最小。为什么是平方和而不是绝对值和这背后有深刻的几何与概率意义。从几何角度看我们可以把每个观测数据点(x_i, y_i)看作一个高维空间维度等于数据点个数中的一个向量。我们寻找的直线实际上是在所有可能的直线一个二维子空间中找到一个点由a, b参数化使得这个点与观测向量之间的欧几里得距离最短。在欧几里得空间中距离就是平方和的平方根最小化距离等价于最小化平方和。这本质上是将观测向量投影到由模型参数张成的子空间上残差向量正是垂直于该子空间的分量。从概率角度看如果我们假设每个观测值的噪声是独立同分布的高斯噪声均值为0那么最大化观测数据的似然函数恰好等价于最小化残差的平方和。高斯分布的概率密度函数中指数项就是-残差^2 / (2*方差)。因此最小二乘解在这个假设下是最大似然估计。虽然现实中的数据噪声未必严格服从高斯分布但中心极限定理告诉我们许多独立微小误差的叠加会趋向于高斯分布这使得最小二乘成为一个极其鲁棒和通用的起点。注意理解“投影”和“最大似然”这两个视角至关重要。当问题从直线拟合升级到复杂的束平差时模型变成了三维点投影到二维像平面的非线性函数但“寻找一个参数空间使得观测值在该参数空间下的投影与自身最接近”这一几何本质没有变。而高斯噪声假设则为我们后续分析协方差、评估优化结果的不确定性提供了理论基础。2.2 非线性最小二乘与迭代求解束平差中的模型——相机成像模型通常为针孔模型加畸变——是关于三维点坐标和相机参数的非线性函数。因此我们面对的是一个非线性最小二乘问题。设我们有m个观测值例如所有图像特征点的像素坐标n个待优化参数所有三维点坐标和相机位姿。我们的目标是最小化目标函数F(x) 1/2 * Σ_i || f_i(x) ||^2其中x是包含所有待优化参数的向量f_i(x)是第i个观测值的残差预测值减去观测值。对于非线性函数我们无法直接求解导数为零的方程。标准的方法是迭代优化从初始猜测x_0开始每次迭代寻找一个增量Δx使得F(x Δx) F(x)。最常用的方法是高斯-牛顿法和列文伯格-马夸尔特法。高斯-牛顿法的核心是对非线性函数f_i(x)在当前估计值x_k处进行一阶泰勒展开f_i(x_k Δx) ≈ f_i(x_k) J_i(x_k) Δx其中J_i是f_i关于x的雅可比矩阵导数。将其代入目标函数我们得到一个关于Δx的线性最小二乘问题min_Δx 1/2 * Σ_i || J_i(x_k) Δx f_i(x_k) ||^2这可以写成正规方程(J^T J) Δx -J^T f其中J是所有J_i堆叠而成的雅可比矩阵f是所有残差堆叠的向量。求解这个线性方程得到Δx然后更新x_{k1} x_k Δx。列文伯格-马夸尔特法是高斯-牛顿法的改进它通过引入一个阻尼因子λ来解决J^T J可能奇异或病态的问题其正规方程变为(J^T J λ I) Δx -J^T f。LM算法会根据本次迭代的效果动态调整λ如果误差下降则减小λ更接近高斯-牛顿法收敛快如果误差上升则增大λ更接近梯度下降法更稳定。这种自适应机制使其成为求解非线性最小二乘问题的实际标准。3. 束平差问题建模与稀疏性挖掘现在我们将最小二乘框架套用到束平差问题上。假设我们有p个相机或关键帧q个三维路标点。第j个相机观测到了第i个路标点产生了一个二维像素观测值z_{ij}。3.1 残差函数定义残差r_{ij}定义为观测值与预测值之差r_{ij} z_{ij} - π(R_j * X_i t_j)其中X_i是世界坐标系下的第i个三维点坐标。R_j,t_j是第j个相机的旋转矩阵和平移向量即相机位姿。π(·)是相机投影函数包括内参焦距、主点和可能的畸变模型将三维相机坐标系下的点投影到二维像素平面。因此束平差的总目标函数是min_{ {R, t}, {X} } 1/2 * Σ_{i,j} || z_{ij} - π(R_j * X_i t_j) ||^2我们需要同时优化所有相机位姿{R, t}和所有三维点坐标{X}。3.2 雅可比矩阵的稀疏结构与舒尔补Schur Complement这是束平差高效求解的灵魂所在。让我们看看雅可比矩阵J长什么样。我们将所有待优化参数排列成向量x [P_1, ..., P_p, X_1, ..., X_q]^T其中P_j代表第j个相机的参数如旋转的李代数、平移、内参等X_i代表第i个三维点。对于残差r_{ij}它只与相机参数P_j和点参数X_i有关与其他相机和其他点无关。因此它的雅可比矩阵J_{ij}是一个“矮胖”的矩阵例如2行对应像素坐标u,v并且只有对应P_j和X_i的两块是非零的J_{ij} [∂r_{ij}/∂P_j, 0, ..., 0, ∂r_{ij}/∂X_i, 0, ..., 0]将所有残差的雅可比矩阵按行堆叠起来得到全局雅可比矩阵J。这个矩阵具有显著的块稀疏结构。相应地高斯-牛顿法中的海塞矩阵近似H J^T J也具有分块结构H [ B E ] [ E^T C ]其中B是一个对角块矩阵更准确地说是块对角占优每个对角块B_jj对应一个相机参数P_j由所有观测到该相机的残差贡献而来。B的维度是相机参数总维度。C也是一个对角块矩阵每个对角块C_ii对应一个三维点X_i由所有观测到该点的残差贡献而来。C的维度是点参数总维度。E是一个稀疏矩阵块E_ji非零当且仅当相机j观测到了点i。E的维度是相机参数维度 × 点参数维度。正规方程H Δx -J^T f可以写为[ B E ] [ ΔP ] [ v ] [ E^T C ] [ ΔX ] [ w ]这里ΔP是所有相机参数的增量ΔX是所有三维点的增量v和w是相应的梯度项。直接求解这个大型线性方程成本极高。但利用其结构我们可以使用舒尔消元Schur Elimination。先从第二个方程解出ΔX关于ΔP的表达式ΔX C^{-1} (w - E^T ΔP)。将其代入第一个方程得到只关于ΔP的方程(B - E C^{-1} E^T) ΔP v - E C^{-1} w这个新的系数矩阵S B - E C^{-1} E^T被称为舒尔补。关键点在于S的维度只与相机数量有关通常远小于原海塞矩阵H的维度因为点的数量远多于相机。S本身也是一个稀疏矩阵其稀疏模式是相机之间的共视关系如果两个相机共同观测到至少一个相同的三维点则S中对应的非对角块非零。求解出ΔP后再回代求解ΔX就非常快了。这种先求解相机增量再求解点增量的方法就是著名的边缘化Marginalization三维点。它极大地降低了计算复杂度是大型束平差得以实现的关键。4. 基础实现使用Ceres Solver构建BA问题理论讲透了我们来看实战。Ceres Solver是一个广泛使用的开源C库专门用于求解大规模非线性最小二乘问题。它内置了对舒尔补稀疏求解的支持是我们实现束平差的利器。4.1 定义代价函数与参数块首先我们需要定义残差计算方式。在Ceres中这通过继承ceres::SizedCostFunction或使用自动微分模板ceres::AutoDiffCostFunction来实现。这里我们展示自动微分的方式它最方便。假设我们有一个简单的针孔相机模型忽略畸变// 简单的针孔投影函数 template typename T bool Project(const T* const camera, // [f, cx, cy, qw, qx, qy, qz, tx, ty, tz] const T* const point, // [x, y, z] T* predictions) { // 1. 内参 T f camera[0]; T cx camera[1]; T cy camera[2]; // 2. 将点从世界坐标系变换到相机坐标系 (使用四元数表示旋转) T qw camera[3], qx camera[4], qy camera[5], qz camera[6]; T tx camera[7], ty camera[8], tz camera[9]; // 四元数旋转 T p[3]; // 相机坐标系下的点 p[0] T(2) * (qw*qw qx*qx - T(0.5)) * point[0] T(2) * (qx*qy - qw*qz) * point[1] T(2) * (qx*qz qw*qy) * point[2] tx; p[1] T(2) * (qx*qy qw*qz) * point[0] T(2) * (qw*qw qy*qy - T(0.5)) * point[1] T(2) * (qy*qz - qw*qx) * point[2] ty; p[2] T(2) * (qx*qz - qw*qy) * point[0] T(2) * (qy*qz qw*qx) * point[1] T(2) * (qw*qw qz*qz - T(0.5)) * point[2] tz; // 3. 投影 T xp p[0] / p[2]; T yp p[1] / p[2]; // 4. 应用内参 predictions[0] f * xp cx; predictions[1] f * yp cy; return true; } // 定义残差计算仿函数 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, const T* const point, T* residuals) const { T predictions[2]; Project(camera, point, predictions); // 残差 预测 - 观测 residuals[0] predictions[0] - T(observed_x); residuals[1] predictions[1] - T(observed_y); return true; } static ceres::CostFunction* Create(const double observed_x, const double observed_y) { // 自动微分残差维度2相机参数维度10点参数维度3 return new ceres::AutoDiffCostFunctionReprojectionError, 2, 10, 3( new ReprojectionError(observed_x, observed_y)); } double observed_x; double observed_y; };4.2 构建问题并配置求解器接下来我们构建优化问题并添加所有的残差项观测值。void RunBundleAdjustment(const std::vectorCamera cameras, std::vectorPoint3D points, const std::vectorObservation observations) { ceres::Problem problem; // 1. 添加参数块 for (size_t i 0; i cameras.size(); i) { // 假设 camera.data() 返回 double[10] 数组 problem.AddParameterBlock(cameras[i].data(), 10); // 可以设置局部参数化例如对于四元数需要保持其为单位四元数 problem.SetParameterization(cameras[i].data(), new ceres::EigenQuaternionParameterization()); } for (size_t j 0; j points.size(); j) { problem.AddParameterBlock(points[j].data(), 3); } // 2. 添加残差块 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 ReprojectionError::Create(observed_x, observed_y); problem.AddResidualBlock(cost_function, nullptr, // 损失函数例如Huber核函数用于鲁棒性 cameras[camera_id].data(), points[point_id].data()); } // 3. 配置求解器选项启用稀疏求解 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; // 4. 运行优化 ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); // 5. 输出结果 std::cout summary.FullReport() \n; std::cout Initial cost: summary.initial_cost \n; std::cout Final cost: summary.final_cost \n; }实操心得在配置ceres::Solver::Options时linear_solver_type的选择至关重要。对于典型的BA问题相机少点多SPARSE_SCHUR是最优选择它内部会利用我们前面分析的稀疏舒尔补结构。如果你的问题中相机参数也特别多且共视关系非常稠密可以尝试ITERATIVE_SCHUR配合预条件子如SCHUR_JACOBI。对于小规模问题DENSE_SCHUR或DENSE_NORMAL_CHOLESKY也可以。务必根据问题规模选择否则内存和计算时间会爆炸。5. 稀疏求解器的选择与配置细节当我们选择了SPARSE_SCHURCeres会调用底层的稀疏线性代数库来求解舒尔补方程S ΔP rhs。Ceres支持多种后端SuiteSparse(默认如果安装)使用CHOLMOD或SPQR进行稀疏Cholesky分解 (LDL^T)。这是最稳定、功能最全的后端支持稀疏矩阵的填充约化排序如AMD、COLAMD能极大提升分解速度。强烈推荐在Linux/macOS上安装SuiteSparse。CXSparseSuiteSparse的一个轻量子集功能较少但无需额外安装。Eigen Sparse纯头文件库无需安装。在小型到中型问题上表现不错但对于超大规模稀疏矩阵其性能和稳定性可能不如SuiteSparse。Accelerate(macOS)苹果系统的加速框架。CUDA如果安装了Ceres的CUDA支持可以使用cuSolver进行GPU加速对于超大规模BA有数量级的提升。编译和安装时通常建议# 安装SuiteSparse依赖 (Ubuntu) sudo apt-get install libsuitesparse-dev # 编译Ceres时CMake会自动检测并启用SuiteSparse支持在代码中我们可以通过options.sparse_linear_algebra_library_type和options.linear_solver_ordering进行更精细的控制。例如可以指定使用AMD排序options.sparse_linear_algebra_library_type ceres::SUITE_SPARSE; options.linear_solver_ordering.reset(new ceres::ParameterBlockOrdering); // 构建排序规则通常先优化相机再优化点这符合舒尔消元的顺序 for (size_t i 0; i cameras.size(); i) { ordering-AddElementToGroup(cameras[i].data(), 0); } for (size_t j 0; j points.size(); j) { ordering-AddElementToGroup(points[j].data(), 1); }6. 鲁棒核函数与异常值抑制在实际的视觉数据中误匹配Outliers是无法避免的。这些错误的观测值会产生巨大的残差严重干扰最小二乘优化因为最小二乘对大的残差惩罚非常重平方项。解决方案是使用鲁棒核函数。核函数的作用是修改损失函数ρ(s)其中s ||r||^2。原始的最小二乘对应ρ(s) s。鲁棒核函数会对大的残差进行“压制”使其对总目标函数的贡献饱和。Ceres内置了多种核函数Huber Loss:ρ(s) s if s δ^2; else 2δ√s - δ^2。在δ以内是二次函数以外是一次函数。这是最常用的核函数能温和地抑制异常值。Cauchy Loss:ρ(s) c^2 log(1 s / c^2)。对异常值的抑制更强。SoftLOne Loss:ρ(s) 2b (√(1s/b) - 1)。行为类似Huber但过渡更平滑。在添加残差块时将nullptr替换为核函数对象ceres::LossFunction* loss_function new ceres::HuberLoss(1.0); // δ1.0像素 problem.AddResidualBlock(cost_function, loss_function, // 使用Huber核 cameras[camera_id].data(), points[point_id].data());注意事项核函数中的尺度参数如Huber的δ需要根据你的问题设定。一个经验法则是δ可以设置为重投影误差的合理阈值例如1-2个像素。设置太小会过度抑制有效数据设置太大则起不到鲁棒作用。通常可以从1.0开始根据优化后残差的分布进行调整。7. 参数化与流形上的优化在BA中相机姿态的旋转部分通常用旋转矩阵R或四元数q表示。然而它们都存在约束旋转矩阵需要正交且行列式为1四元数需要是单位四元数。直接在欧氏空间R^9或R^4中优化这些参数很容易破坏约束导致无效的旋转。解决方案是使用流形优化。对于旋转我们在其切空间李代数如so(3)中进行优化这是一个无约束的3维空间然后通过指数映射更新到流形旋转矩阵或四元数上。Ceres通过LocalParameterization新版本中为Manifold接口支持这一点。对于四元数我们使用内置的EigenQuaternionParameterizationproblem.SetParameterization(camera_rotation_parameter_ptr, new ceres::EigenQuaternionParameterization());对于旋转矩阵可以使用ceres::ProductParameterization组合多个参数化或者使用李代数如角轴作为参数并自定义参数化。同样如果使用了逆深度参数化点或者对相机内参使用了某种参数化如焦距使用其对数也需要设置相应的LocalParameterization来保证优化过程的正确性和数值稳定性。8. 初始化、收敛性与调试技巧非线性优化严重依赖于初始值。一个糟糕的初始值会导致优化陷入局部极小值甚至发散。初始化策略相机位姿通常从SFM运动恢复结构或视觉里程计中获得初始估计。对于单目SLAM可能需要从对极几何或PnP求解。三维点通过三角化获得。内参如果未标定可以设定一个合理的初始值如焦距近似为图像宽度并考虑将其在优化中固定或设置较小的方差。收敛性判断关注Solver::Summary中的termination_type。CONVERGENCE是理想状态。查看initial_cost和final_cost的下降幅度。通常期望下降2-3个数量级。检查iterations次数如果达到最大迭代次数仍未收敛可能需要调整初始值或优化策略。调试与可视化输出每次迭代的代价设置options.minimizer_progress_to_stdout true。分析残差优化后遍历所有残差统计其均值和方差绘制直方图。健康的优化结果残差应近似服从均值为0的高斯分布在核函数影响范围内。可视化重投影误差在图像上画出特征点位置和重投影后的位置直观查看误差大小和分布。使用Ceres的检查工具options.check_gradients true可以检查雅可比矩阵计算是否正确与有限差分对比。options.gradient_check_relative_precision 1e-6。性能调优线程数设置options.num_threads为你的CPU核心数以并行计算雅可比矩阵和残差。内存使用对于超大规模问题SPARSE_SCHUR可能仍然内存不足。可以考虑使用ITERATIVE_SCHUR配合SCHUR_JACOBI预条件子它是无矩阵的内存占用少但可能需要更多迭代。参数块排序一个好的排序能极大减少稀疏Cholesky分解的填充元提升速度。除了默认的相机优先排序对于特定场景如循环闭合可以尝试更复杂的排序策略。9. 从基础BA到实际系统集成一个基础的BA模块实现后要集成到实际的SLAM或SfM系统中还需要考虑更多工程细节关键帧与地图点管理不是所有帧和点都参与全局BA。通常选择关键帧和其观测到的共视地图点。需要维护一个共视图来高效地管理这些关系。增量式BA当系统运行时频繁进行全局BA代价太高。通常采用局部BA优化当前帧及其共视关键帧和地图点和位姿图优化只优化关键帧位姿将地图点约束边化掉。全局BA作为后台线程偶尔运行。外点剔除在优化前和优化中需要主动剔除误匹配。可以在优化几轮后将重投影误差大于阈值的观测标记为外点并移除。尺度不确定性单目单目BA存在尺度模糊性。通常需要固定第一个关键帧的位姿和尺度如固定其平移为0旋转为单位阵并固定某个点的深度或某两个点之间的距离。使用先验信息如果某些参数已知很准确如标定好的相机内参可以在优化中将其固定 (problem.SetParameterBlockConstant) 或添加先验约束如使用ceres::CostFunction添加一个惩罚其偏离初始值的残差项。实现一个稳定高效的BA模块是视觉SLAM/SfM系统的核心。它要求我们对非线性优化理论、稀疏线性代数、计算机视觉几何都有深入的理解。从最小二乘的基本原理出发一步步推导到稀疏舒尔补再到用Ceres实现这个过程本身就是一个将理论扎实落地的绝佳范例。在实际操作中耐心调试参数、分析残差、理解每一次迭代的行为比单纯调通代码更重要。
1. 项目概述从“最小二乘”到“束平差”如果你在三维重建、机器人SLAM或者计算机视觉领域摸爬滚打过一阵子那么“束平差”这个词对你来说一定不陌生。它听起来有点学术甚至有点吓人但说白了它就是解决一个核心问题的“终极武器”当我们用多张照片或者多帧传感器数据来重建一个三维场景时如何让所有观测到的二维点像素坐标和我们估计的三维点、相机姿态之间达到全局最优的匹配这个“最优”在数学上通常就体现为一个非线性最小二乘问题。所以今天我们不谈那些空中楼阁的理论就从一个资深从业者的视角把“束平差”这个听起来高大上的东西掰开了、揉碎了从最根本的最小二乘原理讲起一直聊到如何用Ceres Solver这样的工业级工具手把手实现一个基础但完整的束平差模块。你会发现它的核心思想其实非常直观而实现起来关键在于理解其“稀疏性”并善加利用。2. 核心原理最小二乘的几何与概率视角在深入束平差之前我们必须把它的基石——最小二乘——彻底搞明白。很多人一上来就套公式但如果不理解其背后的“为什么”调参和debug时会非常痛苦。2.1 从直线拟合到投影降维让我们从一个最简单的例子开始用一堆散点拟合一条直线y ax b。最小二乘的目标是找到参数a和b使得所有数据点到这条直线的垂直距离残差的平方和最小。为什么是平方和而不是绝对值和这背后有深刻的几何与概率意义。从几何角度看我们可以把每个观测数据点(x_i, y_i)看作一个高维空间维度等于数据点个数中的一个向量。我们寻找的直线实际上是在所有可能的直线一个二维子空间中找到一个点由a, b参数化使得这个点与观测向量之间的欧几里得距离最短。在欧几里得空间中距离就是平方和的平方根最小化距离等价于最小化平方和。这本质上是将观测向量投影到由模型参数张成的子空间上残差向量正是垂直于该子空间的分量。从概率角度看如果我们假设每个观测值的噪声是独立同分布的高斯噪声均值为0那么最大化观测数据的似然函数恰好等价于最小化残差的平方和。高斯分布的概率密度函数中指数项就是-残差^2 / (2*方差)。因此最小二乘解在这个假设下是最大似然估计。虽然现实中的数据噪声未必严格服从高斯分布但中心极限定理告诉我们许多独立微小误差的叠加会趋向于高斯分布这使得最小二乘成为一个极其鲁棒和通用的起点。注意理解“投影”和“最大似然”这两个视角至关重要。当问题从直线拟合升级到复杂的束平差时模型变成了三维点投影到二维像平面的非线性函数但“寻找一个参数空间使得观测值在该参数空间下的投影与自身最接近”这一几何本质没有变。而高斯噪声假设则为我们后续分析协方差、评估优化结果的不确定性提供了理论基础。2.2 非线性最小二乘与迭代求解束平差中的模型——相机成像模型通常为针孔模型加畸变——是关于三维点坐标和相机参数的非线性函数。因此我们面对的是一个非线性最小二乘问题。设我们有m个观测值例如所有图像特征点的像素坐标n个待优化参数所有三维点坐标和相机位姿。我们的目标是最小化目标函数F(x) 1/2 * Σ_i || f_i(x) ||^2其中x是包含所有待优化参数的向量f_i(x)是第i个观测值的残差预测值减去观测值。对于非线性函数我们无法直接求解导数为零的方程。标准的方法是迭代优化从初始猜测x_0开始每次迭代寻找一个增量Δx使得F(x Δx) F(x)。最常用的方法是高斯-牛顿法和列文伯格-马夸尔特法。高斯-牛顿法的核心是对非线性函数f_i(x)在当前估计值x_k处进行一阶泰勒展开f_i(x_k Δx) ≈ f_i(x_k) J_i(x_k) Δx其中J_i是f_i关于x的雅可比矩阵导数。将其代入目标函数我们得到一个关于Δx的线性最小二乘问题min_Δx 1/2 * Σ_i || J_i(x_k) Δx f_i(x_k) ||^2这可以写成正规方程(J^T J) Δx -J^T f其中J是所有J_i堆叠而成的雅可比矩阵f是所有残差堆叠的向量。求解这个线性方程得到Δx然后更新x_{k1} x_k Δx。列文伯格-马夸尔特法是高斯-牛顿法的改进它通过引入一个阻尼因子λ来解决J^T J可能奇异或病态的问题其正规方程变为(J^T J λ I) Δx -J^T f。LM算法会根据本次迭代的效果动态调整λ如果误差下降则减小λ更接近高斯-牛顿法收敛快如果误差上升则增大λ更接近梯度下降法更稳定。这种自适应机制使其成为求解非线性最小二乘问题的实际标准。3. 束平差问题建模与稀疏性挖掘现在我们将最小二乘框架套用到束平差问题上。假设我们有p个相机或关键帧q个三维路标点。第j个相机观测到了第i个路标点产生了一个二维像素观测值z_{ij}。3.1 残差函数定义残差r_{ij}定义为观测值与预测值之差r_{ij} z_{ij} - π(R_j * X_i t_j)其中X_i是世界坐标系下的第i个三维点坐标。R_j,t_j是第j个相机的旋转矩阵和平移向量即相机位姿。π(·)是相机投影函数包括内参焦距、主点和可能的畸变模型将三维相机坐标系下的点投影到二维像素平面。因此束平差的总目标函数是min_{ {R, t}, {X} } 1/2 * Σ_{i,j} || z_{ij} - π(R_j * X_i t_j) ||^2我们需要同时优化所有相机位姿{R, t}和所有三维点坐标{X}。3.2 雅可比矩阵的稀疏结构与舒尔补Schur Complement这是束平差高效求解的灵魂所在。让我们看看雅可比矩阵J长什么样。我们将所有待优化参数排列成向量x [P_1, ..., P_p, X_1, ..., X_q]^T其中P_j代表第j个相机的参数如旋转的李代数、平移、内参等X_i代表第i个三维点。对于残差r_{ij}它只与相机参数P_j和点参数X_i有关与其他相机和其他点无关。因此它的雅可比矩阵J_{ij}是一个“矮胖”的矩阵例如2行对应像素坐标u,v并且只有对应P_j和X_i的两块是非零的J_{ij} [∂r_{ij}/∂P_j, 0, ..., 0, ∂r_{ij}/∂X_i, 0, ..., 0]将所有残差的雅可比矩阵按行堆叠起来得到全局雅可比矩阵J。这个矩阵具有显著的块稀疏结构。相应地高斯-牛顿法中的海塞矩阵近似H J^T J也具有分块结构H [ B E ] [ E^T C ]其中B是一个对角块矩阵更准确地说是块对角占优每个对角块B_jj对应一个相机参数P_j由所有观测到该相机的残差贡献而来。B的维度是相机参数总维度。C也是一个对角块矩阵每个对角块C_ii对应一个三维点X_i由所有观测到该点的残差贡献而来。C的维度是点参数总维度。E是一个稀疏矩阵块E_ji非零当且仅当相机j观测到了点i。E的维度是相机参数维度 × 点参数维度。正规方程H Δx -J^T f可以写为[ B E ] [ ΔP ] [ v ] [ E^T C ] [ ΔX ] [ w ]这里ΔP是所有相机参数的增量ΔX是所有三维点的增量v和w是相应的梯度项。直接求解这个大型线性方程成本极高。但利用其结构我们可以使用舒尔消元Schur Elimination。先从第二个方程解出ΔX关于ΔP的表达式ΔX C^{-1} (w - E^T ΔP)。将其代入第一个方程得到只关于ΔP的方程(B - E C^{-1} E^T) ΔP v - E C^{-1} w这个新的系数矩阵S B - E C^{-1} E^T被称为舒尔补。关键点在于S的维度只与相机数量有关通常远小于原海塞矩阵H的维度因为点的数量远多于相机。S本身也是一个稀疏矩阵其稀疏模式是相机之间的共视关系如果两个相机共同观测到至少一个相同的三维点则S中对应的非对角块非零。求解出ΔP后再回代求解ΔX就非常快了。这种先求解相机增量再求解点增量的方法就是著名的边缘化Marginalization三维点。它极大地降低了计算复杂度是大型束平差得以实现的关键。4. 基础实现使用Ceres Solver构建BA问题理论讲透了我们来看实战。Ceres Solver是一个广泛使用的开源C库专门用于求解大规模非线性最小二乘问题。它内置了对舒尔补稀疏求解的支持是我们实现束平差的利器。4.1 定义代价函数与参数块首先我们需要定义残差计算方式。在Ceres中这通过继承ceres::SizedCostFunction或使用自动微分模板ceres::AutoDiffCostFunction来实现。这里我们展示自动微分的方式它最方便。假设我们有一个简单的针孔相机模型忽略畸变// 简单的针孔投影函数 template typename T bool Project(const T* const camera, // [f, cx, cy, qw, qx, qy, qz, tx, ty, tz] const T* const point, // [x, y, z] T* predictions) { // 1. 内参 T f camera[0]; T cx camera[1]; T cy camera[2]; // 2. 将点从世界坐标系变换到相机坐标系 (使用四元数表示旋转) T qw camera[3], qx camera[4], qy camera[5], qz camera[6]; T tx camera[7], ty camera[8], tz camera[9]; // 四元数旋转 T p[3]; // 相机坐标系下的点 p[0] T(2) * (qw*qw qx*qx - T(0.5)) * point[0] T(2) * (qx*qy - qw*qz) * point[1] T(2) * (qx*qz qw*qy) * point[2] tx; p[1] T(2) * (qx*qy qw*qz) * point[0] T(2) * (qw*qw qy*qy - T(0.5)) * point[1] T(2) * (qy*qz - qw*qx) * point[2] ty; p[2] T(2) * (qx*qz - qw*qy) * point[0] T(2) * (qy*qz qw*qx) * point[1] T(2) * (qw*qw qz*qz - T(0.5)) * point[2] tz; // 3. 投影 T xp p[0] / p[2]; T yp p[1] / p[2]; // 4. 应用内参 predictions[0] f * xp cx; predictions[1] f * yp cy; return true; } // 定义残差计算仿函数 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, const T* const point, T* residuals) const { T predictions[2]; Project(camera, point, predictions); // 残差 预测 - 观测 residuals[0] predictions[0] - T(observed_x); residuals[1] predictions[1] - T(observed_y); return true; } static ceres::CostFunction* Create(const double observed_x, const double observed_y) { // 自动微分残差维度2相机参数维度10点参数维度3 return new ceres::AutoDiffCostFunctionReprojectionError, 2, 10, 3( new ReprojectionError(observed_x, observed_y)); } double observed_x; double observed_y; };4.2 构建问题并配置求解器接下来我们构建优化问题并添加所有的残差项观测值。void RunBundleAdjustment(const std::vectorCamera cameras, std::vectorPoint3D points, const std::vectorObservation observations) { ceres::Problem problem; // 1. 添加参数块 for (size_t i 0; i cameras.size(); i) { // 假设 camera.data() 返回 double[10] 数组 problem.AddParameterBlock(cameras[i].data(), 10); // 可以设置局部参数化例如对于四元数需要保持其为单位四元数 problem.SetParameterization(cameras[i].data(), new ceres::EigenQuaternionParameterization()); } for (size_t j 0; j points.size(); j) { problem.AddParameterBlock(points[j].data(), 3); } // 2. 添加残差块 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 ReprojectionError::Create(observed_x, observed_y); problem.AddResidualBlock(cost_function, nullptr, // 损失函数例如Huber核函数用于鲁棒性 cameras[camera_id].data(), points[point_id].data()); } // 3. 配置求解器选项启用稀疏求解 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; // 4. 运行优化 ceres::Solver::Summary summary; ceres::Solve(options, problem, summary); // 5. 输出结果 std::cout summary.FullReport() \n; std::cout Initial cost: summary.initial_cost \n; std::cout Final cost: summary.final_cost \n; }实操心得在配置ceres::Solver::Options时linear_solver_type的选择至关重要。对于典型的BA问题相机少点多SPARSE_SCHUR是最优选择它内部会利用我们前面分析的稀疏舒尔补结构。如果你的问题中相机参数也特别多且共视关系非常稠密可以尝试ITERATIVE_SCHUR配合预条件子如SCHUR_JACOBI。对于小规模问题DENSE_SCHUR或DENSE_NORMAL_CHOLESKY也可以。务必根据问题规模选择否则内存和计算时间会爆炸。5. 稀疏求解器的选择与配置细节当我们选择了SPARSE_SCHURCeres会调用底层的稀疏线性代数库来求解舒尔补方程S ΔP rhs。Ceres支持多种后端SuiteSparse(默认如果安装)使用CHOLMOD或SPQR进行稀疏Cholesky分解 (LDL^T)。这是最稳定、功能最全的后端支持稀疏矩阵的填充约化排序如AMD、COLAMD能极大提升分解速度。强烈推荐在Linux/macOS上安装SuiteSparse。CXSparseSuiteSparse的一个轻量子集功能较少但无需额外安装。Eigen Sparse纯头文件库无需安装。在小型到中型问题上表现不错但对于超大规模稀疏矩阵其性能和稳定性可能不如SuiteSparse。Accelerate(macOS)苹果系统的加速框架。CUDA如果安装了Ceres的CUDA支持可以使用cuSolver进行GPU加速对于超大规模BA有数量级的提升。编译和安装时通常建议# 安装SuiteSparse依赖 (Ubuntu) sudo apt-get install libsuitesparse-dev # 编译Ceres时CMake会自动检测并启用SuiteSparse支持在代码中我们可以通过options.sparse_linear_algebra_library_type和options.linear_solver_ordering进行更精细的控制。例如可以指定使用AMD排序options.sparse_linear_algebra_library_type ceres::SUITE_SPARSE; options.linear_solver_ordering.reset(new ceres::ParameterBlockOrdering); // 构建排序规则通常先优化相机再优化点这符合舒尔消元的顺序 for (size_t i 0; i cameras.size(); i) { ordering-AddElementToGroup(cameras[i].data(), 0); } for (size_t j 0; j points.size(); j) { ordering-AddElementToGroup(points[j].data(), 1); }6. 鲁棒核函数与异常值抑制在实际的视觉数据中误匹配Outliers是无法避免的。这些错误的观测值会产生巨大的残差严重干扰最小二乘优化因为最小二乘对大的残差惩罚非常重平方项。解决方案是使用鲁棒核函数。核函数的作用是修改损失函数ρ(s)其中s ||r||^2。原始的最小二乘对应ρ(s) s。鲁棒核函数会对大的残差进行“压制”使其对总目标函数的贡献饱和。Ceres内置了多种核函数Huber Loss:ρ(s) s if s δ^2; else 2δ√s - δ^2。在δ以内是二次函数以外是一次函数。这是最常用的核函数能温和地抑制异常值。Cauchy Loss:ρ(s) c^2 log(1 s / c^2)。对异常值的抑制更强。SoftLOne Loss:ρ(s) 2b (√(1s/b) - 1)。行为类似Huber但过渡更平滑。在添加残差块时将nullptr替换为核函数对象ceres::LossFunction* loss_function new ceres::HuberLoss(1.0); // δ1.0像素 problem.AddResidualBlock(cost_function, loss_function, // 使用Huber核 cameras[camera_id].data(), points[point_id].data());注意事项核函数中的尺度参数如Huber的δ需要根据你的问题设定。一个经验法则是δ可以设置为重投影误差的合理阈值例如1-2个像素。设置太小会过度抑制有效数据设置太大则起不到鲁棒作用。通常可以从1.0开始根据优化后残差的分布进行调整。7. 参数化与流形上的优化在BA中相机姿态的旋转部分通常用旋转矩阵R或四元数q表示。然而它们都存在约束旋转矩阵需要正交且行列式为1四元数需要是单位四元数。直接在欧氏空间R^9或R^4中优化这些参数很容易破坏约束导致无效的旋转。解决方案是使用流形优化。对于旋转我们在其切空间李代数如so(3)中进行优化这是一个无约束的3维空间然后通过指数映射更新到流形旋转矩阵或四元数上。Ceres通过LocalParameterization新版本中为Manifold接口支持这一点。对于四元数我们使用内置的EigenQuaternionParameterizationproblem.SetParameterization(camera_rotation_parameter_ptr, new ceres::EigenQuaternionParameterization());对于旋转矩阵可以使用ceres::ProductParameterization组合多个参数化或者使用李代数如角轴作为参数并自定义参数化。同样如果使用了逆深度参数化点或者对相机内参使用了某种参数化如焦距使用其对数也需要设置相应的LocalParameterization来保证优化过程的正确性和数值稳定性。8. 初始化、收敛性与调试技巧非线性优化严重依赖于初始值。一个糟糕的初始值会导致优化陷入局部极小值甚至发散。初始化策略相机位姿通常从SFM运动恢复结构或视觉里程计中获得初始估计。对于单目SLAM可能需要从对极几何或PnP求解。三维点通过三角化获得。内参如果未标定可以设定一个合理的初始值如焦距近似为图像宽度并考虑将其在优化中固定或设置较小的方差。收敛性判断关注Solver::Summary中的termination_type。CONVERGENCE是理想状态。查看initial_cost和final_cost的下降幅度。通常期望下降2-3个数量级。检查iterations次数如果达到最大迭代次数仍未收敛可能需要调整初始值或优化策略。调试与可视化输出每次迭代的代价设置options.minimizer_progress_to_stdout true。分析残差优化后遍历所有残差统计其均值和方差绘制直方图。健康的优化结果残差应近似服从均值为0的高斯分布在核函数影响范围内。可视化重投影误差在图像上画出特征点位置和重投影后的位置直观查看误差大小和分布。使用Ceres的检查工具options.check_gradients true可以检查雅可比矩阵计算是否正确与有限差分对比。options.gradient_check_relative_precision 1e-6。性能调优线程数设置options.num_threads为你的CPU核心数以并行计算雅可比矩阵和残差。内存使用对于超大规模问题SPARSE_SCHUR可能仍然内存不足。可以考虑使用ITERATIVE_SCHUR配合SCHUR_JACOBI预条件子它是无矩阵的内存占用少但可能需要更多迭代。参数块排序一个好的排序能极大减少稀疏Cholesky分解的填充元提升速度。除了默认的相机优先排序对于特定场景如循环闭合可以尝试更复杂的排序策略。9. 从基础BA到实际系统集成一个基础的BA模块实现后要集成到实际的SLAM或SfM系统中还需要考虑更多工程细节关键帧与地图点管理不是所有帧和点都参与全局BA。通常选择关键帧和其观测到的共视地图点。需要维护一个共视图来高效地管理这些关系。增量式BA当系统运行时频繁进行全局BA代价太高。通常采用局部BA优化当前帧及其共视关键帧和地图点和位姿图优化只优化关键帧位姿将地图点约束边化掉。全局BA作为后台线程偶尔运行。外点剔除在优化前和优化中需要主动剔除误匹配。可以在优化几轮后将重投影误差大于阈值的观测标记为外点并移除。尺度不确定性单目单目BA存在尺度模糊性。通常需要固定第一个关键帧的位姿和尺度如固定其平移为0旋转为单位阵并固定某个点的深度或某两个点之间的距离。使用先验信息如果某些参数已知很准确如标定好的相机内参可以在优化中将其固定 (problem.SetParameterBlockConstant) 或添加先验约束如使用ceres::CostFunction添加一个惩罚其偏离初始值的残差项。实现一个稳定高效的BA模块是视觉SLAM/SfM系统的核心。它要求我们对非线性优化理论、稀疏线性代数、计算机视觉几何都有深入的理解。从最小二乘的基本原理出发一步步推导到稀疏舒尔补再到用Ceres实现这个过程本身就是一个将理论扎实落地的绝佳范例。在实际操作中耐心调试参数、分析残差、理解每一次迭代的行为比单纯调通代码更重要。
