在写完 VIO 前端、搞定 IMU 预积分和对齐之后我一直觉得自己的系统里少了一块“定海神针”——后端优化。前端给出来的位姿和路标点就像刚出锅的菜看着行但噪声大、漂移明显必须放到 BABundle Adjustment光束法平差里重新磨一遍。这一节我选择直接手写单目 BA 求解代码而不是继续挂着通用优化库跑原因很简单滑窗要边缘化、Hessian 要舒尔补消元、LM 要调阻尼这些环节如果一直当黑盒出了问题根本无从下手。这篇文章就把我手写单目 BA 的过程拆开讲适合已经跑通 VIO 前端、正准备深入了解后端的同学也适合那些想让优化求解器“手感”真正属于自己的人。1. 单目BA在VIO后端的位置从滑动窗口到重投影约束1.1 后端到底在优化什么后端优化这件事说穿了就是把“相机在哪些位姿、看到了哪些路标点”这个问题变成一组最小化误差的联合估计。对 VIO 系统来说后端通常跑在一个滑动窗口里窗口中塞了最近 N 帧的关键帧位姿以及这些帧共同观测到的路标点。每一次优化就是在调整这些位姿和路标点坐标让“路标点投影到图像上的位置”和“实际特征点位置”之间的差异也就是重投影误差整体降到最小。手写单目 BA 之前我先在纸上把变量明确了一遍。状态向量由两部分组成一部分是窗口内所有相机位姿 ( T_{wc} \in SE(3) )另一部分是每个路标点在参考系下的三维坐标 ( P_w \in \mathbb{R}^3 )。这里的下标 w 是参考坐标系c 是相机坐标系。单目的特殊之处在于路标点的深度是尺度不确定的前端如果没做逆深度参数化或三角化初值后端收敛会很吃力。优化目标函数写出来就是[ E \sum_{(i,j) \in \mathcal{O}} \rho\left( | z_{ij} - \pi(T_{wc_i}, P_{w_j}) |{\Sigma{ij}}^2 \right) ](\mathcal{O}) 是所有可观测对( z_{ij} ) 是第 i 帧观测到的第 j 个路标点的像素坐标( \pi(\cdot) ) 是投影函数( \rho ) 是核函数用来压制外点的影响。这个公式看起来简单但把它展开成代码每一个符号背后都有一摊推导。1.2 为什么单独看是纯视觉问题VIO 的完整后端通常还要加 IMU 因子、边缘化先验但我在这一节特意只做单目 BA 部分。原因是我希望在理解核心优化机制时不引入额外干扰。IMU 因子本质上是把“预积分残差”加到同一个代价函数里位姿变量的维度和块结构都不变只是残差方程变多了。先把视觉 BA 的求解骨架搭对后面加 IMU 因子就是照着同一个框架再填一个残差模型的事。所以我手写的这段时间里输入只有三类东西关键帧位姿初值、路标点坐标初值、特征点观测值。输出是优化后的位姿和路标点。这个接口如果定义清晰后续无论是接 IMU 预积分因子还是做滑窗边缘化都不用推翻重写。1.3 手写BA前必须明确的输入输出接口这里我强烈建议你先定好代码接口再动笔。我的做法是这样写一个 BaSolver 的类构造函数接收一组帧信息、一组路标点信息、一组观测索引。内部维护一个很朴素的残差块数组每个残差块记录着它关联的位姿 ID、路标点 ID、像素观测值和信息矩阵。优化完成之后把更新后的位姿和路标写回原数组。不要小看这一步。接口乱糟糟后面调试的时候你会不知道是优化算法的问题还是数据组织的问题。我第二次重写时就是以这三个数组为核心重新组织代码调试效率高了很多。2. 重投影残差的雅可比推导代码里每一行都在算什么2.1 单目相机投影链路单目 BA 的核心是投影函数 ( \pi(\cdot) )。假设相机内参是 ( (f_x, f_y, c_x, c_y) )一个世界点 ( P_w ) 先经过位姿变换到相机系 ( P_c R_{cw} P_w t_{cw} )再投影到像素平面[ u f_x \frac{X_c}{Z_c} c_x, \quad v f_y \frac{Y_c}{Z_c} c_y ]残差就是[ r z - \begin{bmatrix} u \ v \end{bmatrix} ]这一行代码写出来人人都会但真正决定 BA 能不能收敛的是残差对状态变量的导数也就是雅可比。雅可比不对优化在最佳点附近都会震荡甚至发散而且这种发散很隐蔽——代价函数可能还在下降但位姿和路标都没在正常数值范围。2.2 对位姿的雅可比左扰动模型求位姿导数时我习惯用 SE(3) 左扰动模型。设位姿 ( T (R, t) )给它左乘一个小扰动 ( \exp(\xi^\wedge) )得到 ( T_{\text{new}} \exp(\xi^\wedge) T )。于是世界点在扰动后的相机系坐标是[ P_c R_{cw} P_w t_{cw} \delta\varphi \times (R_{cw} P_w t_{cw}) \delta v ]写得更紧凑些用 ( \delta\xi (\delta\varphi, \delta v) ) 表示旋转扰动和平移扰动再对像素坐标分别求导最终会得到 2x6 的雅可比[ J_{\xi} -\begin{bmatrix} \frac{f_x}{Z_c} 0 -\frac{f_x X_c}{Z_c^2} \ 0 \frac{f_y}{Z_c} -\frac{f_y Y_c}{Z_c^2} \end{bmatrix} \begin{bmatrix} [P_c]_\times I_3 \end{bmatrix} ]前 3 列是旋转扰动的贡献后 3 列是平移扰动的贡献。推导中关键一点是旋转扰动对三维点的影响是叉乘 ( [P_c]_\times )而不是简单地乘以 ( R )这是新手最容易写错的地方。2.3 对路标点的雅可比路标点 ( P_w ) 对残差的导数要经过两个环节先到相机系再到像素平面。相机系的坐标对路标点的导数就是旋转矩阵 ( R_{cw} )而像素坐标对相机系坐标的导数跟前面那个 2x3 块的左半部分一样。合并之后[ J_P -\begin{bmatrix} \frac{f_x}{Z_c} 0 -\frac{f_x X_c}{Z_c^2} \ 0 \frac{f_y}{Z_c} -\frac{f_y Y_c}{Z_c^2} \end{bmatrix} R_{cw} ]我建议在代码里把这两块分开存因为构造 Hessian 矩阵时位姿块和路标块的性质完全不同后面舒尔补消元也全靠这个分离结构。2.4 数值验证雅可比错了代码也不会报错雅可比写了三遍我都总觉得有错后来学乖了直接用数值差分验证。比如对路标点给 ( P_w ) 的 x 加一个 ( 10^{-6} ) 的微小量重新算一遍残差和解析雅可比对比。两者应该非常接近。分量解析值数值差分相对误差( \partial r_u / \partial X_w )-0.07134-0.071330.014%( \partial r_v / \partial X_w )0.005210.005220.019%误差在 ( 10^{-4} ) 量级以内就说明解析雅可比基本可靠。这个验证脚本我保留在工程里以后每次改动投影模型或涉及相机畸变时都会跑一遍算是给自己上的一道保险。3. 数据结构与整体求解框架把优化问题组织成代码3.1 优化变量组织位姿和路标分开管我把优化变量分成两组一组是位姿用李代数参数化一组是路标用普通三维坐标。位姿我不用 4x4 矩阵直接当变量因为 16 个分量里有约束数值迭代时很容易破坏约束还得不断投影回 SE(3)。更好的办法是存储为 6 维的李代数向量 ( \xi )更新时用指数映射 ( T_{\text{new}} \exp(\delta\xi^\wedge) T_{\text{old}} ) 的方式。这样迭代过程中始终是合法位姿。路标点我一开始直接存 ( (x, y, z) )后来做单目时经常被深度退化或初值深度为负搞到头疼所以建议至少做一个简单的逆深度参数化选项。先不展开讲但这部分在代码里留接口后面真实 VIO 数据上会省很多事。3.2 残差块设计与信息矩阵每个残差块我这样定义struct ResidualBlock { int pose_id; // 关联的位姿编号 int point_id; // 关联的路标点编号 Eigen::Vector2d obs; // 像素观测值 Eigen::Matrix2d info; // 信息矩阵通常是基于特征点协方差的逆 };信息矩阵的默认值我一开始按单位阵处理结果优化出来的轨迹在弱纹理区域抖动明显。后来把信息矩阵设为与观测的像素坐标不确定性挂钩通常给高精度特征点更大的信心权重效果立刻不一样。需要注意的是信息矩阵和协方差的换算关系是 ( \Sigma^{-1} )写代码时别把两者搞反。3.3 求解器主流程Gauss-Newton 到 LM我手写的求解器用一个简单的 LMLevenberg-Marquardt迭代框架。流程大致如下遍历所有残差块分别计算残差 ( r )、位姿雅可比 ( J_\xi )、路标雅可比 ( J_P )。累加构造 ( H J^T \Sigma^{-1} J ) 和右侧 ( b -J^T \Sigma^{-1} r )。对 ( H ) 加阻尼项 ( \lambda I )或者更精细地对角缩放。舒尔补消元把路标变量消掉先解位姿增量。回代求路标增量。更新状态计算代价函数变化决定接受还是拒绝这一步。这个流程本身不复杂真正耗神的是第 4 步的舒尔补和第 3 步的阻尼策略。下面两节分别细说。4. Hessian矩阵的稀疏性与舒尔补消元4.1 Hessian矩阵的块稀疏结构是性能命脉如果直接堆 ( H ) 矩阵并把所有变量拉成一维向量位姿 6 维、路标 3 维一个 10 帧窗口里有几十个路标矩阵也就几百乘几百看起来不大。但滑动窗口中关键帧数量越多、共同观测越密集矩阵规模会快速膨胀。而且 BA 的 ( H ) 矩阵天然有大规模稀疏结构只有同一帧位姿和它观测到的路标点之间存在非零块其他位置都是零。我把变量排序设为“先排位姿、再排路标”于是 ( H ) 矩阵自然分块为[ H \begin{bmatrix} H_{pp} H_{pl} \ H_{lp} H_{ll} \end{bmatrix} ]其中 ( H_{pp} ) 是位姿相关的块维度小但稠密( H_{ll} ) 是路标相关的块维度大但呈现出对角块结构因为每个路标通常只被少数几帧观测到不同路标之间没有直接约束。4.2 利用舒尔补消元解出位姿增量直接对整个 ( H ) 做 Cholesky 分解非常浪费因为解一个稠密矩阵的复杂度是 ( O(n^3) )一旦路标点数量上到几千计算量就失控了。标准做法是舒尔补消元Schur complement把路标增量先用位姿增量表示[ \begin{bmatrix} H_{pp} - H_{pl} H_{ll}^{-1} H_{lp} 0 \ H_{lp} H_{ll} \end{bmatrix} \begin{bmatrix} \delta x_p \ \delta x_l \end{bmatrix}\begin{bmatrix} b_p - H_{pl} H_{ll}^{-1} b_l \ b_l \end{bmatrix} ]先解完位姿增量 ( \delta x_p )再回代 ( H_{ll} \delta x_l b_l - H_{lp} \delta x_p ) 求路标增量。关键是 ( H_{ll} ) 因为是块对角矩阵求逆非常便宜每块只有 3x3。这个消元过程放在代码里我最开始的写法是把整个矩阵准备好再消浪费了很多内存和计算量后来改成边构造边维护 ( H_{ll} ) 的逆速度提升非常明显。下面是一段基于自旋和块矩阵的思路实现的核心伪代码// S: 舒尔补后的位姿块矩阵 MatrixXd S Hpp; VectorXd sg bp; for (size_t pid 0; pid num_poses; pid) { for (size_t rid 0; rid num_points; rid) { if (!hasObservability(pid, rid)) continue; Matrix3d Hll_inv Hll_block(rid).inverse(); // 3x3 小逆 MatrixXd Hpl_block getBlock(Hpl, pid, rid); // 6x3 S.block(pid*6, pid*6, 6, 6) - Hpl_block * Hll_inv * Hpl_block.transpose(); sg.segment(pid*6, 6) - Hpl_block * Hll_inv * bl.segment(rid*3, 3); } } // 解位姿增量 VectorXd dxp S.ldlt().solve(sg);这里有几个容易出错的重灾区。第一Hpl的块索引对位姿是整行整列连续存储的如果你下标算错一位矩阵数值看着没问题但雅可比加到了错误的位置优化会非常慢或发散。第二Hll求逆之前要确认它确实是块对角如果某个路标点被同一帧重复观测就会出现同一路标有两个残差块此时这 3x3 块就不再是单一块而是该路标所有残差贡献的累加。第三舒尔补后的 S 矩阵可能条件数很差求解时不要用S.inverse()而是用LDLT分解数值稳定性和速度都好得多。4.3 边缘化的边界BA里的舒尔补和滑窗边缘化不是一回事做 VIO 后端时我一开始把舒尔补和边缘化混在一起想走了一段弯路。这里要理清纯单目 BA 里的舒尔补只是一种求解技巧消掉的路标变量依然会通过回代更新而滑动窗口边缘化是真正把旧位姿和某些路标点从优化变量中移除同时把移除带来的信息以先验因子的形式保留下来。代码上两者确实共用一套舒尔补数学工具先构造旧变量的信息矩阵然后消元得到剩余变量的先验项。但语义完全不同BA 过程中每次都重新计算路标变量而边缘化是一次性将丢弃变量的信息压缩进剩余变量。如果按 BA 舒尔补的逻辑去写滑窗边缘化结果就是边缘化掉的帧信息丢失系统一致性变差。5. LM求解器中的数值细节与避坑实录5.1 阻尼项到底怎么加才稳LM 算法里阻尼项 ( \lambda ) 的作用是在 Gauss-Newton 和梯度下降之间插值。小 ( \lambda ) 逼近 GN 的快速收敛大 ( \lambda ) 逼近梯度下降的稳定下降。最简单粗暴的更新策略是如果本次迭代让代价函数下降就减小 ( \lambda )否则增大 ( \lambda ) 并重试。但我在实际代码里还做了一项改进就是对阻尼项做对角缩放用 ( \lambda \cdot \operatorname{diag}(H) ) 而不是 ( \lambda I )。原因是位姿和路标变量的尺度差异很大——位姿的平移分量经常是米级而路标点在单目里可能是归一化系下的相机坐标系坐标两者数值范围差出几个量级。统一加同一阻尼会导致小尺度的路标变量被过度约束。具体写法上我在构造 S 矩阵后先取 S 的对角元素最大值作为缩放基准再乘上当前 ( \lambda )。这样不同变量拥有各自的阻尼强度实测收敛更稳定迭代步数也少了一些。5.2 线性求解器的选择从 LLT 到 LDLT解位姿增量方程时我用过三种方式直接求逆、LLT 分解、LDLT 分解。直接求逆最省代码但最不稳定LLT 要求矩阵正定实测 S 矩阵因为有路标点退化或相机几何配置差可能出现接近半正定甚至非正定的情况此时 LLT 会直接失败或给出爆炸解。LDLT 允许对角线有小量负值数值宽容度高而且对内存占用没有本质区别。Eigen 里的S.ldlt().solve(sg)已经封装得很好。对我这种手写实现的人来说关键不是挑选最优雅的分解算法而是先确认 S 矩阵对称性和数值尺度都没有问题。手写 BA 时我会在每轮迭代里打印 S 矩阵最小的几个特征值信息如果接近零多半是路标点共面、相机近似纯旋转或者初值太差导致投影点在焦点附近。5.3 我踩过的坑Nan、发散的隐藏来源踩坑是我收获最多的地方列几个最常见的希望对你有帮助。第一某个残差块里 ( Z_c ) 恰好等于零或非常接近零投影公式直接除以零。这在真实数据里很常见尤其是优化的中间步骤位姿跳得远时。我做了保护如果 ( Z_c 1e-6 ) 米直接把这个残差块丢出优化并打印警告。别小看这个判断它能帮你省掉至少一个下午的查错时间。第二信息矩阵不小心填成零矩阵。有个残差块构造时信息矩阵初始化为 0 但一直没赋值结果这个“观测”看起来完全可信实际上没有给优化提供任何信息。优化能收敛但某些方向会漂移定位问题耗了很久。后来我加了一个断言任何残差块入队时信息矩阵必须严格正定。第三核函数的雅可比权重没乘。加了 Huber 核函数之后残差雅可比要多乘一个 ( \sqrt{w} ) 的权重或者用加权残差 ( r w \cdot r )。如果只换了核函数代价没改雅可比优化效果跟没加核几乎一样外点照样把解拉偏。5.4 核函数到底怎么选单目 BA 对核函数很敏感。没有核函数时一个错误匹配的观测可能把整个优化结果拉飞。我在代码里实现了 Huber 核阈值设为 1.0在像素单位下等效于当重投影误差大于 1 像素后残差对代价的贡献从二次增长变为线性增长。这样外点不会获得与正常点相当的梯度权重。实际测试中Huber 阈值设太大核函数形同虚设设太小正常点的收敛速度也被拖慢了。在室内 VIO 数据集上1 到 2 像素是比较合理的范围。你可以跑一个自动化脚本对同一段数据分别用 0.5、1.0、2.0、5.0 测试看最终重投影误差中位数和轨迹漂移的变化通常能选出稳定阈值。6. 仿真验证与调试心得怎么确认BA代码真写对了6.1 先造仿真数据再碰真实数据我强烈建议在真实 VIO 数据之前先用仿真数据验证整个 BA 代码。仿真环境的优势是你可以拿到真实位姿和路标点作为真值也能精确控制噪声类型和量级。我的做法是随机生成一个半球形的路标点分布让相机沿着一段正弦轨迹运动每隔一帧拍摄一次把路标点投影到图像并添加少量高斯噪声然后把这些观测喂给求解器。特别要加的“毒药”是少量外点比如把 5% 观测点的像素坐标随机拉到图像边缘。这样既能验证 BA 的优化精确性也能顺便检验核函数是否真正生效。这一步做完我心理踏实很多至少证明框架没有严重的方向性错误。6.2 评估指标不只是看代价函数下降代价函数下降只能说明优化过程“稳定”不能说明优化结果“正确”。我用了三个指标交叉验证。第一个是重投影误差的均值和中位数。优化完后均值应该逼近你加噪的水平比如噪声是 0.5 像素标准差则均值应该在 0.4 到 0.6 像素之间。如果均值远低于噪声水平反而要警惕是不是在过拟合。中位数比均值更抗外点我一般两个都打印。第二个是位姿真值对比。仿真环境里有真值我算优化前后位姿相对真值的相对误差。注意绝对位置误差会有累计效应这更多是前端轨迹质量的问题BA 优化关注的是单次窗口中位姿的相对一致性。第三个是路标点收敛到真值的精度。路标误差能反映优化求解是否把信息“放大”得准确。一个常见现象是代价函数正常收敛但路标点在深度方向依旧很散这通常说明观测几何对深度估计不敏感是退化问题不是代码 bug。6.3 和通用优化库交叉对拍我后来还是拿自己的手写实现和 Ceres 这类通用非线性优化库做了对拍在同一个仿真数据上跑。结论很有意思两边优化后的重投影误差几乎一致位姿轨迹也很接近但代码速度和内存占用各有所长。Ceres 的自动求导很省心但我自己手写雅可比时对“哪一项对应哪个变量”理解得更透彻出问题能直接定位到某个残差块。对拍实验最有价值的点是它让我发现一处 H 矩阵漏加项的 bug。当时手写版在某个位姿变量上多优化了零点几度但对代价影响很小和 Ceres 对比后才发现是漏了同一帧两个残差块共享同一个路标点时H 矩阵累加逻辑错误。6.4 关于调试效率的一点点体会到最后我总结出“三段式”调试法先看残差分布再看梯度信息最后才看增量更新。残差分布能告诉你哪些观测异常大梯度信息能告诉你某个变量是否被充分观察增量更新能定位迭代是否震荡。不要一上来就打印位姿数值那是信息密度最低的输出。我在真实 VIO 数据上跑通这一段后明显感觉到自己对后端的掌控力和之前调库时完全不一样。后面再往系统里加 IMU 因子、滑窗边缘化和先验信息整个框架都是现成的只需要顺着残差块和舒尔补的脉络往下填。如果你也在手写 VIO 后端建议这一节的东西务必亲手敲一遍遇到 bug 不要着急逐一排查雅可比、信息矩阵、阻尼策略和线性求解这四个环节大多数问题都出在这四个地方。