在写完 VIO 前端、搞定 IMU 预积分和对齐之后，我一直觉得自己的系统里少了一块“定海神针”——后端优化。前端给出来的位姿和路标点就像刚出锅的菜，看着行，但噪声大、漂移明显，必须放到 BA（Bundle 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.07133 | 0.014% |
| ( \partial r_v / \partial X_w ) | 0.00521 | 0.00522 | 0.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
我手写的求解器用一个简单的 LM（Levenberg-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 不要着急，逐一排查雅可比、信息矩阵、阻尼策略和线性求解这四个环节，大多数问题都出在这四个地方。
