大概是两年前我跟着一份手写VIO的资料把整个系统从头到尾撸了一遍从最开始的系统框架搭建到视觉前端的特征提取与光流追踪再到IMU预积分的推导和代码落位一路走到今天要写的这个主题——后端分析。这是“手写VIO总结”系列的第五篇也是我个人认为整个系统里信息密度最高、最容易劝退的一篇。很多人看VIO的代码看前端还能跟上一到后端就被各种名词砸晕H矩阵、LM、Schur补、边缘化、先验、FEJ每个词单独拿出来都有概念放在一起就完全不知道在干什么。我这篇尽量用聊天的口吻把后端这条链路讲透从“后端到底在优化什么”讲起到误差模型怎么写、求解器怎么搭、滑动窗口和边缘化是怎么回事最后把我在实际调试中踩过的坑和排查思路一并列出来。如果你正在啃VIO后端或者想从零手写一个后端优化模块这篇应该能帮你省不少时间。1. 后端在VIO系统里到底扮演什么角色1.1 前端和后端怎么分工后端吃饱了谁喂的饭VIO系统可以粗暴地分成两块前端负责从图像和IMU观测里拿到中间量后端负责把这些中间量统一变成最优状态估计。前端做的事包括但不限于提取特征点、做帧间匹配或光流追踪、判断关键帧、生成IMU预积分量、估计初始深度和尺度。这些量本身都是带噪声的尤其是视觉匹配经常混进去几个错误匹配IMU积分也会因为零偏和噪声慢慢漂移。如果整个系统只有前端那就相当于一个纯递推的惯性视觉里程计误差只增不减跑几十秒可能就开始飘了。后端解决的正是这个问题。它把所有测量结果丢进同一个优化框架里用“整体最优”的思想让每一个状态同时照顾到所有相关的约束。说得再直白一点前端给的是一个一个“局部判断”后端做的是把这些局部判断全部放到一个法庭上互相质证最后得出一个全局自洽的结论。比如某一帧的特征匹配给错了如果这个错误参与了后端优化它会被相邻帧的多个约束压下去不至于让整个轨迹发生剧烈跳变。手写VIO项目的后端属于比较经典的基于非线性最小二乘的图优化也就是常说的Bundle AdjustmentBA在视觉惯性融合上的扩展。与之相对的是基于滤波器的后端比如MSCKF那种以EKF为核心的做法。二选一的时候我比较推荐先理解非线性优化版本因为它的数学框架更清晰方便后续往因子图、增量优化这些方向扩展。1.2 后端状态向量里到底装了哪些东西后端优化的对象是“状态”一般用一个包含多帧状态的大向量表示[ \mathcal{X} { \mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_n, \boldsymbol{\lambda}_0, \boldsymbol{\lambda}_1, \dots, \boldsymbol{\lambda}_m } ]其中每一帧的状态 (\mathbf{x}_k) 通常包含旋转矩阵 (\mathbf{R}_k) 或对应四元数位置 (\mathbf{p}_k)速度 (\mathbf{v}_k)陀螺仪零偏 (\mathbf{b}_{g,k})加速度计零偏 (\mathbf{b}_{a,k})路标点 (\boldsymbol{\lambda}) 一般不用三维坐标直接表示而是用逆深度参数化原因是视觉SLAM里单目观测天然存在尺度不确定性逆深度在优化中收敛性更好也和初始化过程贴合得更紧密。关于逆深度为什么好网上有不少讨论我的理解是它把“特征点离相机较远”这个物理常识编码进了参数空间而且它的线性化区域比普通三维坐标更适合高斯牛顿迭代。后端的目标就是估计这个 (\mathcal{X})使得所有测量残差平方加权重之后最小化[ \min_{\mathcal{X}} \left| \mathbf{r}{vis} \right|^2{\boldsymbol{\Omega}_{vis}}\left| \mathbf{r}{imu} \right|^2{\boldsymbol{\Omega}_{imu}}\left| \mathbf{r}{prior} \right|^2{\boldsymbol{\Omega}_{prior}} ]这个公式是整个后端的“宪法”后面所有内容都在围绕它转。1.3 为什么必须用非线性最小二乘而不是直接解线性方程如果所有测量都是线性模型那后端就退化成一个线性最小二乘问题闭式解一把梭就完事了。可惜现实不是这样视觉观测的投影模型是针孔模型加畸变IMU预积分又涉及旋转的指数映射整个系统是一个强非线性系统。所以我们需要在某个工作点附近做一阶泰勒展开通过反复迭代逐步逼近最优解。这就像你在山里面找最低点你手里只有脚下这块小坡的坡度信息所以每次只能沿着当前最陡的下坡方向走一小步走完了再重新测坡度再走。高斯牛顿和LM都是这个思路只是步子大小和方向修正的办法略有不同。后端无非是把这个过程用矩阵运算规范地实现出来。2. 三类误差项的数学构建照着抄就行2.1 视觉重投影误差最直观也最容易写错视觉误差的基本思想是一个路标点在第i帧被观测到并三角化它应该也能在第j帧被观测到。如果把它投影到第j帧得到的像素坐标和实际特征点位置对不上就有了残差。用逆深度参数化可以写成[ \mathbf{e}{vis} \mathbf{z}{ij} - \pi\left( \mathbf{R}{cb}\left[ \mathbf{R}{wj}^T \left( \mathbf{R}{wi} \left( \frac{1}{\lambda_i} \mathbf{n}i \right) \mathbf{p}{wi} - \mathbf{p}{wj} \right) \right] \mathbf{p}_{cb} \right) ]这里的 (\mathbf{n}i) 是路标点在第i帧相机坐标系下的单位方向向量(\lambda_i) 是对应逆深度(\pi(\cdot)) 是相机投影函数(\mathbf{R}{cb}) 和 (\mathbf{p}_{cb}) 是从机体坐标系IMU系到相机坐标系的外参。写代码的时候最需要注意的是雅可比矩阵的求法。视觉误差对位姿的导数通常采用左扰动或右扰动模型对逆深度的导数则相对简单就是直接对投影函数里那个“1/λ”求链式偏导。很多人手写后端卡在这一步一个常见的原因是把扰动模型搞混导致数值雅可比和解析雅可比对不上。我的建议是无论采用哪种扰动约定整套代码里必须保持一致最好像我在调试章节里写的那样先做数值梯度校验再做整个优化。2.2 IMU预积分误差核心是bias处理关键是一阶修正IMU预积分误差的推导在该系列的前一篇已经详细展开过我这里只把后端需要用到的那部分拎出来。两个关键帧 i 和 j 之间预积分量可以写成[ \Delta \mathbf{R}{ij} \prod{ki}^{j-1} \exp\left( \left( \boldsymbol{\omega}k - \mathbf{b}{g} \right) \Delta t \right) ][ \Delta \mathbf{v}{ij} \sum{ki}^{j-1} \Delta \mathbf{R}_{ik} \left( \mathbf{a}k - \mathbf{b}{a} \right) \Delta t ][ \Delta \mathbf{p}{ij} \sum{ki}^{j-1} \left( \Delta \mathbf{v}{ik} \Delta t \frac{1}{2} \Delta \mathbf{R}{ik} \left(\mathbf{a}k - \mathbf{b}{a}\right) \Delta t^2 \right) ]然后定义三个残差[ \mathbf{e}R \mathrm{Log}\left( \Delta \mathbf{R}{ij}^T \mathbf{R}_i^T \mathbf{R}_j \right) ][ \mathbf{e}_v \mathbf{R}_i^T \left( \mathbf{v}_j - \mathbf{v}i - \mathbf{g} \Delta t \right) - \Delta \mathbf{v}{ij} ][ \mathbf{e}_p \mathbf{R}_i^T \left( \mathbf{p}_j - \mathbf{p}_i - \mathbf{v}i \Delta t - \frac{1}{2} \mathbf{g} \Delta t^2 \right) - \Delta \mathbf{p}{ij} ]后端迭代过程中陀螺零偏和加计零偏会被不断更新。这里有一个工程关键点预积分量本身是在旧的零偏假设下算出来的所以每次零偏变化后严格来说应该重新做一次预积分。但重新积分比较耗时所以在零偏变化不大时工程上普遍采用一阶展开来修正[ \Delta \mathbf{p}{ij} \approx \overline{\Delta \mathbf{p}}{ij}\frac{\partial \Delta \mathbf{p}{ij}}{\partial \mathbf{b}{g}} \delta \mathbf{b}_{g}\frac{\partial \Delta \mathbf{p}{ij}}{\partial \mathbf{b}{a}} \delta \mathbf{b}_{a} ]对应到代码里就是维护几个预积分量对bias的雅可比矩阵。当bias变化幅度超过某个阈值比如0.1量级时我一般选择干脆重新做一次完整预积分省得到时候查bug查到怀疑人生。IMU误差的信息矩阵来自预积分噪声的协方差递推。每次计算残差时这个信息矩阵必须跟着一起组装进H矩阵。如果信息矩阵给错了最常见的症状就是IMU约束和视觉约束互相打架轨迹看起来平滑但跟真实尺度对不上。2.3 先验误差边缘化给后端留下的“记忆”先验误差不是一开始就有的它是在滑动窗口边缘化的过程中逐渐积累起来的。可以把先验理解成一个“浓缩的历史”——当系统把最老的帧移除优化窗口时并没有把那帧的信息直接扔掉而是通过数学操作把它的信息全部转移到与它有关联的剩余状态上。后端里的先验残差通常写成[ \mathbf{e}{prior} \mathbf{x}{remaining} - \mathbf{x}_{prior} ]对应信息矩阵 (\boldsymbol{\Lambda}_{prior})。这个式子表示“剩余状态应该处于先验均值附近”。它的规模和剩余状态的维度一致而且会随边缘化越来越稠密。关于它怎么生成我会在第4章详细推导。3. 求解器从一堆残差到状态增量3.1 组装H矩阵和信息矩阵本质是在累计“证据”求解非线性最小二乘的核心是把目标函数线性化。假设我们有残差向量 (\mathbf{r}(\mathcal{X}))在当前状态附近做泰勒展开并把目标函数对增量求导置零可以得到增量方程[ \mathbf{H} \Delta \mathbf{x} -\mathbf{b} ]其中[ \mathbf{H} \mathbf{J}^T \mathbf{W} \mathbf{J} ][ \mathbf{b} \mathbf{J}^T \mathbf{W} \mathbf{r} ]这里的 (\mathbf{J}) 是所有残差对状态增量的雅可比矩阵(\mathbf{W}) 是由信息矩阵构成的块对角权重矩阵。(\mathbf{H}) 有一个非常重要的名字叫信息矩阵某种意义上它就是系统状态的“置信度”总和。一块区域如果被多个可靠观测覆盖对应块的信息矩阵数值就会比较大优化时这个区域的状态就不容易被其他错误约束带偏。组装H矩阵的过程很像在一个表格里填数字每个残差项只涉及少数状态变量所以只会在固定位置填入自己的贡献。这也是整个求解过程中最耗内存和精力的一部分但逻辑上非常简单只要维护好状态索引表不要重复填充或漏填就行。3.2 LM策略和核函数怎么防止被野值带偏增量方程直接解很容易出问题因为视觉特征匹配里面总有那么几个outlier还有一些退化场景会让 (\mathbf{H}) 接近奇异。最常见的处理办法是使用LM阻尼[ (\mathbf{H} \lambda \mathbf{D}) \Delta \mathbf{x} -\mathbf{b} ]这里的 (\mathbf{D}) 一般取 (\mathrm{diag}(\mathbf{H})) 而不是单位阵这样阻尼项能对不同尺度的变量产生不同影响。(\lambda) 的调节策略也很经典如果本次迭代残差下降就减小 (\lambda)让算法更接近高斯牛顿加快收敛如果残差上升或没有明显变化就增大 (\lambda)让算法更接近最速下降保证稳定。除了LM阻尼视觉误差还要加鲁棒核函数。我通常用Huber核对超过阈值的残差做降权处理。这相当于在组装H矩阵前先根据残差大小给每个测量打个折扣本质是防止个别错误匹配主导整个优化。核函数阈值一般按像素误差来设1到2个像素比较合适。IMU误差理论上也可能受bias突变影响但我一般不加核函数因为IMU传感器相对可信加了反而可能掩盖真实问题。3.3 Schur补利用稀疏性让求解速度上一个台阶没有结构优化的后端是跑不起来的。H矩阵虽然很大但它的稀疏结构非常明显视觉路标点通常只被少数几帧观测到所以路标点相关的信息矩阵是块对角或接近块对角的位姿之间因为有IMU约束和视觉共视关系则形成相对稠密的结构。利用这个结构可以把H矩阵分块[ \begin{bmatrix} \mathbf{B} \mathbf{E} \ \mathbf{E}^T \mathbf{C} \end{bmatrix} \begin{bmatrix} \Delta \mathbf{x}{pose} \ \Delta \mathbf{x}{lm} \end{bmatrix}\begin{bmatrix} \mathbf{b}{pose} \ \mathbf{b}{lm} \end{bmatrix} ]其中 (\mathbf{C}) 对应路标点的块(\mathbf{B}) 对应位姿状态的块。由于 (\mathbf{C}) 是块对角的它的逆求解起来非常快所以我们可以先用Schur消元把路标点消掉得到只关于位姿的增量方程[ \left( \mathbf{B} - \mathbf{E} \mathbf{C}^{-1} \mathbf{E}^T \right) \Delta \mathbf{x}{pose} \mathbf{b}{pose} - \mathbf{E} \mathbf{C}^{-1} \mathbf{b}_{lm} ]解出位姿增量后再回代求解路标点增量。实际实现时这一步是后端性能的分水岭。如果不懂Schur补直接对整个H矩阵做稠密分解窗口大一点就跑不动了。如果做了Schur补滑动窗口内位姿数量一般只有几十个稠密分解也不是问题路标点部分再按块回代整个求解过程可以在毫秒级完成。4. 滑动窗口与边缘化后端设计的分水岭4.1 为什么窗口必须存在后端不能一直吃关键帧如果你天真地认为后端优化只在乎精度把所有关键帧都无条件加入那系统很快会被算力拖垮。设每个关键帧状态维度是15旋转、位置、速度、2个零偏几千个关键帧和几十万个路标点哪怕H矩阵不是完全稠密内存和求解时间都难以接受。更重要的是VIO本身的可观性是有上限的。在没有GPS、没有磁力计、没有回环的情况下系统的可观自由度只有姿态、位置、速度、零偏和尺度中的一部分剩下几个方向存在固有的漂移。保留越来越长的历史窗口并不能把这些不可观方向“补全”只会让计算成本无限增长。因此工程上普遍采用滑动窗口让优化永远集中在最近一段时间的关键帧上。这个设计既保证了局部精度又控制了计算量。窗口大小怎么定并不是越大越好。很多手写VIO项目里窗口取10到15帧关键帧这基本是精度和实时性的一个平衡点。我自己调试时试过把窗口拉大到30帧精度提升微乎其微但单帧耗时几乎翻倍所以在算力受限的平台上一定要控制窗口长度。4.2 边缘化的数学本质就是把变量“积掉”滑动窗口意味着最老的关键帧迟早要离开优化窗口。如果直接把它从问题中删除那么它关联的那些历史观测信息就被白白丢弃了新的窗口可能因为信息不足而产生漂移。正确做法是边缘化marginalization。联合概率的角度看这个问题最清楚。假设要移除的变量是 (\mathbf{x}_a)保留的变量是 (\mathbf{x}_b)它们在当前信息矩阵下的联合分布可以表示为[ p(\mathbf{x}_a, \mathbf{x}_b) \propto \exp\left( -\frac{1}{2} \begin{bmatrix} \mathbf{x}a - \boldsymbol{\mu}a \ \mathbf{x}b - \boldsymbol{\mu}b \end{bmatrix}^T \begin{bmatrix} \boldsymbol{\Lambda}{aa} \boldsymbol{\Lambda}{ab} \ \boldsymbol{\Lambda}{ba} \boldsymbol{\Lambda}{bb} \end{bmatrix} \begin{bmatrix} \mathbf{x}_a - \boldsymbol{\mu}_a \ \mathbf{x}_b - \boldsymbol{\mu}_b \end{bmatrix} \right) ]对 (\mathbf{x}_a) 做高斯积分后(\mathbf{x}_b) 仍然服从高斯分布但它的信息矩阵和mean会变成[ \boldsymbol{\Lambda}{prior} \boldsymbol{\Lambda}{bb} - \boldsymbol{\Lambda}{ba} \boldsymbol{\Lambda}{aa}^{-1} \boldsymbol{\Lambda}_{ab} ][ \mathbf{b}{prior} \mathbf{b}b - \boldsymbol{\Lambda}{ba} \boldsymbol{\Lambda}{aa}^{-1} \mathbf{b}_a ]这个过程和上一章Schur补在数学上完全一样你可以理解为“在求解时把路标点消掉”和“把老状态从窗口里移除”本来就是同一件事。只是求解时的Schur补是临时的、每轮迭代重新算而边缘化得到的先验则是长期持有的会一直参与后续迭代直到它涉及的变量全部离开窗口。4.3 FEJ问题边缘化最大的坑没有之一边缘化看起来很数学优雅但实操中坑很多最大的是FEJFirst Estimated Jacobian一阶估计雅可比问题。简单说边缘化发生时变量处于某个特定的线性化点。我们在构建先验时用的雅可比矩阵都是在那个时刻计算的。之后后端继续迭代保留变量的状态不断被更新这时候再算其他残差项的雅可比时线性化点已经变了。如果先验项内部还保留着老线性化点下的信息而其他项用的是新线性化点下的信息整个优化就会变得不自洽严重时甚至导致先验信息被“重复计费”或者让优化收敛到一个明显偏离真实解的局部极值。FEJ的处理办法很直接对先验中涉及的变量后续迭代计算雅可比时统一固定在边缘化发生那一刻的线性化点不让它跟着迭代更新。这样虽然雅可比不是最新但保证了整个问题的一致性。我在手写版本里把先验块的雅可比矩阵写成常量在迭代过程中不重新计算只在每次迭代时更新残差值。代码看起来有点粗糙但运行起来非常稳。如果想精细化也可以采用一些开源实现里更聪明的策略但我个人建议新手先用最土的方式做把系统跑通再说。另外还要注意先验块的变量参与新的边缘化时不能简单地把先验块当作普通残差再边缘化一次而要单独处理否则依然会产生信息重复累加的问题。这个坑会影响整个系统的长期稳定性比较隐蔽。5. 实操心法与排错速查这些东西文档里不写5.1 我的后端调试顺序先把纯视觉跑通我强烈建议不要一上来就端到端跑VIO后端否则出了问题你根本没法定位到底是视觉误差的雅可比错了还是IMU预积分的信息矩阵错了还是边缘化的时候FEJ没做对。我的调试顺序分成四步走第一步做纯视觉BA。只用几帧图像和它们的共视路标点不做IMU不做边缘化跑一个普通的最小二乘。如果这一步能收敛重投影残差均值降到亚像素级别就说明投影公式和雅可比基本没问题。这一步也可以用仿真数据来验证用N组随机路标点投影到N帧加一点噪声再优化回来看是否能恢复到真值。第二步加入IMU预积分残差。用仿真或公开数据集里的IMU数据先只优化相邻帧之间的相对位姿和速度看预积分残差是否在迭代后趋近于零。重点检查bias一阶修正雅可比否则很容易出现迭代发散的诡异现象。第三步加滑动窗口和边缘化。这时只边缘化最老关键帧且不做FEJ也能跑只是精度可能差点。先把边缘化本身的舒尔补逻辑验证对了再回头处理FEJ。第四步再加核函数和LM阻尼。把视觉野值通过核函数压住让系统在真实数据上也能稳定收敛。5.2 常见错误与排查清单我踩过的坑都在这我整理了一张表是后端调试时最常碰见的几类现象和排查方向对你排查自己代码时会有帮助现象可能原因排查方法优化一步就发散残差爆增雅可比方向反了或信息矩阵给得过大用数值雅可比和解析雅可比逐项对比残差下降但轨迹仍然漂移信息矩阵权重不对或某个可观方向缺少约束只保留视觉误差跑一遍看漂移是否消失加入IMU后系统抖动bias初值不准先用固定bias跑确认无误后再在线估计边缘化后残差突然跳变FEJ没做或先验与当前残差重叠把先验停用看问题是否消失求解出来的H矩阵条件数过大观测不足尺度接近退化打印条件数检查是否有特征点分布过集中后段运行时间逐渐变长窗口长度没控制住或先验块参与重新边缘化检查关键帧总数和历史先验是否被反复使用这里特别说一下信息矩阵权重。很多手写代码里视觉信息矩阵就是单位阵乘以某个固定系数IMU信息矩阵来自预积分协方差两者之间的相对尺度如果没调好后端会长期处于“视觉不信、IMU太信”或“IMU不信、视觉太信”的失衡状态。调权重最土的办法是跑一段已知轨迹看每一步的重投影残差和IMU残差各自的均值把它们调到同一个数量级再根据实际表现微调。5.3 数值校验小技巧解析雅可比过不过关几行代码见分晓手写后端最让我头大的就是雅可比。后来学乖了每写一个残差项先做一次数值梯度校验再继续这个方法帮我少走了很多弯路。数值校验的思路非常简单对状态向量的第k个分量加一个小扰动再用中心差分近似梯度把它和解析算出来的雅可比列对比function checkJacobian(residualFun, x, jac, step 1e-6) for k in eachindex(x) xp copy(x); xp[k] step xm copy(x); xm[k] - step jac_num[:, k] (residualFun(xp) - residualFun(xm)) / (2 * step) end return norm(jac_num - jac) 1e-3 end注意扰动步长不能太大也不能太小1e-6在大多数位姿和逆深度尺度下都是合适的。对旋转的扰动校验比较特殊因为你用四元数表示状态时必须在切空间里加扰动而不是直接给四元数四个分量都加微小偏移。我的做法是专门定义增量向量和更新函数让校验和目标函数用同一个状态表示这样测出来的雅可比才是真正参与求解的那个雅可比。另外一个实用小技巧把优化后的残差分布画成直方图。如果残差近似服从以0为中心的高斯分布说明误差模型和权重分配比较合理如果残差直方图明显偏斜或拖着长尾说明还有系统误差没建模比如bias变化太大、外参标定不准、或者畸变模型对不上。写到这里手写VIO后端这部分我最大的感受是里面的每个概念单独拎出来都不算难视觉重投影误差就是投影再相减IMU预积分就是惯性导航里那套传播方程边缘化就是对一个高斯分布做积分。但这些概念组合在一起再落到代码里就会出现各种奇奇怪怪的问题雅可比方向反了、信息矩阵忘了转置、边缘化时先验块被加了两遍、LM的阻尼项忘记对尺度归一化。任何一个环节出错后端要么发散要么收敛到错误解而且debug起来特别难因为你看到的只有一条漂移的轨迹根本不知道是哪个环节出了问题。如果有人问我怎么快速入门VIO后端我的建议很简单先把不带边缘化的纯BA跑通再一步一步加IMU、加滑动窗口、加先验每一步都用仿真数据验证。后端不是靠背公式能学明白的必须自己把H矩阵打印出来把每次迭代的残差变化曲线画出来亲眼看到它一步步收敛才能真正理解这套东西为什么这么设计。这也是手写一遍VIO后端最大的价值所在。