1. 从理论到代码BA到底在优化什么很多朋友在学VINS或者视觉SLAM的时候都会卡在BA光束法平差这一关。理论公式看了一堆什么重投影误差、最小二乘、LM算法感觉都懂了。但一打开Ceres的例程看到那一堆CostFunction、ResidualBlock、ParameterBlock瞬间又懵了。理论和代码就像两张皮怎么也贴不到一块去。我自己刚开始搞VINS的时候也在这个坑里扑腾了好久。今天我就想用最“人话”的方式把BA从数学公式到Ceres代码实现的这条链路一步步拆开给你看。我们不讲那些空中楼阁的理论就聚焦一件事那些复杂的理论概念到底是怎么变成一行行能跑的C代码的首先我们得彻底搞清楚BA到底在优化个啥。别被“光束法平差”这个高大上的名字吓住。咱们想象一个最简单的场景你拿着手机在房间里拍了好几张不同角度的照片照片里都有同一个桌子角特征点。BA要干的事儿就是通过这些照片里桌子角的位置反推出这个桌子角在真实房间里的三维坐标同时也推算出你每张照片拍摄时手机的具体位置和朝向也就是相机的位姿。这里的关键在于“调整”。一开始我们猜一个桌子角的三维位置再猜一下每张照片的拍摄位姿然后用这些猜测值把猜的桌子角位置“投影”到每张照片上看看投影出来的像素位置和照片上实际看到的像素位置差多少。这个差值就是重投影误差。BA的目标就是通过不停地、智能地调整我们对三维点位置和相机位姿的猜测让所有照片上、所有特征点的这个重投影误差的总和变得最小。这个过程本质上就是一个庞大的优化问题。我们把所有待优化的变量一堆三维点坐标一堆相机位姿参数堆成一个长长的向量叫参数向量。我们的目标函数就是所有重投影误差的平方和。优化算法比如LM的任务就是找到一组参数向量的值让这个目标函数的值降到最低。听起来是不是和机器学习里调参数有点像没错思想是相通的。但BA的特殊性在于它的稀疏性。一张照片不会看到所有的三维点一个三维点也不会出现在所有的照片里。这就导致在计算梯度、构造更新方程时会形成一个非常稀疏的矩阵。利用好这种稀疏结构是BA能够高效求解大规模问题的关键这也是为什么我们不用通用的优化库而要用Ceres或者g2o这类专门为SLAM优化设计的原因。2. 理论基石LM算法与稀疏性到底在说什么理解了BA在优化什么我们再来看看它具体是怎么“算”的。这里的主角是列文伯格-马夸尔特Levenberg-Marquardt, LM算法。你可以把它理解成梯度下降和高斯-牛顿法这两个“高手”的合体并且带了一个智能开关。高斯-牛顿法是咋想的呢它觉得我在当前猜测点附近用一阶泰勒展开也就是线性近似来模拟那个复杂的重投影误差函数有点太粗糙了。它想用二阶近似近似成一个二次函数这样找最小值就更准更快。这需要计算目标函数的海森矩阵Hessian也就是二阶导数矩阵。但海森矩阵计算量巨大而且BA问题里直接算海森矩阵也不方便。高斯-牛顿法取了个巧它用雅可比矩阵J的转置乘自己J^T * J来近似海森矩阵。这样一来每次迭代要解的方程就是(J^T * J) * δx -J^T * e其中e是当前的重投影误差向量δx就是我们要求的参数更新量。解出δx然后用x δx更新参数x。高斯-牛顿法在离最终答案比较近的时候收敛速度飞快。但缺点是如果初始猜测离得太远这个J^T * J近似可能不靠谱导致算法发散根本不收敛。这时候最速下降法就登场了。它的思想特别朴素站在山坡上想最快下到谷底那就沿着最陡的方向梯度的反方向走。它的更新公式是x - λ * gradient其中λ是步长。这个方法特别鲁棒只要步长设得合适哪怕起点再离谱也能保证让目标函数下降。但它的毛病是快到谷底时会走“之”字形折线收敛速度变得奇慢无比。LM算法的高明之处就在于它把这两个方法结合了。它求解的方程是(J^T * J λ * I) * δx -J^T * e。看它在高斯-牛顿法的J^T * J基础上加了一个λ * II是单位矩阵。这个λ就是个调节阀当λ很小比如接近0时方程就退化成了(J^T * J) * δx -J^T * e这就是高斯-牛顿法追求快速收敛。当λ很大时J^T * J相比之下可以忽略方程近似为λ * I * δx -J^T * e也就是δx (-1/λ) * J^T * e。而J^T * e正好就是目标函数的梯度所以这变成了最速下降法追求稳定下降。LM算法在运行时会根据本次迭代的效果动态调整λ。如果迭代后误差下降了说明这一步走得好下次就减小λ更像高斯-牛顿法加速冲刺。如果误差反而上升了说明步子迈大了就增大λ更像最速下降法稳住步伐小步试探。这个机制保证了LM既安全又高效。那稀疏性又是怎么发挥作用的呢我们来看那个关键方程(J^T * J) * δx -J^T * e里的J^T * J矩阵。在BA问题中雅可比矩阵J本身是稀疏的因为每个残差只依赖于少数几个相机和点导致J^T * J是一个具有特殊块状结构的稀疏矩阵。对这种矩阵进行直接的求逆或者分解如Cholesky分解LLT来计算δx计算量可以比稠密矩阵降低好几个数量级。Ceres库内部会自动识别问题的稀疏模式并采用如DENSE_SCHUR、SPARSE_SCHUR或ITERATIVE_SCHUR等专门的线性求解器来高效地解这个方程这是我们作为使用者无需手动实现但必须理解的底层逻辑因为它直接关系到我们配置求解器选项时的选择。3. Ceres实战如何将BA模型“翻译”成代码理论懂了现在进入最关键的环节用Ceres把它实现出来。Ceres的核心是定义残差Residual和构建问题Problem。我们得扮演一个“翻译官”把数学上的重投影误差模型“翻译”成Ceres能听懂的语言。3.1 定义残差计算模型CostFunction这是最核心的一步。我们需要告诉Ceres给定一个相机参数和一个三维点坐标如何计算出重投影误差。在Ceres里我们通过定义一个仿函数Functor即一个重载了operator()的类或结构体来实现。让我们结合原始文章中的SnavelyReprojectionError来逐行解析struct SnavelyReprojectionError { SnavelyReprojectionError(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 { // 1. 旋转将世界坐标系下的三维点(point)转换到相机坐标系下 T p[3]; ceres::AngleAxisRotatePoint(camera, point, p); // camera[0,1,2]是轴角表示的旋转 // 2. 平移加上相机的平移向量 camera[3,4,5] p[0] camera[3]; p[1] camera[4]; p[2] camera[5]; // 3. 投影到归一化平面假设相机坐标系下相机光心指向z轴正方向 T xp p[0] / p[2]; // 归一化x坐标 T yp p[1] / p[2]; // 归一化y坐标 // 4. 考虑径向畸变使用Brown-Conrady模型camera[7]k1, camera[8]k2 const T l1 camera[7]; const T l2 camera[8]; T r2 xp*xp yp*yp; T distortion T(1.0) r2 * (l1 l2 * r2); // 5. 变换到像素坐标系乘以焦距 camera[6] (f) const T focal camera[6]; T predicted_x focal * distortion * xp; T predicted_y focal * distortion * yp; // 6. 计算残差预测值 - 观测值 residuals[0] predicted_x - T(observed_x); residuals[1] predicted_y - T(observed_y); return true; } static ceres::CostFunction* Create(const double observed_x, const double observed_y) { return (new ceres::AutoDiffCostFunctionSnavelyReprojectionError, 2, 9, 3( new SnavelyReprojectionError(observed_x, observed_y))); } double observed_x; double observed_y; };关键点拆解模板参数Toperator()必须是模板函数使用typename T。这是因为Ceres在内部可能使用双精度double也可能使用特殊类型如Jet进行自动求导。我们的所有计算都必须用T类型以保证兼容性。参数顺序(const T* const camera, const T* const point, T* residuals)。前两个是输入参数块指针最后一个是输出残差指针。这个顺序和我们在Create函数中声明的维度是对应的。相机参数模型这里用了9个参数。camera[0,1,2]是旋转轴角即旋转向量camera[3,4,5]是平移camera[6]是焦距fcamera[7,8]是畸变参数k1, k2。这是BAL数据集的格式你的项目可能需要调整比如在VINS-Mono中可能使用四元数平移表示位姿内参可能是焦距和主点。自动求导注意我们并没有手动计算残差关于相机和三维点的雅可比矩阵ceres::AutoDiffCostFunction这个神器帮我们做了。我们只需要像写普通数学公式一样写出残差的计算过程它利用C模板元编程技术在编译期自动推导出导数。AutoDiffCostFunction的模板参数SnavelyReprojectionError, 2, 9, 3分别表示代价函数类型、残差维度这里是2维x和y方向误差、第一个参数块相机的维度9、第二个参数块三维点的维度3。3.2 构建优化问题与添加残差块定义好残差模型后我们需要组装整个优化问题。这就像用乐高积木搭建一个复杂的结构每一对“观测到的特征点像素坐标”就是一个乐高单元残差块它连接着“相机”和“三维点”这两块积木参数块。// 1. 创建优化问题 ceres::Problem problem; // 2. 假设我们有一个数据集遍历所有的观测每个观测是一个特征点在某个图像中的像素位置 for (int i 0; i num_observations; i) { // 获取第i个观测的像素坐标 (obs_x, obs_y) double obs_x observations[2*i]; double obs_y observations[2*i 1]; // 2.1 使用工厂函数创建代价函数残差块 ceres::CostFunction* cost_function SnavelyReprojectionError::Create(obs_x, obs_y); // 2.2 获取第i个观测对应的相机参数指针和三维点坐标指针 double* camera bal_problem.mutable_camera_for_observation(i); double* point bal_problem.mutable_point_for_observation(i); // 2.3 将残差块添加到问题中 problem.AddResidualBlock(cost_function, nullptr, // 损失函数Loss Function这里用nullptr表示平方损失 camera, // 参数块1相机参数 point); // 参数块2三维点坐标 }关键点拆解AddResidualBlock这是核心操作。它告诉Ceres“这里有一个残差计算模型cost_function它的值依赖于这两个参数块camera和point。请你在优化时考虑这个残差对这两个参数的约束。”损失函数Loss Function第二个参数我们传了nullptr这代表使用标准的二范数平方损失即残差^2。但在实际SLAM中特征点匹配常有误匹配外点为了增强鲁棒性我们通常会使用鲁棒核函数比如Huber损失、Cauchy损失。你可以这样用new ceres::HuberLoss(1.0)。核函数的作用是降低大残差可能是外点的权重防止它们把优化带偏。参数块的管理camera和point必须是double*指针指向存储这些参数的内存地址。Ceres会通过指针直接修改这些内存中的值来更新参数。你需要确保这些内存地址在优化过程中是有效的并且不同残差块可以共享同一个参数块比如多张图像看到同一个三维点它们都会指向同一个point地址。3.3 配置求解器与执行优化问题搭建好了最后就是设置“发动机”求解器并启动它。// 配置求解选项 ceres::Solver::Options options; options.linear_solver_type ceres::DENSE_SCHUR; // 线性求解器类型 // options.linear_solver_type ceres::SPARSE_SCHUR; // 对于更大规模问题使用稀疏求解器 options.max_num_iterations 50; // 最大迭代次数 options.minimizer_progress_to_stdout true; // 将优化过程输出到控制台方便调试 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.BriefReport() std::endl; // std::cout summary.FullReport() std::endl; // 更详细的报告关键选项解析linear_solver_type这是最重要的选项之一。DENSE_SCHUR适用于中小规模问题比如相机和点不多它利用BA问题的舒尔补Schur Complement特性来高效求解。SPARSE_SCHUR适用于大规模BA问题它同时利用了舒尔补和系数矩阵的稀疏性是VINS等系统最常用的选项。DENSE_NORMAL_CHOLESKY或SPARSE_NORMAL_CHOLESKY直接对正规方程J^T * J进行Cholesky分解。max_num_iterationsLM算法是迭代算法这里设置最大迭代步数。通常几十到几百步足够收敛。minimizer_progress_to_stdout强烈建议在调试时设为true。它会输出每次迭代的代价、梯度、步长等信息让你直观感受优化是否在正常工作代价是否在持续下降。各种tolerance收敛条件。可以根据精度要求调整。运行后summary会告诉你优化是否成功summary.IsSolutionUsable()、迭代了多少次、初始和最终的代价是多少、用了多长时间等。优化完成后最优的相机参数和三维点坐标就已经更新在你传入的camera和point指针所指向的内存里了。4. 工程细节与避坑指南把代码跑起来只是第一步在实际的VINS或者SLAM项目中你会遇到更多工程上的挑战。这里分享几个我踩过的坑和对应的解决方案。4.1 旋转的参数化为什么不用旋转矩阵细心的你可能发现了在BAL例程和很多入门材料中旋转用的是轴角Angle-Axis也就是一个3维向量其方向代表旋转轴模长代表旋转角度。而在VINS等系统中更常见的是使用四元数Quaternion。为什么不用更直观的3x3旋转矩阵呢根本原因在于优化算法需要在欧式空间中进行加法运算。LM算法求解的是x δx。旋转矩阵属于李群SO(3)它对加法不封闭两个旋转矩阵相加不再是旋转矩阵。而轴角和四元数在考虑其约束后对应的**李代数so(3)**是一个向量空间可以做加法。Ceres为几种常见的旋转表示提供了自动求导的本地参数化LocalParameterization。在Ceres中使用四元数和平移向量表示位姿的示例// 假设你的相机参数块前4维是四元数(qw, qx, qy, qz)后3维是平移(tx, ty, tz) double camera[7]; // 在添加参数块时需要告诉Ceres前4维是四元数有特殊的更新规则 problem.AddParameterBlock(camera, 7); // 创建四元数的本地参数化。Eigen的Quaterniond在内存中是(x, y, z, w)但Ceres的EigenQuaternionParameterization默认是(w, x, y, z)。 // 务必注意你使用的四元数内存布局 ceres::LocalParameterization* quaternion_local_parameterization new ceres::EigenQuaternionParameterization(); problem.SetParameterization(camera, quaternion_local_parameterization); // 在你的残差仿函数中需要将四元数转换为旋转矩阵来旋转点 template typename T bool operator()(const T* const camera, const T* const point, T* residuals) const { // camera[0,1,2,3] 是四元数 (w, x, y, z) T q[4] {camera[0], camera[1], camera[2], camera[3]}; // camera[4,5,6] 是平移 T t[3] {camera[4], camera[5], camera[6]}; T p[3]; // 旋转后的点 // 你需要一个用四元数旋转点的函数。Ceres没有内置可以自己实现或使用Eigen。 // 例如使用Eigen注意类型转换 // Eigen::QuaternionT q_eigen(q[0], q[1], q[2], q[3]); // Eigen::MapEigen::MatrixT, 3, 1 p_eigen(p); // Eigen::Mapconst Eigen::MatrixT, 3, 1 point_eigen(point); // p_eigen q_eigen * point_eigen; // ... 后续投影和残差计算 ... }重要提示如果你使用轴角Ceres内置的ceres::AngleAxisRotatePoint函数已经帮你处理了李群李代数的转换。如果你用四元数就需要自己管理旋转计算并正确设置参数块的本地参数化否则优化会在非流形空间上进行导致错误。4.2 尺度问题与参数初始化BA是一个非凸优化问题初始值的好坏直接决定了优化能否收敛到正确的全局最优或一个好的局部最优。如果你把相机位姿和三维点坐标都初始化为0或者随机数优化几乎百分之百会失败。初始化策略相机位姿对于视觉里程计第一帧可以设为单位位姿旋转为单位矩阵平移为0。后续帧可以通过对极几何八点法、五点法或PnP计算一个相对位姿作为初始值。三维点坐标通过三角化Triangulation来初始化。利用至少两帧图像上匹配的特征点像素坐标和对应的相机位姿计算出三维点的初始位置。尺度单目SLAM存在尺度不确定性。BA无法恢复绝对尺度。通常的做法是固定第一帧的平移量为0或者固定某个三维点之间的距离为基准尺度。在VINS中通常会与IMU融合来估计和稳定尺度。4.3 鲁棒核函数应对误匹配的利器前面提到了损失函数。在真实图像中特征匹配不可能完美总会有一些错误的匹配外点。这些外点会产生巨大的重投影误差。如果使用简单的平方损失L2范数这些巨大的误差项会在目标函数中占据主导地位优化算法会为了减小这些“错误”的误差而把正确的参数也拉偏导致结果完全错误。鲁棒核函数的作用就是压制这些大残差的影响。常用的有Huber Loss当残差小于某个阈值δ时使用平方损失大于δ时使用线性损失。它像平方损失和绝对损失L1的结合对中小外点有一定容忍度。Cauchy Losslog(1 (残差/尺度)^2)。它对大残差的压制效果比Huber更强。SoftLOne Loss2 * sqrt(1 残差) - 2。也是一种常用的鲁棒核。在Ceres中添加Huber损失ceres::LossFunction* loss_function new ceres::HuberLoss(1.0); // 阈值delta设为1.0个像素 problem.AddResidualBlock(cost_function, loss_function, // 传入损失函数对象 camera, point);选择合适的阈值如HuberLoss的delta很重要通常与你的观测噪声水平相关可以设为特征点定位误差的若干倍例如1.0-2.0像素。4.4 调试技巧当优化不收敛时怎么办看着控制台输出的迭代信息如果代价不降反升或者来回震荡最后报告NO_CONVERGENCE该怎么办检查残差计算这是最常见的错误来源。写一个简单的测试固定相机和三维点参数手动计算一个残差和你仿函数输出的结果对比。确保投影模型内参、畸变、坐标系转换世界系到相机系是R*p t还是R*(p - t)完全正确。检查雅可比矩阵虽然用了自动求导但有时还是需要验证。Ceres提供了数值求导方式NumericDiffCostFunction。你可以用自动求导和数值求导分别构造代价函数对同一组参数计算残差和雅可比看是否一致。不一致说明你的残差函数有不可微的点或者实现有误。打印中间变量在仿函数的operator()中用std::cout打印关键中间变量如旋转后的点p、归一化坐标xp, yp、畸变系数等确保它们数值合理没有出现NaN或inf。简化问题先优化一个最小规模的子问题。比如只用两帧图像和它们看到的几个点关闭畸变优化固定内参。让问题尽可能简单先让优化能跑通。然后再一步步增加复杂度。调整求解器选项尝试减小max_num_iterations看看前期是否下降。尝试不同的linear_solver_type。对于非常病态的问题比如尺度差异巨大可以尝试打开options.use_nonmonotonic_steps true。检查参数块和内存确保你传递给AddResidualBlock的每个参数块指针在整个优化生命周期内都是有效的并且没有被意外修改。确保同一个物理变量如一个特定的相机位姿在所有残差块中用的是同一个指针地址。从我自己的经验来看BA的调试是一个需要耐心和细致的过程。理论清晰是基础但将理论无误地翻译成代码并处理好各种边界情况和数值稳定性问题才是工程实现中最考验人的地方。当你第一次看到自己实现的BA成功地将一堆杂乱的点云和相机位姿优化到一个精确、一致的状态时那种成就感是非常棒的。希望这篇从理论到Ceres实战的拆解能帮你打通这关键的一环。