企业官网建设流程全解析

后端优化是VIO系统里的重头戏，但这个话题特别容易劝退新人。原因很简单：市面上现成的优化库太多，很多人调一圈接口就能跑出结果，可一旦遇到工程问题——收敛慢、Hessian奇异、内存爆掉——就完全无从下手。我自己也经历过这种“能跑但不懂”的阶段，所以手写VIO系列到了第五节，我决定把后端优化单独拎出来，从头写一个单目BA求解器，把数学推导、Jacobian组装、Schur补消元这些环节全部摊开讲清楚。

这一节先说结论：本次实践不接IMU预积分，不接滑窗边缘化，只做纯单目BA。也就是说，我们手上有一组相机位姿和一组路标点，目标是通过重投影误差最小化，把这两类状态一起修正。这是后端优化的最小闭环，也是最值得手写一遍的部分。等到这套代码跑通了，后面再接IMU factor、边缘化、滑窗，思路基本是同一个套路，不会感到跳跃。

如果你正卡在“看懂公式但写不出代码”的阶段，或者想理解VIO后端在求解器层面到底发生了什么，这篇文章应该能给你把最后一块拼图补上。

1. 单目BA快速热身：我先回答三个“为什么”

1.1 BA不是黑盒，它就是最小二乘

BA的全称是Bundle Adjustment，翻译过来是“光束法平差”。名字听着高级，本质却不复杂：三维空间里的路标点，投影到相机成像平面上，会得到一组预测像素坐标；而相机实际观测到的像素坐标是已知的。预测和观测之间总有偏差，这个偏差叫重投影误差。BA要做的事，就是调整所有相机位姿和所有路标点位置，让所有误差的平方和尽可能小。

用一句话概括：这是一个大型非线性最小二乘问题，优化变量是“相机位姿＋路标点”，代价函数是所有重投影误差的总和。在VIO后端里，我们还会加进IMU预积分因子、先验因子，但单目BA是理解这一切的最小结构。

1.2 为什么非要手写，直接调优化库不好吗

很多开源系统里确实可以直接调用对方现成的后端，跑起来速度也不差。但至少有三个理由让我坚持手写一遍：

• 内部行为可控。库封装得越深，你在工程上越难针对具体问题做干预，比如阻尼参数调整、异常值剔除、增量更新策略。
• 方便验证。手写代码时，每一步都可以和理论公式对照，甚至用数值Jacobian做交叉验证。这是理解算法的最佳途径。
• 换框架成本低。你只要真正理解了一次BA，后面手里的任何优化库对你来说都只是按需调用，而不是依赖。

本次实践的代码量不大，核心就是构建Hessian矩阵、解线性方程组、更新增量状态这三件事。接下来，我会从数学形式逐步推进到代码实现。

2. 先定坐标约定，再谈状态变量

2.1 单目BA里的状态向量拆解

我们先定义要估计的东西：

• 相机位姿，用 T_cw 表示，含义是“世界坐标系到相机坐标系的变换”。这个变换包含旋转 R 和平移 t。
• 路标点，用 P_w 表示，含义是世界坐标系下的三维点坐标。

在代码里，我不会把旋转存成3x3矩阵直接优化，因为旋转矩阵有9个参数但只有3个自由度，直接优化会引入额外约束。正确做法是用李代数扰动模型：旋转用so(3)的3参数向量表示，每次迭代求解一个6维的相机增量（3维平移增量 + 3维旋转增量），路标点则用3维增量。

坐标约定这块，我踩过很深的坑。只差一个符号，整个Jacobian全错，而且这种错误在误差曲线上几乎看不出来，只有重新跑一次仿真数据才能定位。所以建议你在动手前就把一个约定写死在注释里：本代码默认 T_cw，公式推导和代码必须一致，绝对不能混用。

2.2 观测模型与重投影误差

假设某个路标点 P_j 被第 i 帧相机观测到，观测到的像素坐标是 z_ij = (u, v)。预测值这样算：

X_c = R * P_w + t z_pred = (f_x * X_c.x / X_c.z + c_x, f_y * X_c.y / X_c.z + c_y)

于是误差项为：

e_ij = z_ij - z_pred

整个BA的代价函数就是所有误差项的二范数平方和。误差维度是2，状态维度是6+3=9，这就是一个典型的“2行9列Jacobian”的因子。

2.3 Hessian矩阵为什么天然稀疏

如果直接对所有变量求导，得到的是一个巨大的稠密矩阵，对100个位姿和5000个点来说，变量数就是100x6+5000x3=15600，稠密Hessian矩阵会有约2.4亿个元素，内存直接爆炸。

但仔细看，每个误差只关联“一个位姿 + 一个路标点”，完全不涉及其他位姿和点。这导致整个Hessian矩阵呈现明显的分块稀疏结构：位姿-位姿块、路标-路标块、以及相互关联的交叉块。利用这个结构，我们可以把矩阵组装成几块子矩阵单独处理，这也是后面Schur补消元能大幅度提速的根本原因。

3. 核心代码环节：残差、Jacobian与Hessian组装

3.1 残差计算的细节：用归一化坐标还是像素坐标

理论推导里常用归一化平面坐标，因为公式简洁；但工程实践中我建议直接使用像素坐标计算误差。理由很直接：像素坐标是传感器直接输出的观测值，协方差更接近“每像素一个像素”的噪声模型，处理起来不需要额外转换，代码也直观。

当然，如果你使用鱼眼相机或带畸变的相机，还必须先把畸变模型加进投影链路。本次代码先用针孔模型，畸变暂不考虑，但投影函数接口要预留畸变参数的位置，后面扩展时不用大改。

下面是投影函数的代码片段：

Eigen::Vector2d project(const Eigen::Matrix3d& K, const Eigen::Matrix3d& R, const Eigen::Vector3d& t, const Eigen::Vector3d& P_w) { Eigen::Vector3d X_c = R * P_w + t; double x = X_c.x(); double y = X_c.y(); double z = X_c.z(); double u = K(0, 0) * x / z + K(0, 2); double v = K(1, 1) * y / z + K(1, 2); return Eigen::Vector2d(u, v); }

注意判断 z 的正负和是否为0。路标点被相机看到时，它应该在相机前方，z应该大于0；如果出现 z 小于等于0的观测，多半是数据外点或初始化错误，应该在构建Jacobian之前就剔除。

3.2 解析Jacobian推导与实现

我们采用左乘扰动模型，把相机位姿的增量定义为：

delta_xi = (delta_rho, delta_phi)

其中 delta_rho 是平移增量，delta_phi 是旋转增量。对投影函数求导，可以得到误差对相机增量的Jacobian：

J_cam = - (d_pi / d_Xc) * [ I_3, -skew(X_c) ]

其中 d_pi / d_Xc 是投影函数对相机坐标的导数：

d_pi / d_Xc = [ fx/Z, 0, -fx*X/Z^2; 0, fy/Z, -fy*Y/Z^2 ]

再看路标点部分。假设世界坐标点有一个小的扰动 delta_P，相机坐标的扰动就是 R * delta_P，于是误差对路标点的Jacobian为：

J_point = - (d_pi / d_Xc) * R

这段代码写出来是这个效果：

Eigen::Matrix<double, 2, 6> J_cam; Eigen::Matrix<double, 2, 3> J_point; Eigen::Matrix<double, 2, 3> dpi; dpi << K(0,0)/X_c.z(), 0, -K(0,0)*X_c.x()/(X_c.z()*X_c.z()), 0, K(1,1)/X_c.z(), -K(1,1)*X_c.y()/(X_c.z()*X_c.z()); Eigen::Matrix3d skew_Xc; skew_Xc << 0, -X_c.z(), X_c.y(), X_c.z(), 0, -X_c.x(), -X_c.y(), X_c.x(), 0; J_cam.block<2,3>(0, 0) = -dpi * Eigen::Matrix3d::Identity(); J_cam.block<2,3>(0, 3) = -dpi * skew_Xc; J_point = -dpi * R;

这段代码最容易错的地方是旋转增量的正负号以及叉乘矩阵的顺序。搞混一次，迭代时会发现误差根本降不下去，或者下降到一定程度开始反复震荡。建议写完后立刻做数值Jacobian交叉验证。

3.3 数值Jacobian交叉验证，这是我的保命符

解析Jacobian写起来快，但验证起来必须严谨。我习惯每次都跑一遍数值Jacobian，用中心差分法去核对每一列。

Eigen::Matrix<double, 2, 6> J_cam_num; double eps = 1e-6; Eigen::Matrix3d R_plus = R * Sophus::SO3d::exp(Eigen::Vector3d::UnitX() * eps).matrix(); Eigen::Vector3d t_plus = t; Eigen::Vector2d z_plus = project(K, R_plus, t_plus, P_w); Eigen::Matrix3d R_minus = R * Sophus::SO3d::exp(-Eigen::Vector3d::UnitX() * eps).matrix(); Eigen::Vector2d z_minus = project(K, R_minus, t_minus, P_w); J_cam_num.col(3) = (z_plus - z_minus) / (2.0 * eps);

这里的 col(3) 对应旋转增量第一个轴。平移增量的数值验证同理。实测下来，解析值和数值值的差异在1e-6量级就属于正常；如果差异跑到1e-3量级，基本可以断定符号或顺序有问题。

4. 求解大杀器：Schur补消元与分块求解

4.1 正规方程与分块结构

把误差项的Jacobian全部组装起来，得到正规方程：

H * delta = -g

其中 H 是信息矩阵，g 是梯度向量。把所有变量分成两类：

• C：相机位姿，数量记为 N，维度 6N
• P：路标点，数量记为 M，维度 3M

于是 H 可以写成如下分块形式：

H = [ U W; W^T V ]

这里 U 是相机-相机块，V 是路标-路标块，W 是两者交叉块。我们的目标是从这个巨型线性系统里解出增量。

4.2 利用V矩阵的分块对角性质做消元

路标点之间不存在直接关联。每个路标点只出现在它自己的3x3对角块里，所以 V 是分块对角矩阵，而不是稠密矩阵。这给了我们一个极好的条件：可以对 V 逐块求逆，复杂度极低。

先对正规方程做行变换，消去路标增量。具体方法是用 V 的逆去约化：

(U - W * V^{-1} * W^T) * delta_c = -g_c + W * V^{-1} * g_p

左侧括号里的矩阵称为Schur补，记作 S。由于 S 只和相机位姿相关，维度变成 6N，比原来的 6N+3M 小得多。解出 delta_c 之后，再回代：

delta_p = V^{-1} * (-g_p - W^T * delta_c)

整个流程里，V^{-1} 的计算是一个一个3x3矩阵求逆，而不是对整个大矩阵求逆，速度差距非常明显。

4.3 代码上的分块实现

我这里给出一个简化但完整的Schur消元片段：

int n_cam = frames.size(); int n_pts = points.size(); Eigen::MatrixXd U(6*n_cam, 6*n_cam); Eigen::MatrixXd W(6*n_cam, 3*n_pts); Eigen::MatrixXd V(3*n_pts, 3*n_pts); Eigen::VectorXd g_c(6*n_cam); Eigen::VectorXd g_p(3*n_pts); // 组装过程省略，通过添加每个观测对应的 J_cam^T * J_cam 等完成 // 先对每个路标点单独求逆 V 块 Eigen::MatrixXd S = U; Eigen::VectorXd rhs = -g_c; for (int j = 0; j < n_pts; j++) { Eigen::Matrix3d V_inv = V.block<3,3>(3*j, 3*j).inverse(); S -= W.block(6*n_cam, 3*j, 6*n_cam, 3) * V_inv * W.block(6*n_cam, 3*j, 6*n_cam, 3).transpose(); rhs += W.block(6*n_cam, 3*j, 6*n_cam, 3) * V_inv * g_p.segment<3>(3*j); } // 解相机增量 Eigen::VectorXd delta_c = S.ldlt().solve(rhs); // 回代求路标增量 Eigen::VectorXd delta_p(3*n_pts); for (int j = 0; j < n_pts; j++) { Eigen::Matrix3d V_inv = V.block<3,3>(3*j, 3*j).inverse(); delta_p.segment<3>(3*j) = V_inv * (-g_p.segment<3>(3*j) - W.block(6*n_cam, 3*j, 6*n_cam, 3).transpose() * delta_c); }

这里有一点必须提醒：如果路标点数量特别大，W 矩阵用稠密Eigen矩阵存储，内存很快就会爆炸。教学习惯用小规模数据还好，到实际工程里应当把 W 换成稀疏存储，或干脆逐路标点叠加 Schur 项，不要显式保存完整 W。我在代码里保留这个显式版本，是为了方便对照公式，性能上并不最优。

4.4 Schur补与边缘化的关系

很多同学问：Schur补消元是不是就是边缘化？从线性代数角度看，两者用的是同一个工具：对联合高斯分布做条件化，消除一部分变量后，剩余变量的信息矩阵会更新为 Schur 补。在VIO滑窗里，我们会刻意“边缘化”掉最老的帧或最老的路标点，把它们的约束信息压缩成一个先验因子，再继续维护滑动窗口。单目BA里的 Schur 消元虽然在迭代内部使用，不改变长期状态，但理解了这个矩阵操作，滑窗的边缘化代码基本就是同一个配方。

5. 完整迭代求解流程与参数调试

5.1 从残差到更新的完整步骤

实际求解器不会只解一次线性方程，而是迭代多轮。标准流程是：

1. 用当前状态计算所有预测像素坐标和残差。
2. 计算所有Jacobian。
3. 组装 H 和 g。
4. 加入LM阻尼项 lambda * I。
5. 执行Schur消元，解出 delta_c 和 delta_p。
6. 用 delta 更新相机位姿和路标点。
7. 重新计算残差，判断代价是否下降，调整 lambda。
8. 满足收敛条件或达到最大迭代次数时退出。

位姿更新用李代数左乘扰动，不是直接加法。平移部分可以直接加，旋转部分必须用指数映射更新：

Sophus::SE3d T_new = T_old * Sophus::SE3d::exp(delta);

如果这里写成了直接加旋转矩阵，第一次迭代可能看起来正常，但两三轮之后各种数值问题都会浮现出来。

5.2 LM阻尼：单目BA里的关键调节器

LM（Levenberg-Marquardt）算法的核心思想是：当迭代离最优解远时，用接近梯度下降的策略大步探索；当靠近最优解时，退化为高斯牛顿法快速收敛。这个切换通过阻尼系数 lambda 完成。

我常用的策略：

• 初始 lambda 取 1e-3 到 1e-2。
• 每轮迭代后，如果代价下降，则 lambda 缩小3倍；如果代价上升，则 lambda 扩大10倍，并且不采纳这次增量，重新计算。
• 当相对代价变化低于 1e-7 时，判定收敛。

注意 lambda 的缩放倍数会影响收敛速度。倍数太大，迭代步数增加；倍数太小，数值不稳定。3和10是我实测下来比较省心的组合，但不代表所有场景最优。

5.3 单目BA特有的不可观问题

单目视觉无法恢复绝对尺度，也无法唯一确定世界坐标系的位置。理论上，整个系统存在7个不可观方向：3个整体平移、3个整体旋转、1个整体尺度。这意味着正规方程里的Hessian矩阵在最优点附近是奇异的，直接求逆会出问题。

LM的阻尼项天然缓解了这个问题。因为 H + lambda * I 把不可观方向的零特征值提高到了 lambda，矩阵变得可逆。但这也带来一个副作用：不可观方向上的迭代收敛很慢，误差可能一直徘徊在一个很小的数值上不动。工程上建议固定第一帧相机位姿，或者额外加一个软约束，把尺度锚定在一个合理值上。手写练习里，我一般在初始化时把第一帧位姿设为固定，只优化后续帧和所有路标点。

6. 常见翻车现场与排查思路

6.1 第一次迭代误差反而暴涨

这是我见过最多的情况。原因主要有两个：一个是符号问题，一个是阻尼策略问题。先检查Jacobian符号是否和误差定义的一致，最简单的方法是把数值Jacobian打印出来和解析Jacobian逐列对比。如果一致，再检查步长是否过大，尝试把初始 lambda 调大一个数量级。

6.2 迭代十几轮后误差降到0.001左右就卡住

这种情况通常不是代码bug，而是接近收敛。单目BA的像素误差本身存在噪声下限，不可能完全到0。比较合理的判断标准是相对变化量而不是绝对误差。如果两次迭代的代价相对变化小于1e-6，就可以认为收敛了。此外，如果路标点初始化质量很差，BA会卡在局部最优，需要在三角化阶段做好外点剔除。

6.3 Hessian里出现NaN或Inf

多半是某个路标点的深度为0，或者求逆时出现了病态矩阵。建议在投影函数入口处加断言，保证 z > 1e-6。同时检查相机内参是否从单位矩阵输入的，内参错误会导致投影结果异常，Jacobian跟着全乱。

我把常见问题整理成一张速查表，方便你排查：

6.4 性能优化：从稠密到稀疏的转变

我最初用全稠密矩阵做实验，10帧、500个路标点时还能跑，但到60帧、5000个路标点时内存已经吃紧。后来把W矩阵的组装改成逐路标点叠加Schur项，内存消耗立刻下降了一个量级。再往后，如果点规模上万，就应该引入稀疏Cholesky分解，甚至利用并行计算加速Jacobian组装。

从工程角度看，BA的耗时大头往往不是线性求解，而是大量重投影和Jacobian计算的重复操作。先用OpenMP对特征点循环做并行化，通常能拿到很可观的加速比。

最后再补一句我的实操心得

这次手写单目BA，花时间最多的地方不是公式推导，反而是取符号约定和Jacobian验证。只要这些前期工作做得扎实，后面Schur消元和迭代更新基本就是一气呵成的事。如果你也正在写VIO后端，我强烈建议你每写一个模块，就留一个“数值验证开关”，用有限差分去卡解析Jacobian。这是我做过性价比最高的调试投资。等这套纯视觉BA稳定跑通，再用同样的代码框架去接IMU预积分因子，你会发现复用的不只是代码，还有对后端优化整个流程的掌控感。