企业官网建设流程全解析

后端优化这四个字，在VIO（视觉惯性里程计）的学习路线里属于最硬核的一块骨头。前端视觉里程计我们能把位姿跑起来，但那只是提供了一批质量参差不齐的“初值”。真正决定系统精度上限和长期稳定性的，其实是最后的联合优化：把相机看到的东西和IMU量到的加速度、角速度放在同一个代价函数里，用非线性最小二乘同时调整所有状态量。这篇记录就是我在手写VIO后端优化时的完整学习笔记，包含作业里踩过的坑、代码结构的设计思路，以及让结果稳定跑出来的关键细节。适合已经跑通前端、正准备往图优化和因子图方向迈进的同学参考。

我把它分成几个层面来讲：先是后端优化在整个系统里的位置和建模方式，再聊因子图、信息矩阵这些绕不开的概念，接着落到代码层面讲残差、雅可比、H矩阵怎么填，最后是我在实际调试中遇到的高频问题和排查手册。

1. 后端优化在整个VIO系统里到底站在哪

1.1 前端和后端的分工

VIO系统通常拆成两部分：前端负责视觉特征跟踪、IMU预积分的测量值组织，以及给出每一帧的初始位姿估计；后端接收前端提供的观测约束，对所有状态量做全局或局部的重新调整。

前端的工作看起来已经很“够用”了——大多数前端都能输出平滑的轨迹，但问题在于它存在累积漂移。每一帧的位姿估计都是在前一帧基础上递推的，误差会一层层叠上去，时间一长轨迹就开始飘。后端的作用，就是利用重投影约束和IMU约束把“已经发生过”的状态量再次拉回来，让轨迹和地图满足全局一致的几何关系。

我个人的理解是：前端像是一个匆匆忙忙的助理，快速给出一份草稿；后端像是一个精算师，接手草稿之后仔细核对账本。助理做得再好，账本也得精算师把关。这就是后端优化的价值所在。

1.2 为什么不能“一次性全局优化”

有一个很自然的疑问：既然后端优化这么厉害，为什么不把从第一帧开始的所有帧和所有地图点都拿来做非线性最小二乘，一次解得干干净净？答案是计算复杂度。假设我们有N个关键帧，M个路标点，目标函数里的变量维度大约是 6N + 若干深度参数M。若N和M都涨到几千甚至上万，对应的信息矩阵H矩阵就会变得巨大，直接做稠密求解会把内存和算力拖垮。

在工程里，我们采用滑动窗口方案。窗口内只放最近N个关键帧，当新帧进入窗口时，把最老的帧“边缘化”掉。但“边缘化”不是把旧帧简单扔掉，而是把旧帧留下的约束信息转成先验项，叠加到新的代价函数里。这样既能控制计算量，又不会彻底丢失历史约束。

1.3 后端要解决的三个核心问题

后端优化的本质，是在最大化后验概率（MAP）的框架下求一组状态量。这个过程落到实现上，就是解决下面三个问题：

第一，定义状态向量。VIO里要估计的变量比纯视觉SLAM更多：不仅包括每帧的旋转、平移，还有速度、加速度计零偏、陀螺仪零偏，甚至相机与IMU之间的外参。第二，定义误差模型。视觉特征的重投影误差和IMU预积分残差，都是测量值和模型预测值之间的差值。第三，构造优化问题并求解。把误差的平方和写成一个代价函数，用高斯牛顿（Gauss-Newton）或列文伯格-马夸尔特（Levenberg-Marquardt）迭代求解。遇到误匹配时还要引入鲁棒核函数来降低外点的影响。

这三点听起来抽象，但它们串起来其实就是一段可以运行的代码。我后面会逐个展开。

2. 状态量与误差模型的数学骨架

2.1 状态向量的定义方式

在经典VIO后端中，待优化的变量分为几类：窗口内的关键帧位姿，包括世界系到IMU系的旋转和平移；IMU的速度、加速度计零偏、陀螺仪零偏；相机和IMU之间的外参；以及每个路标点的深度或逆深度参数。

我采用的是一种常见的配置：对每帧定义IMU状态为十二维向量，包括旋转四元数、平移向量、速度、加速度计零偏和陀螺仪零偏。四元数只是表达方式，优化时使用所谓“最小参数表示”的右乘扰动模型，这样能避免过参数化和奇异问题。对于路标点，我选择了逆深度而不是普通的三维坐标。原因很简单：逆深度参数化在特征点距离较远时数值稳定性更好，而且能自然地处理相机观测中的尺度不确定性。

注意：状态量的定义不是随便定的，它直接决定了你的雅可比矩阵怎么推导。定义变量时就要想清楚“优化时用什么参数化方式”，否则后边填雅可比时容易一团乱麻。

2.2 IMU预积分残差

IMU的测量频率通常远高于视觉帧率，因此相邻两关键帧之间会积累大量IMU测量数据。如果每次优化时都对所有IMU测量重新进行数值积分，迭代次数一多，计算量会非常可怕。预积分的思路是：把两帧之间的IMU相对运动量提前积好，作为“批量测量值”，优化时直接拿这个测量值构造残差即可。

预积分残差的形式比较固定，大致是三个分量：旋转残差、速度残差、位置残差。旋转残差是两帧间由估计状态推出的旋转增量与预积分旋转量之间的差值；速度残差和位置残差类似，只不过还涉及重力向量和零偏修正项。零偏如果发生了改变，预积分量需要用零偏的一阶雅可比来做修正，这一步是练习中的关键细节。

我踩过的一个坑是：一开始没有对预积分的噪声协方差做传递，导致信息矩阵里的权重给得不准，优化出来的轨迹虽然不爆，但收敛得很慢。正确的做法是在预积分过程中同步传递噪声协方差，这个协方差最终会作为IMU残差的权重矩阵。

2.3 视觉重投影残差

视觉误差的基础模型是相机投影：把路标点的世界坐标投影到某一关键帧的归一化相机平面上，和观测坐标做差。实际实现里通常写成更紧凑的形式，即所谓“重投影误差”。

对每个视觉观测，需要维护该观测属于哪个路标点、在哪两帧里被观测到、像素坐标是多少。优化中计算残差时，会用当前待估计的位姿和路标点深度计算出投影位置。投影位置和实际观测位置的距离就是重投影残差。

需要注意，多视几何里一个路标点可能出现在窗口内多帧中。为了让误差模型更清晰，我选择了其中一帧作为“参考帧”，用参考帧的观测和当前帧的观测一起构建残差，这就是视觉SfM里常见的“多视几何约束”，写代码时用一个循环遍历所有观测即可。

2.4 联合代价函数

把IMU残差和视觉残差放到一起，目标函数就是所有残差的马氏距离加权平方和。加上边缘化生成的先验项，就可以紧凑地写成：

代价函数 = 先验项 + IMU残差项 + 视觉重投影残差项。

每一项的权重来自于预积分的协方差矩阵和视觉观测的像素噪声协方差。从概率角度看，这个代价函数对应的是在假设高斯噪声条件下的最大后验估计。后端优化的过程就是令代价函数对状态量求导为零，然后通过迭代更新找到最优状态。

3. 图优化与因子图视角下的后端

3.1 贝叶斯网络怎样退化成因子图

拿到一堆状态量和观测之后，你会发现它们之间天然存在条件独立关系：某一帧的位姿不和遥远过去的帧直接相关，只和它相邻的帧以及它们共同观测的路标点相关。用贝叶斯网络图来表达这个依赖关系非常自然，但贝叶斯网络里有向边、有归一化约束，做求积不太方便，所以工程上更常用因子图。

因子图是一种二分图：一边是变量节点，代表状态量；一边是因子节点，代表一个约束或一个观测。IMU预积分产生一个“两帧之间的惯性因子”，视觉观测产生一个“特征-相机位姿因子”，边缘化则会生成一个“先验因子”。每条因子边本质上是一个残差项，多个因子加起来就是总的代价函数。

我发现，理解因子图对写代码最大的帮助是：它告诉你H矩阵的非零块该填在哪。因子图里每一条边，都会在与其相连的两个变量块对应的H矩阵位置产生非零块。所以写代码之前先在草稿纸上把因子图画出来，H矩阵的结构就一目了然了。

3.2 信息矩阵的稀疏结构为何珍贵

在下面的章节我会详细讲H矩阵怎么组装，这里先强调一个概念：H矩阵的稀疏性。每一个变量块只和少数几个其他变量块有约束关系，因此H矩阵绝大多数位置都是零。用Eigen或者任何稀疏求解器时，如果能预先把稀疏模式定下来，内存占用和求解速度会有数量级的提升。

因子图边少、连接稀疏，H矩阵的稀疏度就高。反过来，如果错误地把所有观测都做成“全连接”，H矩阵就会变稠，求解直接卡死。所以代码里构建因子时要严格按帧间约束关系建立残差，千万不要引入多余连接。

3.3 滑动窗口如何保证计算量可控

一个滑动窗口内通常保留若干关键帧和若干路标点，窗口滑动时，轨迹末端的约束被边缘化。引入边缘化项之后，窗口内的信息矩阵不再只是“当前窗口观测”生成的信息，还叠加了被边缘化状态的先验信息。这样既保持了窗口大小固定，又不会让早期观测的信息彻底丢失，是工程上维持效率和精度平衡的关键设计。

关于边缘化的技术细节，我放到后面专门讲，它是整个后端优化的难点和最容易出错的地方。

4. 手写后端的核心模块与代码落地

4.1 数据结构和代码骨架

我这次实现的代码结构并不复杂，但对数据流向要求很明确。主要模块包括几个部分：

• 关键帧类：保存帧ID、位姿、速度、零偏，以及该帧对应的观测列表。
• 路标点类：保存路标ID、逆深度参数、被哪些帧观测到、对应像素坐标。
• 预积分器类：负责两帧之间IMU测量数据的预积分，提供残差和雅可比。
• 优化器类：负责组装H矩阵和b向量，迭代求解，并更新所有状态。

为了做单元测试，我构建了一个非常小的模拟场景：两张图像，若干个人工设定的特征点，外加一段模拟IMU数据。这样能让代码在几百毫秒内跑完，任何一步崩溃都能快速定位。

4.2 雅可比矩阵的推导与程序实现

雅可比矩阵是后端代码里最容易被写错的部分。每次迭代求解都需要算出残差对每个相关状态量的偏导数矩阵，然后用雅可比乘上雅可比的转置去组装信息矩阵。

视觉重投影残差对相机位姿的雅可比，可以利用链式法则拆成“误差对投影点求导”和“投影点对位姿求导”两部分。越到后面的映射关系（三维坐标到归一化平面坐标）需要谨慎地处理。

IMU残差的雅可比更复杂，因为状态量包括位置、速度、旋转和零偏。旋转量对应的雅可比使用四元数扰动模型，推导时写成右乘扰动形式会更简洁。我在推导时参考了很多资料，但真正弄通之后发现规律性很强：每个残差只对它真正依赖的变量求偏导，不要想着“求所有变量的偏导”，那样既低效又容易出错。

代码里我建议把雅可比求值函数和残差求值函数分开，且都放到统一接口里。迭代求解时频繁调用残差和雅可比，如果内部有大量重复计算，性能会很难看。预积分量如果在残差里重复计算，就会白白浪费CPU。

4.3 组装H矩阵和b向量

求解非线性最小二乘的关键是组装线性系统 H * dx = b，其中H是所有残差项的信息矩阵累加，b是梯度向量。

我的做法是，先遍历所有残差，统计H矩阵中哪些块位置非零，构建一个“稀疏模板”，然后用稀疏矩阵的预留接口一次性分配内存。再遍历一遍所有残差，把每个残差的雅可比矩阵按块填入H和b。

这里有个很大的教训：H矩阵是对称的，但很多人在填充时只填了下三角或者只填了上三角，导致求解器报“矩阵不对称”。正确做法是每次填入雅可比时，同时更新 H += J^T * W * J，这个结果天然对称，但浮点运算的舍入误差可能导致上下三角不完全一致，必要时可以在迭代前做一次“强制对称”，把上三角复制到下三角。

4.4 从Hessian看系统可观测性

后端优化的可观测性很有意思。VIO系统在没有绝对位置观测的情况下，全局位置、姿态、尺度存在不可观的“零空间”。这意味着即使优化收敛，H矩阵在某些方向仍然是奇异的。

经典的H矩阵对旋转误差的惩罚远强于对位置误差的惩罚，因此如果自己的实现没有任何全局约束，轨迹跑一段时间后会明显产生漂移。理解这一点对排查问题很有帮助：如果H矩阵的最小奇异值特别小或者直接奇异，不一定是代码写错了，可能只是当前的数据缺乏对那个方向约束。

遇到这种情况，我会通过检查H矩阵的零空间来辅助判断。如果零空间对应的向量确实指向全局漂移方向，说明系统尺度不定，属于正常现象；如果零空间指向某个局部位姿分量，那才是代码问题，比如某个观测的雅可比没有正确连接。

4.5 LM迭代与更新策略

高斯牛顿法可以有效处理病态问题，但实际中IMU和视觉误差的尺度差异很大，高斯牛顿容易发散。我采用LM（列文伯格-马夸尔特）方法：在H矩阵对角线上加一个阻尼系数λ，它会在梯度下降和高斯牛顿之间自适应切换。λ大时更接近香草梯度下降，稳定性好但收敛慢；λ小时更接近高斯牛顿，收敛快但容易震荡。

每轮迭代求解线性系统得到增量 dx，然后对新状态计算新的代价函数值。如果代价下降，就接受更新并把λ减小；如果代价上升，就拒绝更新并增大λ。这个策略在大多数后端实现里都是标配。

还有一个小细节：求出增量之后，对四元数状态的更新必须用“四元数乘法”，而不是加法。平移、速度、零偏直接加增量即可，旋转则要把增量转换成四元数后做归一化。如果忽略了归一化，更新几轮之后四元数就不是单位长度，数值会漂移出合理的几何意义。

5. 鲁棒性处理与外点抑制

5.1 普通最小二乘为什么扛不住误匹配

纯最小二乘假设每个残差都是高斯分布的噪声，但特征匹配里难免出现错误匹配，也就是“外点”。外点的残差通常很大，平方之后会对代价函数产生巨大影响，可能把优化结果整个拉歪，甚至导致迭代不收敛。

我最初调试时总是怀疑自己的雅可比写错了，后来静下心分析失败案例，发现罪魁祸首往往是一批错误匹配。普通最小二乘下，一个错误的像素坐标可以直接把整个窗口的位姿拉几十厘米甚至几米。

5.2 鲁棒核函数的选择与参数

解决外点问题的常用手段是M估计，也就是在残差平方前面加一个鲁棒权重。最常用的是Huber核：

• 当残差小于阈值δ时，和普通平方项一样；
• 当残差大于阈值δ时，惩罚从二次变成线性，让大的残差不至于过度影响总代价。

实现时不需要把整个目标函数改成新的形式，只需要在组装H和b时给每个残差乘上一个由鲁棒核计算出的权重标量。这样改动很小，效果却非常明显。

阈值δ的选取要看实际情况，像素重投影误差通常在1到2像素以内算正常，所以我把δ设成1.5左右。如果手头数据集的外点比例特别大，可以适当调小δ，让更小的残差就开始降权。但δ太小时，内点的约束也会被削弱，轨迹精度反而下降。

实际操作中我会做两步：先用鲁棒核跑一轮优化，统计所有残差的中位数和绝对中位差（MAD），再用这个统计结果自适应地估计噪声尺度，把超过三倍MAD的观测直接标为外点并剔除。这个方法比单纯调δ更稳健。

6. 边缘化与先验信息构建

6.1 为什么要边缘化而不是直接删帧

滑动窗口在移除最老关键帧时，如果直接把它的所有观测都删除，那么它曾经约束过的路标点、它在窗口内其他帧上的共同观测信息都会丢失，造成信息损失。边缘化（Marginalization）的思想是：先用待删除状态把所有相关残差的信息矩阵做一次条件化处理，然后把这些信息“压缩”成一个只和剩余状态相关的先验项。

这个操作在概率意义上相当于：把那几个状态变量从联合分布中积分掉，结果是剩余变量上的高斯分布参数发生了更新。落到线性代数上，就是对H矩阵做一次Schur Complement（舒尔补变换）。

6.2 Schur Complement的操作流程

假设我们要把变量x_m边缘化，剩余变量为x_r，线性系统可以分块为：

| H_mm H_mr | |dx_m| |b_m| | H_rm H_rr | |dx_r| = |b_r|

边缘化后的信息矩阵和b值更新为：

H_rr_new = H_rr - H_rm * H_mm^{-1} * H_mr

b_r_new = b_r - H_rm * H_mm^{-1} * b_m

本质上就是对左上块的Schur Complement。这个过程把“被边缘化变量”的影响折换为剩余变量的额外信息。代码实现时关键是矩阵分块操作要严谨，尤其注意所有残差在叠加进H时，必须用边缘化发生时刻的线性化点来计算雅可比。

我在验证这段代码时用的最简单方法：构造一个两帧系统，一帧要被移出窗口，卡尔曼滤波式的解析结果和Schur Complement数值结果对比。如果数值和解析结果偏差在1e-6以内，说明分块和求逆没有写错。

6.3 FEJ（First Estimate Jacobians）问题

边缘化最隐蔽的坑是FEJ问题。边缘化发生时，雅可比在旧的线性化点处计算，生成先验项。下一次优化迭代中，其他残差会在最新的状态点重新线性化。如果两个线性化点不一致，先验项和其他约束就在不同的坐标系里做近似，可能引入人为的“伪观测”，让系统产生错误偏好。

一个可行的规避方式：对已经被边缘化过的状态所对应的先验项，在后续计算雅可比时，只使用第一次边缘化时的状态取值，而不是不断更新后的新值。这就是First Estimate Jacobians的由来。

我不是每个项目都严格做FEJ，但配合边缘化时一定会有意识地检查：删除帧的雅可比计算状态是否被更新，边缘化后新残差的共享变量有没有被重复线性化。如果不做FEJ，系统在退化场景下容易突然翻转，轨迹出现非常诡异的跳跃。

6.4 先验项在下一轮窗口里怎么用

边缘化生成的先验项，和普通的残差项一样，同样需要组装进H矩阵和b向量。它作为一些剩余状态变量的额外二次项存在，雅可比矩阵固定为边缘化时保存的雅可比，不随迭代更新。每次组装线性系统时，都要把先验项的贡献先加进去，然后再加新的IMU和视觉残差。

这里有一个经验：先验项的规模如果太大（信息矩阵数值很大），会压制新观测的更新能力，导致系统对真实运动响应迟钝；如果太小，旧信息就等于被丢掉了，滑窗变成了局部BA。实践中可以通过调试信息矩阵的数值量级来平衡。

我在做作业时发现，边缘化后轨迹在窗内帧数变少时容易出现抖动。后来发现原因是先验项没有乘以正确的鲁棒核，导致被边缘化帧里的某些内点误匹配被当成强约束拖住了新帧。所以对先验项同样要加鲁棒化处理，或者至少在做边缘化之前就要完成外点剔除，别把脏信息转到先验里。

7. 作业运行中必踩的坑与排查手册

7.1 前端数据接不上后端：时间戳与坐标系对齐

手写后端最容易被忽略的是数据对齐。前端关键帧的时间戳、IMU测量的时间戳、像素观测的时间戳如果没有统一基准，后端优化的数据流就没法对齐，预积分量用错时间窗口，结果几乎必然飘掉。

我的建议是先画一张数据流向图：IMU原始测量进入预积分器，按关键帧时间戳生成预积分量；视觉特征观测从前端输出，按帧ID和路标点ID组织成观测结构。在进入优化之前，先做一轮数据清洗，把时间戳单调性、帧间观测数量异常的数据打印出来检查。

7.2 H矩阵奇异或者迭代发散

H矩阵奇异最常见的原因是某个待优化变量完全没有被任何观测约束。比如某帧的路标点观测全部缺失，或者IMU残差没有关联到速度变量。这种情况一般通过打印H矩阵的行列式或者最小特征值就能发现。

迭代发散的原因则相对复杂：可能是雅可比符号反了，可能是鲁棒核权重给错，也可能是增量更新时四元数忘了归一化。排查时我用一个技巧：把迭代次数设置为1，然后人工检查第一次更新之后的残差变化方向。如果误差增大，十有八九是雅可比符号或更新公式的方向错了。

7.3 优化结果验证：轨迹与残差统计

最终验证时不能只盯着轨迹图看。我习惯把两类数据打印出来：每一轮迭代的代价函数值变化曲线，以及优化后所有IMU残差和视觉残差的均值、RMS。如果代价函数单调下降但残差RMS仍然很大，说明某类约束和另一类约束之间权重失衡；如果RMS很小但轨迹和真实值差很远，那就是可观测性或边缘化先验有问题。

对于数据集提供真值的情况，我会额外算一下轨迹的ATE（绝对轨迹误差），一般误差在厘米到分米量级。

7.4 我的调试顺序建议

第一次跑固定流程的话，我从简到难逐步增加模块：先只优化视觉重投影残差，跑通之后再加入IMU预积分残差，再加入边缘化先验。每增加一个模块就做一次回归验证，这样一旦出现问题，搜索范围就被限制在新增模块内部。

这个习惯救了我很多次。有些同学一上来就加满所有约束，一旦崩了根本不知道是视觉的锅还是IMU的锅，排查成本极高。模块化验证思路虽然看起来“慢”，但实际上是最快的省时间路径。

这次从零手写后端优化，我最大的感受是：非线性最小二乘这块，看十遍推导不如自己手推一遍雅可比；理解H矩阵的稀疏结构，靠文字描述不如动手画一遍因子图。最初我总试图去找现成的代码框架套用，但真正把手动求导、稀疏填充、边缘化一步步写完，整个VIO系统的全貌才真正落地了。

最后再分享一个调试小技巧：每次填充完H和b之后，不妨先做一次行列式检查和线性求解测试，再进入真实迭代。如果线性系统本身就奇异的，后面结果不可能对。用这种“先验证在干嘛、再验证对不对”的思路，跑通后端优化的效率会高很多。