后端优化是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状态维度是639这就是一个典型的“2行9列Jacobian”的因子。2.3 Hessian矩阵为什么天然稀疏如果直接对所有变量求导得到的是一个巨大的稠密矩阵对100个位姿和5000个点来说变量数就是100x65000x315600稠密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 是旋转增量。对投影函数求导可以得到误差对相机增量的JacobianJ_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::Matrixdouble, 2, 6 J_cam; Eigen::Matrixdouble, 2, 3 J_point; Eigen::Matrixdouble, 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.block2,3(0, 0) -dpi * Eigen::Matrix3d::Identity(); J_cam.block2,3(0, 3) -dpi * skew_Xc; J_point -dpi * R;这段代码最容易错的地方是旋转增量的正负号以及叉乘矩阵的顺序。搞混一次迭代时会发现误差根本降不下去或者下降到一定程度开始反复震荡。建议写完后立刻做数值Jacobian交叉验证。3.3 数值Jacobian交叉验证这是我的保命符解析Jacobian写起来快但验证起来必须严谨。我习惯每次都跑一遍数值Jacobian用中心差分法去核对每一列。Eigen::Matrixdouble, 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维度 6NP路标点数量记为 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比原来的 6N3M 小得多。解出 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.block3,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.segment3(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.block3,3(3*j, 3*j).inverse(); delta_p.segment3(3*j) V_inv * (-g_p.segment3(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 从残差到更新的完整步骤实际求解器不会只解一次线性方程而是迭代多轮。标准流程是用当前状态计算所有预测像素坐标和残差。计算所有Jacobian。组装 H 和 g。加入LM阻尼项 lambda * I。执行Schur消元解出 delta_c 和 delta_p。用 delta 更新相机位姿和路标点。重新计算残差判断代价是否下降调整 lambda。满足收敛条件或达到最大迭代次数时退出。位姿更新用李代数左乘扰动不是直接加法。平移部分可以直接加旋转部分必须用指数映射更新Sophus::SE3d T_new T_old * Sophus::SE3d::exp(delta);如果这里写成了直接加旋转矩阵第一次迭代可能看起来正常但两三轮之后各种数值问题都会浮现出来。5.2 LM阻尼单目BA里的关键调节器LMLevenberg-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跟着全乱。我把常见问题整理成一张速查表方便你排查现象可能原因处理方式误差发散Jacobian符号错误数值Jacobian逐列对比误差震荡阻尼系数过小调大初始lambda误差卡住接近局部最优检查路标初始化质量出现NaN深度非正或矩阵奇异投影前校验z值加对角阻尼收敛慢未固定第一帧位姿加上锚定约束或调大阻尼6.4 性能优化从稠密到稀疏的转变我最初用全稠密矩阵做实验10帧、500个路标点时还能跑但到60帧、5000个路标点时内存已经吃紧。后来把W矩阵的组装改成逐路标点叠加Schur项内存消耗立刻下降了一个量级。再往后如果点规模上万就应该引入稀疏Cholesky分解甚至利用并行计算加速Jacobian组装。从工程角度看BA的耗时大头往往不是线性求解而是大量重投影和Jacobian计算的重复操作。先用OpenMP对特征点循环做并行化通常能拿到很可观的加速比。最后再补一句我的实操心得这次手写单目BA花时间最多的地方不是公式推导反而是取符号约定和Jacobian验证。只要这些前期工作做得扎实后面Schur消元和迭代更新基本就是一气呵成的事。如果你也正在写VIO后端我强烈建议你每写一个模块就留一个“数值验证开关”用有限差分去卡解析Jacobian。这是我做过性价比最高的调试投资。等这套纯视觉BA稳定跑通再用同样的代码框架去接IMU预积分因子你会发现复用的不只是代码还有对后端优化整个流程的掌控感。 
长河君 · 2026/10/14 4:12:55 · 阅读 1.2k 手写单目BA求解器：从数学推导到Schur补消元的完整实践
后端优化是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状态维度是639这就是一个典型的“2行9列Jacobian”的因子。2.3 Hessian矩阵为什么天然稀疏如果直接对所有变量求导得到的是一个巨大的稠密矩阵对100个位姿和5000个点来说变量数就是100x65000x315600稠密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 是旋转增量。对投影函数求导可以得到误差对相机增量的JacobianJ_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::Matrixdouble, 2, 6 J_cam; Eigen::Matrixdouble, 2, 3 J_point; Eigen::Matrixdouble, 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.block2,3(0, 0) -dpi * Eigen::Matrix3d::Identity(); J_cam.block2,3(0, 3) -dpi * skew_Xc; J_point -dpi * R;这段代码最容易错的地方是旋转增量的正负号以及叉乘矩阵的顺序。搞混一次迭代时会发现误差根本降不下去或者下降到一定程度开始反复震荡。建议写完后立刻做数值Jacobian交叉验证。3.3 数值Jacobian交叉验证这是我的保命符解析Jacobian写起来快但验证起来必须严谨。我习惯每次都跑一遍数值Jacobian用中心差分法去核对每一列。Eigen::Matrixdouble, 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维度 6NP路标点数量记为 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比原来的 6N3M 小得多。解出 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.block3,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.segment3(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.block3,3(3*j, 3*j).inverse(); delta_p.segment3(3*j) V_inv * (-g_p.segment3(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 从残差到更新的完整步骤实际求解器不会只解一次线性方程而是迭代多轮。标准流程是用当前状态计算所有预测像素坐标和残差。计算所有Jacobian。组装 H 和 g。加入LM阻尼项 lambda * I。执行Schur消元解出 delta_c 和 delta_p。用 delta 更新相机位姿和路标点。重新计算残差判断代价是否下降调整 lambda。满足收敛条件或达到最大迭代次数时退出。位姿更新用李代数左乘扰动不是直接加法。平移部分可以直接加旋转部分必须用指数映射更新Sophus::SE3d T_new T_old * Sophus::SE3d::exp(delta);如果这里写成了直接加旋转矩阵第一次迭代可能看起来正常但两三轮之后各种数值问题都会浮现出来。5.2 LM阻尼单目BA里的关键调节器LMLevenberg-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跟着全乱。我把常见问题整理成一张速查表方便你排查现象可能原因处理方式误差发散Jacobian符号错误数值Jacobian逐列对比误差震荡阻尼系数过小调大初始lambda误差卡住接近局部最优检查路标初始化质量出现NaN深度非正或矩阵奇异投影前校验z值加对角阻尼收敛慢未固定第一帧位姿加上锚定约束或调大阻尼6.4 性能优化从稠密到稀疏的转变我最初用全稠密矩阵做实验10帧、500个路标点时还能跑但到60帧、5000个路标点时内存已经吃紧。后来把W矩阵的组装改成逐路标点叠加Schur项内存消耗立刻下降了一个量级。再往后如果点规模上万就应该引入稀疏Cholesky分解甚至利用并行计算加速Jacobian组装。从工程角度看BA的耗时大头往往不是线性求解而是大量重投影和Jacobian计算的重复操作。先用OpenMP对特征点循环做并行化通常能拿到很可观的加速比。最后再补一句我的实操心得这次手写单目BA花时间最多的地方不是公式推导反而是取符号约定和Jacobian验证。只要这些前期工作做得扎实后面Schur消元和迭代更新基本就是一气呵成的事。如果你也正在写VIO后端我强烈建议你每写一个模块就留一个“数值验证开关”用有限差分去卡解析Jacobian。这是我做过性价比最高的调试投资。等这套纯视觉BA稳定跑通再用同样的代码框架去接IMU预积分因子你会发现复用的不只是代码还有对后端优化整个流程的掌控感。