1. 视觉惯性里程计后端优化到底在解决什么问题
视觉惯性里程计（VIO）走到第六章，基本上就进入了整个系统最核心也最硬核的部分——后端优化。前面的章节你可能已经搭好了前端的光流跟踪、IMU预积分、初始化流程，但那些更多是在“搭架子”，真正决定整个系统精度和鲁棒性的，是后端如何把视觉观测和惯性观测融合在一起做联合优化。
我先说清楚这一章要干什么。简单讲，VIO后端要解决的是一个最大后验估计问题：给定一串IMU测量和若干帧图像特征观测，估计出每一时刻的位姿、速度、零偏，以及路标点的三维坐标。这个问题的数学本质是一个非线性最小二乘，通常用滑动窗口的方式控制计算量，窗口内做联合优化，窗口外做边缘化。
为什么后端这么重要？因为前端光流跟踪会漂，IMU积分会飘，单独拿出来都撑不住长时间运行。后端的作用就是把这些有噪声的观测放在一个统一的目标函数里，通过迭代优化让所有状态量互相“拉扯”到一个自洽的解。你可以理解为：前端负责提供线索，后端负责当法官，把所有线索综合起来给出最合理的判决。
这一章的作业通常要求你手写一个基于滑动窗口的VIO后端，包括构建信息矩阵、求解增量方程、处理边缘化先验。听起来吓人，但拆开来看就是几个模块的拼接。下面我按实际写代码的顺序，把每个模块的设计思路、公式推导和实现细节都过一遍。
1.1 为什么选择滑动窗口而不是全局优化
全局BA当然精度最高，但计算量随时间线性增长，跑几十秒就卡死了。滑动窗口的思路是只保留最近N帧的状态，老的状态通过边缘化转成先验信息塞进信息矩阵里。这样计算量恒定，同时保留了历史信息对当前估计的约束。
这里有个关键取舍：窗口越大精度越好但越慢，窗口越小越快但容易丢信息。实际工程里通常取10到20帧，具体看你的算力和场景动态程度。我自己的经验是，室内低速场景10帧够用，室外高速或者剧烈运动建议15帧以上。
边缘化的数学操作是Schur补。把状态向量分成两部分：要保留的（当前窗口内的状态）和要丢弃的（滑出窗口的老状态）。对信息矩阵做Schur补，就能把老状态的影响“凝聚”成一个先验项加到保留状态上。这个操作会引入填充（fill-in），让信息矩阵变稠密，这是滑动窗口不可避免的代价。
注意：边缘化时一定要保证被边缘化的状态和保留状态之间的约束被正确传递，否则会丢失信息导致估计不一致。很多初学者在这里出错，表现为优化结果逐渐漂移甚至发散。
2. 状态向量定义与信息矩阵构建的完整推导
2.1 状态向量到底包含哪些量
在VIO后端里，状态向量通常定义为：
$$ \mathbf{x} = [\mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_N, \lambda_0, \lambda_1, \dots, \lambda_M] $$
其中每个IMU状态 $\mathbf{x}_k$ 包含：
$$ \mathbf{x}_k = [\mathbf{p}_k, \mathbf{q}_k, \mathbf{v}_k, \mathbf{b}_a, \mathbf{b}_g] $$
分别是位置、旋转四元数、速度、加速度计零偏、陀螺仪零偏。路标点用逆深度 $\lambda$ 参数化，这样数值上更稳定，尤其是远距离点。
为什么用逆深度而不是深度？因为逆深度的不确定性更接近高斯分布，优化时收敛更好。远距离点的深度可能从10米到无穷远，用深度参数化的话初值很难给，用逆深度的话初值给0就代表无穷远，很自然。
零偏为什么放在状态里估计而不是提前标定？因为零偏会随温度和时间缓慢变化，在线估计能实时补偿。但零偏也不能频繁大幅变化，所以要在优化里加随机游走约束，限制相邻时刻零偏的变化量。
2.2 信息矩阵的块结构
信息矩阵 $\mathbf{H}$ 是整个优化问题的核心，它的结构直接决定了求解效率。对于VIO问题，$\mathbf{H}$ 天然具有稀疏块结构：
- 对角线上是每个状态块的自身信息
- IMU约束连接相邻两帧的IMU状态
- 视觉约束连接观测帧的IMU状态和对应的路标点
这个稀疏性非常重要，因为我们可以用Schur补先消去路标点，得到一个只关于IMU状态的缩减矩阵，然后再求解。这就是所谓的“边缘化路标点”，能把求解规模从 (N×15 + M) 降到 N×15，M通常远大于N，所以加速非常明显。
具体操作是：把 $\mathbf{H}$ 按IMU状态和路标点分块：
$$ \mathbf{H} = \begin{bmatrix} \mathbf{H}{xx} & \mathbf{H}{x\lambda} \ \mathbf{H}{\lambda x} & \mathbf{H}{\lambda\lambda} \end{bmatrix} $$
然后对 $\mathbf{H}_{\lambda\lambda}$ 求逆做Schur补：
$$ \mathbf{H}{xx}' = \mathbf{H}{xx} - \mathbf{H}{x\lambda} \mathbf{H}{\lambda\lambda}^{-1} \mathbf{H}_{\lambda x} $$
右端项也做相应变换。由于 $\mathbf{H}_{\lambda\lambda}$ 是块对角矩阵（每个路标点只和观测到它的帧有关），求逆可以逐块进行，效率很高。
实操心得：构建 $\mathbf{H}$ 矩阵时一定要用稀疏矩阵存储，否则内存和计算都会爆炸。我一般用Eigen的SparseMatrix，填充时用triplet列表最后统一构建，比逐个insert快很多。
3. IMU预积分与视觉重投影的残差构建
3.1 IMU预积分的残差怎么算
IMU预积分的核心思想是：在两个关键帧之间，把IMU测量积分成相对运动约束，这样优化时改变关键帧状态不需要重新积分。预积分的残差定义为：
$$ \mathbf{r}_{\text{IMU}} = \begin{bmatrix} \mathbf{r}_p \ \mathbf{r}q \ \mathbf{r}v \ \mathbf{r}{ba} \ \mathbf{r}{bg} \end{bmatrix} $$
其中位置残差：
$$ \mathbf{r}_p = \mathbf{q}_i^{-1}(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}i \Delta t + \frac{1}{2}\mathbf{g}\Delta t^2) - \hat{\alpha}{ij} $$
速度残差：
$$ \mathbf{r}_v = \mathbf{q}_i^{-1}(\mathbf{v}_j - \mathbf{v}i + \mathbf{g}\Delta t) - \hat{\beta}{ij} $$
旋转残差：
$$ \mathbf{r}q = 2[\hat{\gamma}{ij}^{-1} \otimes \mathbf{q}_i^{-1} \otimes \mathbf{q}j]{xyz} $$
零偏残差就是相邻时刻零偏之差。
这些残差对状态量的雅可比需要仔细推导，尤其是旋转部分涉及四元数的扰动模型。我建议在纸上先推一遍，再写代码，否则很容易搞错符号。
3.2 视觉重投影残差与鲁棒核函数
视觉残差是路标点在归一化平面上的重投影误差：
$$ \mathbf{r}{\text{vis}} = \mathbf{z}{ij} - \pi(\mathbf{q}_i^{-1}(\mathbf{p}_j - \mathbf{p}_i)) $$
其中 $\mathbf{z}_{ij}$ 是归一化平面观测，$\pi$ 是投影函数。这个残差对位姿和路标点的雅可比是标准推导，注意链式法则别漏项。
实际中一定要加鲁棒核函数，比如Huber核，来抑制外点的影响。外点可能来自误匹配或者动态物体，不加核函数的话一个外点就能把整个优化带偏。Huber核的阈值一般取1到2个像素对应的归一化坐标值。
注意：视觉残差的信息矩阵要除以观测噪声方差，这个方差通常假设为1到2个像素。如果实际跟踪精度差，可以适当放大噪声，让视觉约束权重降低。
4. 增量方程求解与边缘化先验的处理
4.1 用LM算法还是高斯牛顿
增量方程 $\mathbf{H}\Delta\mathbf{x} = \mathbf{b}$ 的求解，我推荐用Levenberg-Marquardt（LM）而不是纯高斯牛顿。原因很简单：VIO问题的初值不一定好，纯高斯牛顿容易发散，LM通过调节阻尼因子能在初值差的时候更稳。
LM的阻尼因子更新策略：如果这次迭代残差下降了，就减小阻尼，更接近高斯牛顿；如果残差上升了，就增大阻尼，更接近梯度下降。实际实现时初始阻尼取1e-4左右，下降因子0.1，上升因子10。
求解器方面，因为 $\mathbf{H}$ 是稀疏的，可以用Cholesky分解。Eigen的SimplicialLDLT对稀疏对称正定矩阵很合适。如果矩阵接近奇异，可以加一个小的对角正则项。
4.2 边缘化先验的构建细节
边缘化是滑动窗口里最容易出错的地方。具体操作步骤：
- 确定要边缘化的状态：通常是窗口里最老的那一帧IMU状态，以及只被这一帧观测到的路标点。
- 构建完整的 $\mathbf{H}$ 和 $\mathbf{b}$，包括所有约束。
- 把 $\mathbf{H}$ 按保留状态和边缘化状态分块。
- 对边缘化状态做Schur补，得到先验信息矩阵 $\mathbf{H}{\text{prior}}$ 和先验向量 $\mathbf{b}{\text{prior}}$。
- 把先验加到下一次优化的 $\mathbf{H}$ 和 $\mathbf{b}$ 里。
这里有个关键点：先验的雅可比是在上一次线性化点处计算的，下次优化时线性化点变了，先验需要做一阶近似修正。具体做法是用一阶泰勒展开把先验残差从旧线性化点搬到新线性化点。这个操作叫“先验的fej（First Estimate Jacobian）”处理，不做的话会导致估计不一致。
实操心得：边缘化时一定要检查 $\mathbf{H}_{\lambda\lambda}$ 是否可逆。如果某个路标点只被一帧观测到，它的信息矩阵可能是奇异的，需要加一个小的正则项或者直接丢弃这个路标点。
5. 常见问题排查与调试技巧实录
5.1 优化不收敛或者发散怎么办
这是最常见的问题，排查顺序如下：
| 问题现象 | 可能原因 | 排查方法 |
|---|---|---|
| 残差震荡不下降 | 雅可比符号错误 | 用数值雅可比对比解析雅可比 |
| 优化后位姿跳变 | 边缘化先验错误 | 关闭边缘化测试是否正常 |
| 逐渐漂移 | 零偏估计发散 | 检查零偏随机游走约束权重 |
| 直接发散 | 初值太差或外点太多 | 加鲁棒核，检查初始化 |
数值雅可比对比是我最常用的调试手段。写一个函数用有限差分算雅可比，和解析雅可比逐项对比，误差应该在1e-6以内。如果某项差很多，基本就是推导错了。
5.2 边缘化导致的填充问题
边缘化会让信息矩阵变稠密，尤其是路标点被边缘化后，原本不相关的IMU状态之间会出现新的约束。这个填充会随时间累积，最终让矩阵变得很稠密，求解变慢。
缓解方法有几个：一是控制窗口大小，别太大；二是边缘化时尽量选择观测少的帧；三是可以用一些稀疏化技巧，比如只保留先验的块对角部分。但稀疏化会损失信息，需要权衡。
我自己的做法是窗口取10帧，边缘化时把只被老帧观测到的路标点一起边缘化掉，这样填充可控。实测下来跑几分钟矩阵稀疏度还能接受。
5.3 时间同步与标定误差的影响
VIO对时间同步很敏感，IMU和相机的时间戳差1毫秒就可能引入明显误差。如果发现优化结果有系统性偏移，先检查时间同步。可以在状态里加一个时间偏移量在线估计，但会增加复杂度。
外参标定误差也会影响精度，尤其是相机和IMU之间的旋转。如果外参不准，视觉和惯性约束会互相矛盾，优化结果会折中，表现为精度下降但不发散。建议先用标定工具把外参标准，再跑VIO。
注意：在线估计外参虽然方便，但和外参、零偏、速度等状态耦合严重，容易收敛到局部最优。除非标定条件很差，否则建议离线标定好固定住。
6. 代码实现中的关键细节与性能优化
6.1 稀疏矩阵的构建与求解
Eigen的SparseMatrix构建时，用triplet列表是最快的：
std::vector<Eigen::Triplet<double>> triplets; triplets.reserve(estimated_nonzeros); // 填充triplets for (int i = 0; i < num_blocks; ++i) { // 添加块 } Eigen::SparseMatrix<double> H(num_params, num_params); H.setFromTriplets(triplets.begin(), triplets.end());求解用：
Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>> solver; solver.compute(H); Eigen::VectorXd dx = solver.solve(b);如果H接近奇异，可以加对角正则：
H.diagonal().array() += 1e-8;6.2 雅可比的解析推导与验证
每个残差块的雅可比都要手推。以视觉残差为例，对位姿的雅可比需要用到链式法则：
$$ \frac{\partial \mathbf{r}}{\partial \delta \xi} = \frac{\partial \mathbf{r}}{\partial \mathbf{p}'} \frac{\partial \mathbf{p}'}{\partial \delta \xi} $$
其中 $\mathbf{p}'$ 是相机坐标系下的点。这部分推导要仔细，符号很容易错。推完后用数值雅可比验证，确认无误再集成。
6.3 滑动窗口的状态管理
窗口管理包括：新帧进来时，把新状态加入窗口；老帧滑出时，触发边缘化。状态在内存里的排列要固定，方便索引。我一般用一个结构体数组存IMU状态，路标点用哈希表存，键是特征ID。
新帧加入时，要用IMU预积分把新帧和上一帧之间的约束建好。如果新帧和上一帧之间没有IMU数据，要插值或者等待。实际中IMU频率远高于相机，一般不会缺。
7. 实测效果与精度评估方法
7.1 用什么指标评估VIO精度
常用指标是ATE（绝对轨迹误差）和RPE（相对位姿误差）。ATE衡量全局一致性，RPE衡量局部精度。计算时先把估计轨迹和真值轨迹对齐（通常用Umeyama算法做相似变换对齐），然后算均方根误差。
如果没有真值，可以用闭环检测或者回环误差来间接评估。也可以把VIO估计的轨迹和纯视觉、纯惯性的结果对比，看融合是否带来提升。
7.2 不同场景下的表现差异
室内低速场景，VIO通常能跑到厘米级精度，因为特征丰富、运动平缓。室外高速场景，特征跟踪容易丢，IMU积分误差累积快，精度会下降到分米级甚至米级。剧烈运动比如快速旋转，陀螺仪量程可能饱和，零偏估计也会变差。
我实测下来，在走廊场景跑100米，ATE大概在0.1到0.3米之间，取决于纹理和光照。如果地面纹理少，视觉约束弱，精度会明显下降。这时候可以适当增大IMU权重，但也不能太大，否则视觉就白加了。
7.3 参数调优的经验
关键参数就几个：窗口大小、鲁棒核阈值、IMU噪声、视觉噪声、零偏随机游走噪声。调参顺序建议：先调噪声参数让残差量级合理，再调鲁棒核阈值抑制外点，最后调窗口大小平衡精度和速度。
IMU噪声参数可以从数据手册查，但实际值往往偏大，因为还有未建模的误差。我一般把手册值乘以2到5倍作为初值，然后根据优化结果微调。视觉噪声取1到2像素比较稳。
实操心得：调参时一次只改一个参数，改完跑同一段数据对比ATE。同时改多个参数的话根本不知道是哪个起了作用。记录每次实验的参数和结果，形成表格，方便回溯。
8. 从作业到工程的差距在哪里
作业通常只要求跑通一个仿真或者小数据集，但工程落地要考虑的东西多得多。首先是鲁棒性，作业数据干净，实际数据有各种异常：图像模糊、IMU饱和、时间戳跳变。这些都要有异常检测和处理机制。
其次是计算资源，作业在PC上跑，工程可能跑在嵌入式平台上，算力有限。这时候要优化矩阵求解，比如用定点数、降低窗口大小、减少迭代次数。还要考虑内存占用，稀疏矩阵的存储也要优化。
最后是长期稳定性，作业跑几十秒，工程要跑几小时。零偏的缓慢变化、外参的温漂、地图的累积误差，这些在长时间运行中都会暴露。需要设计在线标定和零偏重估计机制。
我个人体会是，把作业跑通只是入门，真正难的是让它在各种条件下都稳定工作。建议做完作业后，找一些有挑战性的公开数据集跑一跑，看看在快速运动、弱纹理、光照变化下的表现，这样能更快发现自己的实现有哪些不足。后续可以尝试加入回环检测和全局优化，把滑动窗口的漂移进一步消除，形成一个完整的SLAM系统。
