先把结论放在前面：后端优化这一块，是整个VIO系统里“性价比”最高也最容易被低估的部分。前端做得再好，姿态和轨迹也在肉眼可见地往外飘；后端一接上，全局约束一拉，轨迹立刻“收回来”了。这个系列走到第五期，前几篇我们把视觉前端、IMU传播、初始化都过了一遍，这一篇专门聊后端优化从原理到手写代码的整个过程——也是我作业和代码里花时间最多、踩坑最密集的一个模块。
1. 前端把姿态越推越歪：后端优化到底在补什么课
1.1 递推式估计的“近视眼”问题
先说我实际跑过的场景：前端连续跟踪了四五十个关键帧，自认为每帧位姿都给得很准，结果纯旋转扫过走廊再回到出发点，轨迹在地上画出一个扇形缺口——头尾对不上。这不是前端跟踪崩了，而是递推式估计的本质缺陷：每个新帧的位姿都建立在上一帧的基础上，误差不是被消除，而是被“继承”再叠加。就像蒙着眼睛走路的人，每步偏3度，走完一圈偏得离谱。
前端里程计做的事情，本质是帧间运动估计和短窗口的相对定位。它不关心“这个路标点在世界系下到底长在哪”，只关心“我上一帧到这一帧动了多少”。所以它天然缺少全局一致性。后端优化补的正是这一课：把一段历史窗口里所有关键帧位姿、所有路标点放在一起，建立跨帧约束，再联合求解一个“让所有约束同时被满足得最好”的状态估计。这才是SLAM里真正意义上的“定位与建图”。
1.2 从最小二乘到最大后验：后端优化的数学起点
后端优化的问题框架其实可以归到最大后验估计（MAP）上：在给定所有观测z的前提下，求解最可能的状态x*。假设观测噪声服从高斯分布，MAP问题可以转成最小化负对数似然，最终落到一个加权最小二乘问题：
x* = argmin Σ ||r_i(x)||²_Σi
其中r_i是第i个残差项（视觉重投影残差、IMU预积分残差、边缘化先验残差），Σi是该项的信息矩阵（协方差逆矩阵）。每个残差项衡量的是“预测观测量与实际观测量的差距”。所有项加起来构成系统的总代价，剩下的交给非线性优化器去迭代求解。
这里的核心理解是：后端不改变系统能获得哪些信息，而是改变信息的使用方式。前端是“用局部约束更新当前状态”，后端是“用所有历史约束修正窗口内全部状态”。同一个路标点可能被5个关键帧看到，前端只用最近两帧约束它，后端则一次把5个约束全部拉进代价函数。所以后端对漂移的抑制能力是结构性的，不是调调参数能替代的。
1.3 为什么是BA而不是滤波器
学过滤波的同学可能会问：EKF不也能融合IMU和视觉吗？为什么主流VIO方案都转向了优化？关键原因是滤波器的马尔可夫假设——它只维护当前状态的均值和协方差，所有历史信息都被压缩到一阶矩和二阶矩里。遇到高度非线性的视觉几何约束时，EKF的一次线性化近似容易偏差过大，而且一旦线性化点选错，误差没法通过迭代修正。
BA优化则不同，它在每次迭代时对全部变量重新构建残差和雅可比，反复逼近非线性最优解。计算代价确实更大，但换来的是精度和鲁棒性。现代处理器上用滑动窗口BA维持30Hz输出完全可行，所以从精度和可维护性上，优化方案都更有优势。
2. 滑动窗口的本质是“算力换约束”：关键帧选择与窗口裁剪策略
2.1 全局BA为什么在实时VIO里活不下去
理论上最优解是每一帧都丢进优化器，甚至把走过的所有轨迹全部作为变量一起优化——这就是全局BA。但代价是致命的：每加一帧，所有状态变量都可能被更新，优化复杂度随变量数非线性上涨；窗口积累到几百帧后，一次迭代要解的超大线性系统会让实时性彻底崩溃。连续长时间的全局BA，哪怕在桌面级GPU上也需要秒级计算，放到嵌入式平台更不现实。
所以VIO后端普遍采用滑动窗口策略：固定一个窗口大小（通常是10到15个关键帧），新帧进、旧帧出，每轮只优化窗口内的状态变量。这是一种典型的“算力换约束”思路——把历史信息用边缘化先验压缩成少量残差项，形式上还在约束窗口内的变量，但计算量被限制在可接受的范围内。
2.2 哪些帧值得进窗口：关键帧选择策略
不是所有图像帧都值得作为优化变量。关键帧选择直接决定约束质量和计算量。我采用的策略是三个条件取“或”：
- 视差足够大：与最近关键帧的平均视差超过阈值（比如20像素），说明该帧提供了足够的几何观察；
- IMU预积分量变化大：旋转或平移增量达到阈值，说明IMU约束发生了显著变化；
- 共视特征数跌出阈值：说明当前帧与窗口内其他帧的联系减弱，需要新关键帧把约束链条接上。
纯旋转的场景特别容易出问题——视差很小的纯旋转会让视觉三角化退化，导致约束空洞。所以视差判断是关键帧选择的灵魂，别因为画面变化大就急着插帧，要看真正的几何视角变化。
2.3 丢帧不是删数据：信息转移的关键
滑动窗口的默认做法是“新帧进来，最老帧踢出去”。但如果只是直接从变量表里删掉老帧的位姿，它与地图点、与相邻帧之间的约束就凭空消失了。这些约束对应的信息矩阵分量如果被粗暴丢弃，系统会丢掉历史积累的“记忆”，前后端精度都会受损。
正确做法是先边缘化，再用边缘化先验把信息转移给剩余变量。整个过程的核心是舒尔补——把被删帧对应的信息矩阵分块消去，把它的信息“叠加”到剩余块上。这样，老帧虽然不再是优化变量，但它留下的约束信息仍然以先验形式存在于代价函数中。这个差别是滑动窗口后端能否长期维持精度的分水岭。
3. 三类残差如何塞进同一个代价函数：视觉、IMU预积分、边缘化先验
3.1 视觉重投影残差：把3D点投到像素再比较误差
视觉残差是后端最核心的观测项。每一个被多个关键帧观测到的路标点，在每一帧上产生一个残差：先根据当前估计的路标点世界坐标和该帧位姿，把点投影到像素平面，再与观测到的像素坐标求差。
e_vis = z_obs - π(T_wc · P_w)
其中π是相机投影函数（含畸变模型）。实现时最常踩的坑是坐标系不一致：路标点的坐标是世界系下，投影时要先转到相机系，再经过内参和畸变；位姿虽然是李群形式，但在代码里往往以四元数+平移存，做变换时容易搞混旋转顺序。还有一个容易忽略的小细节就是摄像机和IMU的外参——如果你在优化的是IMU系位姿，那么视觉残差里要额外串一次外参矩阵。
3.2 IMU预积分残差：两帧之间的“加速度计+陀螺仪仲裁”
IMU的原始测量是角速度和加速度，不能直接作为姿态约束。相邻两个关键帧之间需要积分IMU测量，得到旋转、速度、位置的增量作为相对约束。这个积分过程如果放在每次优化迭代里重新做，计算量爆炸；预积分的思想是提前把两帧之间的相对增量计算出来，优化迭代时只更新与偏置相关的部分。
预积分残差形式上很简单：用当前估计的状态计算出的“两帧间应该有的增量”减去“预积分测量得到的增量”。但难点在于两个地方：一是偏置变化时，预积分量需要一阶近似更新，这里要维护预积分量对偏置的雅可比矩阵；二是IMU噪声通过积分会累积，残差的协方差矩阵需要通过连续时间噪声传播来递推计算。很多作业里残差算对了，但信息矩阵给错，导致优化权重失衡，表现就是轨迹“不自信”，一会儿信视觉一会儿信IMU，剧烈抖动。
3.3 边缘化先验残差：被滚出窗口的那一帧想说的话
边缘化产生的先验残差，形式上是一个固定维度的残差项，约束的是窗口内剩余变量的线性组合。它在代价函数里占的项数不多，但信息矩阵是稠密块，常常会显著改变增量方程的求解结构。
边缘化先验残差注意两点：一是它的残差值和雅可比要基于边缘化那一刻的线性化点，一旦窗口内变量的估计更新，先验残差要跟着变化，但雅可比保持固定；二是先验信息矩阵的对角占优问题——如果某个变量和先验关联度特别高，增量方程求解时可能出现数值病态，需要额外正则化。
3.4 总代价函数怎么拼装
三类残差在一个系统里不是各管各的，而是共同构成总代价函数：
E_total = Σ_vis ||e_vis||² + Σ_imu ||e_imu||² + ||e_prior||²
视觉残差是稀疏的（一个路标点只关联少量帧），IMU残差是稠密链式的（窗口内相邻帧之间），先验残差关联变量范围最广。优化器每次迭代时，对所有残差项在当前线性化点计算雅可比，加权组合，构建增量方程。谁的权重足（信息矩阵大），谁的约束就越“硬”。
这里有个值得反复体会的点：视觉残差和IMU残差是互补的。纯视觉在无纹理区域退化，纯IMU在长时间运动后漂移大；后端优化的本质就是让这两类约束不断“互相纠正”。窗口内视觉得到的相对约束修正IMU漂移，IMU提供的尺度与重力方向约束修正视觉单目尺度模糊。这也是VIO比单独视觉或单独IMU都稳的根本原因。
4. 边缘化的数学内核：舒尔补、信息矩阵与零空间问题
4.1 一个具体例子：把最老帧边缘化会发生什么
假设当前窗口状态量分成两部分：要被去掉的老帧相关变量x_m，和要保留下来的变量x_r。构建的增量方程形如：
[H_mm H_mr; H_rm H_rr] [δx_m; δx_r] = [b_m; b_r]
直接把老帧删掉意味着忽略H_mm、H_mr、H_rm块，等价于白白扔掉这些信息。边缘化要做的是用舒尔补把老帧对保留变量的影响“压缩”进新的信息矩阵：
H_rr_new = H_rr - H_rm · H_mm⁻¹ · H_mr b_rr_new = b_r - H_rm · H_mm⁻¹ · b_m
新信息矩阵H_rr_new就是边缘化先验的信息矩阵。它保留了老帧通过约束传递给剩余变量的全部影响。代价是H_rr_new不再是稀疏的——因为老帧连接了多个保留变量，消去之后这些保留变量之间出现新的关联块。
我在代码里用Eigen的块操作实现这一过程。实现时最关键的是求H_mm的逆：H_mm本身是稀疏的，但求逆后通常变稠密，所以要先做稀疏Cholesky分解再回代，而不是直接调用稠密矩阵求逆。手写前两版我直接用了稠密求逆，窗口到15帧、路标点几百个时，单次边缘化耗时飙到几百毫秒，后来改成稀疏分解才降到毫秒级。
4.2 数学图像上发生了什么：信息矩阵的谱结构
边缘化有一个很容易被忽视的性质：老帧被消去后，保留变量的信息矩阵会变稠密，这可能引入虚假的强关联。更麻烦的是，边缘化时如果不同残差的雅可比在线性化点不一致，会人为放大某些方向的置信度，导致系统“过度自信”——明明没有观测支撑，却信誓旦旦地锁死某个自由度。这就是FEJ（First Estimate Jacobian）问题的由来。
所谓FEJ，简单说就是同一个变量在视觉残差、IMU残差、边缘化先验中被线性化时，要统一使用该变量第一次被估计时的线性化点，而不是各自用当前的估计值重新线性化。否则相当于同一个状态被“观测”了多次——每次观测都用不同的参考点，等于人为重复计数，等价于信息矩阵被夸大。
实现在代码层面就是在构建雅可比时，对固定变量统一从缓存中取线性化点。比如路标点在窗口期间会多次被优化更新，但视觉残差计算雅可比时用的路标点坐标，应该是它被首次三角化时的值（或其固定代理值），而不是最新估计值。调试时可以用“信息矩阵的特征值”检查：如果某个方向的特征值异常大且找不到对应的真实观测支撑，多半就是FEJ没处理好。
4.3 边缘化的触发时机与顺序
不是每次优化迭代都要做边缘化。边缘化发生在“有旧帧要被移出窗口”时，通常跟随关键帧管理一起触发。边缘化的顺序有讲究：一般先边缘化最老的关键帧对应的位姿，同时把与视觉路标点相关的稠密部分处理掉，再处理IMU的偏置变量。相关变量如果顺序杂乱，信息矩阵的填充模式会变得不可控，稀疏性被破坏，后续求解性能明显下降。
5. 手写增量方程求解时的那些“一列火车都装不下”的坑
5.1 H矩阵的稀疏结构值得你好好设计数据结构
后端优化最核心的计算瓶颈不在残差和雅可比计算，而在解增量方程。设所有状态变量维度为n，载入全部雅可比之后的信息矩阵H是n×n，直接稠密存储完全不可行——关键帧10个、路标点500个时，n轻松到几千维，稠密矩阵光存储就几十MB量级，求解更是灾难。
解决方案是利用H的稀疏性。我把变量分为两类：位姿/速度/偏置这类“小变量”和路标点这类“大变量”。视觉约束只在观测到同一个路标点的帧之间存在块，IMU约束只在相邻帧之间存在块，所以H的结构基本上是带状+对角块的混合。存成块稀疏格式后，一个窗口的H矩阵占用不过几MB。
具体实现我用了Eigen的SparseMatrix，自定义块结构，按列分配非零元素。这里有一个经验：预分配稀疏矩阵的nonZeros数会大幅加速，别让Eigen频繁动态扩张。
5.2 LM的λ怎么更新，残差为什么降不下去
增量方程是带正则化的阻尼牛顿形式：
(J^T J + λI) δx = -J^T r
λ决定迭代步长：λ大时退化为梯度下降（稳健但慢），λ小时接近牛顿法（快但容易发散）。经典的策略是：每次迭代后计算实际下降量与一阶近似下降量的比值ρ，ρ接近1说明近似好，减小λ；ρ太小说明近似差，增大λ并丢弃本次迭代。
手写实现里最容易翻车的点有两个。第一，λ更新要和“拒绝迭代”逻辑配合好：一旦ρ小于某阈值（比如0.25），当前增量被拒绝，状态不变，λ翻倍重算；很多初学者忘了拒绝状态更新，导致残差越优化越大。第二，H矩阵半正定时加λ才对特征值有拉高效果，但如果λ初始值太大，前面几步迭代会变得极其缓慢，看起来“粘住了”——要配合残差下降曲线判断是否还在正常优化轨道上。
5.3 求解后如何把增量回代到状态里：注意顺序
增量δx不是直接加在状态变量上的。位姿是李群元素，增量在李代数空间，需要用指数映射更新：
T_new = exp(δξ^) · T_old
平移和速度是向量，可以直接加；偏置也可以直接加，但要小心偏置一旦更新，IMU预积分残差的测量值也要根据偏置修正一阶近似更新。路标点则是直接加增量。这个顺序问题在初版代码里让我吃亏不小：先更新了位姿，再回头更新预积分相关量，导致残差里用的“当前状态”总比新状态慢半拍，花了很长时间才理清。
更系统性的做法是更新状态后，将窗口内与IMU预积分相关的“测量值”做一次偏置补偿刷新，再进入下一轮迭代。这样残差计算的基准才和当前状态一致。
5.4 收敛判断：别只盯着位姿模长
后端迭代的收敛条件通常是：
- 增量范数小于阈值（比如所有变量增量均方根小于1e-6）；
- 残差下降量小于阈值（比如与上一轮相比下降小于1e-5）；
- 达到最大迭代次数。
我踩过的坑是只盯着位姿增量判断收敛，忽略了路标点的更新。路标点维度占比极大，位姿增量范数看起来已经很小，其实路标点的增量还远未收敛。判断收敛时应该把位姿、偏置、路标点的增量分开计算，取各自的模长，再综合判定。
6. 验证后端写对了没有：从零更新测试到残差曲线审查
6.1 零更新测试：不给优化，一切应当静止
后端代码写完第一件事，不是跑数据，而是跑零更新测试：构建完所有残差和H矩阵后，不执行任何优化迭代，直接看增量方程的解是否全为零。理论上，如果所有残差项在初始状态下的雅可比正确，增量方程在初始点求得的δx应该是接近零的。
但这个测试通过不代表系统正确，因为残差可能恒为零（比如投影函数搞错导致残差永远等于0）。所以零更新测试只能排除“连基本线性化都错”的粗大错误，正确的残差验证还要靠数值梯度检查。
6.2 数值梯度检查的妙用
数值梯度检查是后端开发最重要的诊断工具。做法很简单：对单个残差项，给某个状态分量加一个微小扰动ε，用数值差分近似雅可比，与解析雅可比对比：
J_num = (r(x+εe_i) - r(x-εe_i)) / 2ε
两者的一致性检验很严格：维度可以小到1e-6，但数值差分本身有截断误差，所以通常看相对误差的位数，而不是绝对相等。逐项检查视觉重投影残差、IMU预积分残差对位姿、偏置、路标点的雅可比，能快速定位“哪一项雅可比公式写错，或者坐标变换漏了一步”。初版代码里我靠这个检查抓出了三个隐藏问题，其中两个都是坐标变换顺序颠倒。
6.3 真实数据上的“掐表看趋势”
跑公开数据集时，不要只看最终轨迹漂移大不大，要分阶段判断：
- 后端开启前，看前端轨迹是否已经积累漂移；
- 后端开启后，看轨迹飘逸变化是否被“收住”；
- 优化前后轨迹对比，看绝对位置误差和相对位置误差的曲线。
还要盯残差下降曲线：每次迭代的残差和应当单调下降（除非λ拒绝导致的反复），如果出现残差振荡上升，说明增量步长过大或雅可比存在符号错误。用曲线和图的方式审查，比盯着数字准得多——数字能骗人，曲线趋势很难骗人。
6.4 从自建小场景到复杂路线的调试策略
我调试时习惯从自采小场景入手：室内桌面或走廊，可控、纹理适中、运动平滑。先在这样的小场景上用简单的运动方式（原地旋转、小范围平移）跑通流程，再逐步增加场景复杂度。直接上复杂数据集调试，轨迹一大、约束一多，出问题根本定位不到哪一环。
调试顺序建议：
- 单个视觉残差项，验证雅可比。
- 单个IMU预积分残差，验证雅可比。
- 固定路标点，只优化位姿。
- 固定位姿，只优化路标点。
- 完整窗口优化，先不用边缘化。
- 最后接入边缘化先验。
每步都跑零更新测试和数值梯度检查，确认没问题再进入下一步。这个顺序相当于把系统拆成互不干扰的验证单元，比一上来就全开调要省太多时间。
收尾：一个我反复用的调试习惯
最后分享一个我每次写后端都会坚持的调试习惯：维护一个残差项和雅可比的“数值检查”开关，用环境变量控制。开发阶段始终打开，跑数据前先做一遍全量检查再进入实际运行；上线或提交实验前，再关闭并跑一遍完整数据流。这个习惯帮我避免过好几次“改了一处矩阵维数，另一个残差被连带影响”的隐性故障。手写VIO的后端优化，真正的门槛不在抄公式，而在把公式变成可调、可信、可复现的代码过程——把这个过程理顺了，你才算真正“拥有”了这套后端，而不只是照着别人的框架改参数。
