1. 从“能跑通”到“真明白”：为什么我坚持手推VIO第六章的每个公式
很多人做视觉惯性里程计（VIO）的作业，习惯直接翻开源代码，把参数一填、编译一跑，看到轨迹和真值对上了就收工。我最初也是这个路子，直到有一次在第六章的滑动窗口优化里，轨迹突然在某个转弯处炸开，残差曲线像心电图一样乱跳，我才意识到——那些被我跳过的公式推导，恰恰是定位系统能不能稳住的关键。
第六章在整个VIO学习路径里是个分水岭。前面几章讲IMU预积分、视觉观测模型，更多是“怎么把量测建出来”；到了第六章，核心问题变成了“怎么在有限算力下，把历史状态和当前状态放在一起，做一次既快又准的联合优化”。这就是滑动窗口优化和边缘化要解决的事。标题里说的“含公式推导依据及编程思路”，我理解重点不在“推导”本身，而在于搞清楚每个矩阵块是怎么来的、为什么这么填、填错了会怎样。
这篇文章适合两类人：一类是正在做VIO第六章作业、被H矩阵和边缘化搞得头大的同学；另一类是想从“调包侠”进阶到“能自己改优化器”的开发者。我会把公式背后的物理意义、编程时的矩阵组装顺序、以及我踩过的几个典型坑，按我自己的理解重新讲一遍。不追求数学上的绝对严谨，但求你看完之后，能对着代码说清楚每一行在干什么。
提示：本文涉及的推导均基于公开的VIO理论框架，代码思路以C++和Eigen为主，不依赖任何特定平台或私有库。
2. 滑动窗口里的“取舍之道”：边缘化到底在边缘掉什么
2.1 从“全量优化”到“窗口优化”的动机
先想一个最朴素的问题：如果我把从开机到现在的所有相机位姿、速度、IMU零偏都放进优化器，会怎样？答案很简单——状态向量会无限膨胀，H矩阵的维度跟着涨，求解时间从毫秒级变成秒级，最后连实时性都保不住。但历史信息又不能全扔，因为当前帧的约束很多来自过去帧的观测。
滑动窗口的思路就是：只保留最近N帧的状态，把更早的状态“挤”出去。但“挤”不是直接删除，直接删除等于丢掉了那些帧对窗口内状态的约束，优化结果会漂。正确的做法是边缘化：把要移除的状态通过舒尔补（Schur complement）的方式，把它们的约束信息“折叠”进剩下的状态里。
这里有个很形象的类比。假设你在做一个团队项目，有个老成员要离职。直接让他走人，他手里的客户关系就断了；正确的做法是让他把客户对接给接替的人，这样客户资源还在团队里。边缘化就是那个“对接”的过程，舒尔补就是对接的具体操作。
2.2 舒尔补的矩阵形式与物理含义
假设我们把状态分成两部分：待边缘化的状态 ( x_m ) 和保留的状态 ( x_r )。H矩阵和梯度向量也相应分块：
[ \begin{bmatrix} H_{mm} & H_{mr} \ H_{rm} & H_{rr} \end{bmatrix} \begin{bmatrix} \delta x_m \ \delta x_r \end{bmatrix}
\begin{bmatrix} b_m \ b_r \end{bmatrix} ]
舒尔补的操作是消去 ( \delta x_m )，得到只关于 ( \delta x_r ) 的方程：
[ (H_{rr} - H_{rm} H_{mm}^{-1} H_{mr}) \delta x_r = b_r - H_{rm} H_{mm}^{-1} b_m ]
左边那个新矩阵就是舒尔补矩阵，它把 ( x_m ) 的信息“压缩”进了 ( x_r ) 的方程里。编程时你不需要真的去求 ( H_{mm}^{-1} )，而是用矩阵分解（比如LDLT）来解这个线性系统。
我一开始不理解为什么边缘化之后信息矩阵会变稠密。后来想通了：原本 ( x_r ) 内部可能只有稀疏连接，但通过 ( H_{rm} H_{mm}^{-1} H_{mr} ) 这一项，所有和 ( x_m ) 有联系的 ( x_r ) 变量之间都产生了新的关联。这就像团队里两个人本来不熟，但通过一个共同对接的客户，他们之间也有了信息传递。
2.3 先验信息的“锚定”作用
边缘化之后，被移除状态的信息并没有消失，而是变成了一个先验项，加在后续的优化里。这个先验项的形式是：
[ \frac{1}{2} | x_r - x_r^{prior} |^2_{H_{prior}} ]
其中 ( H_{prior} ) 就是舒尔补矩阵。这个先验的作用是“锚定”窗口内的状态，防止它们在后续优化中漂移。如果没有这个先验，窗口滑动几次之后，整个轨迹就会像断了线的风筝。
我在实际调试时发现，先验的权重如果给得太小，轨迹会在窗口边缘出现明显的“折角”；给得太大，又会导致优化器对新观测不敏感，轨迹反应迟钝。这个权重的调节，本质上是在“相信历史”和“相信当前观测”之间找平衡。
注意：边缘化时一定要保证被边缘化的状态和保留状态之间的观测是完整的。如果某个观测同时涉及被边缘化状态和保留状态，这个观测必须被保留下来参与舒尔补，否则信息会丢失。
3. H矩阵的组装：从残差块到全局矩阵的映射逻辑
3.1 残差块的定义与雅可比计算
第六章的优化问题里，残差主要来自三部分：IMU预积分残差、视觉重投影残差、以及边缘化先验残差。每个残差块都对应一个雅可比矩阵，雅可比矩阵决定了这个残差对各个状态变量的敏感度。
以IMU预积分残差为例，它涉及两个关键帧的位姿、速度、零偏。残差向量通常是15维（位置、姿态、速度、加速度零偏、陀螺仪零偏），对应的雅可比矩阵是一个15×30的块（因为涉及两个时刻的状态，每个时刻15维）。这个雅可比的计算在预积分理论里有标准公式，但编程时最容易出错的是姿态的扰动方式。
我见过不少同学在计算姿态雅可比时，直接用四元数的加法，结果优化根本不收敛。正确的做法是用扰动模型：在姿态的切空间上做小量扰动，然后推导残差对扰动的导数。具体来说，如果姿态用四元数 ( q ) 表示，扰动是 ( \delta \theta )，那么：
[ q \oplus \delta \theta \approx q \otimes \begin{bmatrix} 1 \ \frac{1}{2} \delta \theta \end{bmatrix} ]
雅可比就是残差对这个 ( \delta \theta ) 的导数。这个细节在代码里就是一个左乘还是右乘的区别，但结果天差地别。
3.2 信息矩阵的物理意义与调参经验
每个残差块都有一个对应的信息矩阵，它表示这个观测的“可信度”。IMU预积分的信息矩阵来自预积分协方差，视觉重投影的信息矩阵通常取单位阵乘以一个权重。
这里有个我踩过的坑：一开始我把视觉信息矩阵设得很大，觉得视觉观测更准。结果优化器几乎忽略了IMU的约束，轨迹在视觉纹理缺失的地方直接飞掉。后来我把视觉权重降到和IMU协方差匹配的量级，轨迹才稳下来。
信息矩阵的调参没有万能公式，但有一个原则：信息矩阵应该反映观测的真实不确定度。IMU预积分的协方差可以通过预积分传播计算出来，视觉重投影的协方差和特征点提取的精度有关。如果你不确定，可以先都设为单位阵，然后根据残差曲线的表现微调。
3.3 全局H矩阵的稀疏结构与组装顺序
把所有残差块的雅可比和信息矩阵乘起来，再按照状态变量的顺序“填”进全局H矩阵，这就是组装过程。全局H矩阵是稀疏的，因为每个残差只涉及少数几个状态。
编程时，我习惯用一个哈希表来管理状态变量的索引：每个状态变量（比如某个关键帧的位姿）对应一个唯一的ID，组装时通过ID找到它在全局矩阵中的位置。这样即使窗口滑动、状态增删，索引管理也不会乱。
组装顺序上，我建议先处理IMU残差，再处理视觉残差，最后加边缘化先验。因为IMU残差涉及的变量少、结构规整，先填进去可以让H矩阵的稀疏模式更清晰；视觉残差涉及变量多，后填可以避免频繁的索引查找。
| 残差类型 | 涉及状态 | 雅可比维度 | 信息矩阵来源 |
|---|---|---|---|
| IMU预积分 | 两帧位姿、速度、零偏 | 15×30 | 预积分协方差传播 |
| 视觉重投影 | 两帧位姿、路标点 | 2×(6+6+3) | 特征点提取精度 |
| 边缘化先验 | 窗口内保留状态 | 与保留状态维度一致 | 舒尔补矩阵 |
提示：组装H矩阵时，一定要保证雅可比的符号和残差的定义一致。我见过因为符号搞反导致优化方向完全相反的案例，轨迹直接往反方向跑。
4. 编程实现中的“暗礁”：从矩阵分解到窗口滑动的完整链路
4.1 线性求解器的选择与LDLT分解
H矩阵是对称正定的（在加了先验和阻尼之后），所以可以用LDLT分解来求解。Eigen里直接有Eigen::LDLT可以用，但要注意H矩阵的稀疏性——如果直接用稠密LDLT，窗口稍微大一点就会很慢。
我的做法是用Eigen::SimplicialLDLT，它针对稀疏矩阵做了优化。组装时用Eigen::SparseMatrix的三元组列表（Triplet）来填，最后一次性压缩成CSR格式。这样比逐个插入元素快很多。
还有一个细节：LDLT分解要求矩阵正定，但边缘化后的舒尔补矩阵可能只是半正定。这时候需要加一个小的阻尼项 ( \lambda I )，保证数值稳定性。阻尼的大小我一般取1e-6到1e-4之间，太小了不稳定，太大了收敛慢。
4.2 窗口滑动的触发条件与状态管理
窗口滑动不是每来一帧就滑，而是当窗口内的关键帧数量超过阈值时才触发。阈值的选择和相机的帧率、场景的运动速度有关。我一般设10到15帧，运动快的时候少一点，运动慢的时候多一点。
滑动时，最老的关键帧被边缘化。但要注意：如果最老的关键帧和次老的关键帧之间有很强的共视关系，直接边缘化最老帧可能会导致信息丢失。这时候可以先边缘化路标点，再边缘化关键帧，分两步走。
状态管理上，我用一个双端队列来存关键帧，队列头是最老的，队列尾是最新的。每次滑动时，从队列头弹出，同时更新哈希表里的索引映射。这个映射关系一定要维护好，否则组装H矩阵时会访问到已经释放的内存。
4.3 残差计算中的数值稳定性处理
残差计算里最容易出数值问题的是四元数的归一化和旋转矩阵的正交化。优化过程中，四元数可能会因为数值误差而不再归一化，导致旋转矩阵不再是正交阵。我的做法是每次迭代后都对四元数做一次归一化，并且在计算旋转矩阵时用归一化后的四元数。
另一个问题是深度值。视觉重投影残差里涉及路标点的深度，如果深度估计不准，残差会很大。我在代码里加了一个深度值的下限保护，比如最小深度设为0.1米，防止深度跑到负数或零导致除零错误。
还有一个容易被忽略的点：残差的计算顺序。如果先算视觉残差再算IMU残差，视觉残差里的位姿可能还是上一轮迭代的旧值。正确的做法是每轮迭代开始时，先把所有状态变量更新到当前估计值，再统一计算所有残差。
5. 调试实录：轨迹漂移、矩阵奇异与收敛异常的排查路径
5.1 轨迹在窗口边缘出现“折角”的根因定位
我第一次跑通第六章代码时，轨迹在窗口边缘总是出现一个明显的折角，像是被什么东西拽了一下。排查过程是这样的：
第一步，检查边缘化先验的权重。我把先验信息矩阵乘以一个系数，从1.0调到0.1，折角变小了但没消失。说明先验权重有影响，但不是根因。
第二步，检查被边缘化状态的观测完整性。我发现有一个视觉观测同时涉及被边缘化帧和保留帧，但在组装舒尔补时，这个观测的雅可比块没有被正确包含进去。修正之后，折角消失了。
这个问题的本质是：边缘化时，所有涉及被边缘化状态的观测都必须参与舒尔补，否则信息会丢失。我在代码里加了一个检查：遍历所有残差块，如果某个残差涉及被边缘化状态，就把它标记为“必须参与边缘化”。
5.2 H矩阵奇异的三种常见诱因
H矩阵奇异是优化里最让人头疼的问题之一。我遇到过三种情况：
第一种是状态变量不可观。比如窗口里只有一个关键帧，但IMU残差涉及两个时刻的状态，这时候H矩阵的某些块是零，导致奇异。解决办法是保证窗口里至少有两个关键帧。
第二种是信息矩阵为零。如果某个残差的信息矩阵设成了零矩阵，那这个残差对H矩阵没有贡献，对应的状态变量就没有约束。检查方法是打印每个残差块的信息矩阵，看有没有全零的。
第三种是数值误差累积。长时间运行后，H矩阵的条件数变得很大，LDLT分解失败。这时候加阻尼项或者定期做一次全量优化（如果算力允许）可以缓解。
5.3 收敛曲线震荡的调参思路
收敛曲线震荡通常和阻尼系数、学习率有关。我的调参顺序是：先调阻尼系数，再调信息矩阵权重，最后调窗口大小。
阻尼系数从1e-6开始，每次乘以10，直到曲线不再震荡。如果阻尼加到1e-2还震荡，那大概率是雅可比计算有误，需要回去检查公式。
信息矩阵权重的影响更隐蔽。如果IMU和视觉的权重不匹配，优化器会在两个约束之间“打架”，表现为残差曲线周期性起伏。我的经验是让视觉权重大致等于IMU协方差逆的迹的均值，这样两者的量级相当。
| 现象 | 可能原因 | 排查方法 | 解决措施 |
|---|---|---|---|
| 窗口边缘折角 | 边缘化观测不完整 | 检查涉及被边缘化状态的残差 | 补全舒尔补中的雅可比块 |
| H矩阵奇异 | 状态不可观/信息矩阵为零 | 打印H矩阵秩、检查信息矩阵 | 增加关键帧、修正信息矩阵 |
| 收敛震荡 | 阻尼不当/权重不匹配 | 调阻尼系数、对比残差量级 | 加阻尼、匹配信息矩阵量级 |
注意：调试时一定要把中间变量打印出来，比如每轮迭代的残差范数、H矩阵的条件数、舒尔补矩阵的最小特征值。这些数字比轨迹图更能说明问题。
6. 从作业到实战：这套优化框架还能怎么用
第六章的滑动窗口优化框架，本质上是一个带先验的稀疏非线性最小二乘求解器。它的应用远不止VIO作业。我在后来的几个项目里，把这套框架直接搬到了其他问题上。
比如做激光雷达里程计时，点云配准的残差也可以用同样的方式组装H矩阵，只是雅可比的计算方式不同。再比如做多传感器融合时，把不同传感器的残差块按同样的逻辑填进全局矩阵，边缘化的策略也类似。
如果你想继续深入，我建议从两个方向扩展：一是把固定窗口改成动态窗口，根据场景的复杂度自动调整窗口大小；二是引入鲁棒核函数，处理视觉里的外点。这两个方向在公开的VIO文献里都有成熟方案，但自己动手实现一遍，理解会完全不一样。
我个人在实际操作中的体会是：第六章的公式推导，第一遍看会觉得繁琐，第二遍对着代码看会清晰很多，第三遍自己从头写一遍，才算真正掌握。那些矩阵块不是数学游戏，它们对应的是“哪个状态在约束哪个状态”的物理关系。把这个关系想清楚了，代码自然就写对了。
