VINS 实战解析 - 从BA理论到Ceres实现的关键步骤拆解

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基础上，加了一个λ * I（I是单位矩阵）。这个λ就是个调节阀：

• 当λ很小（比如接近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::AutoDiffCostFunction<SnavelyReprojectionError, 2, 9, 3>( new SnavelyReprojectionError(observed_x, observed_y))); } double observed_x; double observed_y; };

关键点拆解：

• 模板参数T：operator()必须是模板函数，使用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]是焦距f，camera[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_iterations：LM算法是迭代算法，这里设置最大迭代步数。通常几十到几百步足够收敛。
• 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::Quaternion<T> q_eigen(q[0], q[1], q[2], q[3]); // Eigen::Map<Eigen::Matrix<T, 3, 1>> p_eigen(p); // Eigen::Map<const Eigen::Matrix<T, 3, 1>> point_eigen(point); // p_eigen = q_eigen * point_eigen; // ... 后续投影和残差计算 ... }

重要提示：如果你使用轴角，Ceres内置的ceres::AngleAxisRotatePoint函数已经帮你处理了李群李代数的转换。如果你用四元数，就需要自己管理旋转计算，并正确设置参数块的本地参数化，否则优化会在非流形空间上进行，导致错误。

4.2 尺度问题与参数初始化

BA是一个非凸优化问题，初始值的好坏直接决定了优化能否收敛到正确的全局最优（或一个好的局部最优）。如果你把相机位姿和三维点坐标都初始化为0或者随机数，优化几乎百分之百会失败。

初始化策略：

1. 相机位姿：对于视觉里程计，第一帧可以设为单位位姿（旋转为单位矩阵，平移为0）。后续帧可以通过对极几何（八点法、五点法）或PnP计算一个相对位姿作为初始值。
2. 三维点坐标：通过三角化（Triangulation）来初始化。利用至少两帧图像上匹配的特征点像素坐标和对应的相机位姿，计算出三维点的初始位置。
3. 尺度：单目SLAM存在尺度不确定性。BA无法恢复绝对尺度。通常的做法是固定第一帧的平移量为0，或者固定某个三维点之间的距离为基准尺度。在VINS中，通常会与IMU融合来估计和稳定尺度。

4.3 鲁棒核函数：应对误匹配的利器

前面提到了损失函数。在真实图像中，特征匹配不可能完美，总会有一些错误的匹配（外点）。这些外点会产生巨大的重投影误差。如果使用简单的平方损失（L2范数），这些巨大的误差项会在目标函数中占据主导地位，优化算法会为了减小这些“错误”的误差，而把正确的参数也拉偏，导致结果完全错误。

鲁棒核函数的作用就是压制这些大残差的影响。常用的有：

• Huber Loss：当残差小于某个阈值δ时，使用平方损失；大于δ时，使用线性损失。它像平方损失和绝对损失（L1）的结合，对中小外点有一定容忍度。
• Cauchy Loss：log(1 + (残差/尺度)^2)。它对大残差的压制效果比Huber更强。
• SoftLOne Loss：2 * 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，该怎么办？

1. 检查残差计算：这是最常见的错误来源。写一个简单的测试，固定相机和三维点参数，手动计算一个残差，和你仿函数输出的结果对比。确保投影模型（内参、畸变）、坐标系转换（世界系到相机系，是R*p + t还是R*(p - t)）完全正确。
2. 检查雅可比矩阵：虽然用了自动求导，但有时还是需要验证。Ceres提供了数值求导方式（NumericDiffCostFunction）。你可以用自动求导和数值求导分别构造代价函数，对同一组参数计算残差和雅可比，看是否一致。不一致说明你的残差函数有不可微的点或者实现有误。
3. 打印中间变量：在仿函数的operator()中，用std::cout打印关键中间变量（如旋转后的点p、归一化坐标xp, yp、畸变系数等），确保它们数值合理（没有出现NaN或inf）。
4. 简化问题：先优化一个最小规模的子问题。比如只用两帧图像和它们看到的几个点，关闭畸变优化，固定内参。让问题尽可能简单，先让优化能跑通。然后再一步步增加复杂度。
5. 调整求解器选项：尝试减小max_num_iterations看看前期是否下降。尝试不同的linear_solver_type。对于非常病态的问题（比如尺度差异巨大），可以尝试打开options.use_nonmonotonic_steps = true。
6. 检查参数块和内存：确保你传递给AddResidualBlock的每个参数块指针在整个优化生命周期内都是有效的，并且没有被意外修改。确保同一个物理变量（如一个特定的相机位姿）在所有残差块中用的是同一个指针地址。

从我自己的经验来看，BA的调试是一个需要耐心和细致的过程。理论清晰是基础，但将理论无误地翻译成代码，并处理好各种边界情况和数值稳定性问题，才是工程实现中最考验人的地方。当你第一次看到自己实现的BA成功地将一堆杂乱的点云和相机位姿优化到一个精确、一致的状态时，那种成就感是非常棒的。希望这篇从理论到Ceres实战的拆解，能帮你打通这关键的一环。