这个系列写到第五篇终于到了我最不想写、但也最绕不开的部分手写VIO的后端。如果你是从第一篇一路跟过来的应该已经体会过理论框架的绕、视觉前端的碎、IMU预积分的繁琐以及初始化阶段那种“跑起来了但总觉得哪里不对”的微妙。但真到了后端前面那些都只能算热身。满屏的雅可比、信息矩阵、Schur消元、边缘化轮番上场稍不留神整个优化就会在几轮迭代后发散而且你大概率一时半会查不出原因。这篇文章想做的事很简单把后端这一大坨东西拆开讲清楚它到底在解决什么问题、残差是怎么搭起来的、H矩阵为什么长那个样子、边缘化为什么是手写实现里最大的坑。写给谁给我自己做个总结也给正在手写VIO、或者想从调包变成真正懂原理的读者一份可以照着走的路线图。看完你不需要记住所有公式但你应该能在白板上把VIO后端的整体骨架画出来知道每一块代码为什么存在。1. 后端的本质把一段轨迹和一堆地图点“摆成自洽状态”我建议你写后端之前先别碰代码拿一张纸把优化问题写出来。后端本质上不是某个算法而是一种“让所有传感器观测互相讲得通”的约束求解过程。前端给你的是帧间运动估计和特征跟踪IMU给你的是帧间的相对运动增量但它们都有噪声、都有误差。后端要做的就是找到一个状态量轨迹 地图点让所有观测在这个状态下出现的概率最大。1.1 从最大后验到最小二乘省略的是推导保留的是算法骨架学VIO后端时最容易被概率公式劝退。其实手写时你不需要每次都从贝叶斯公式推起但必须清楚它的结论落在哪里。假设我们要估计滑动窗口内所有状态 X已知所有观测 Z想让后验概率 P(X|Z) 最大。套用贝叶斯公式并把与 X 无关的项扔掉再假设所有噪声服从高斯分布取负对数之后最大化后验就等价于最小化加权平方和X* argmin_X ( Σ_visual ||r_v(X)||²_Σv Σ_imu ||r_imu(X)||²_Σimu )这里的下标 Σ 表示“马氏距离”残差转置乘信息矩阵再乘残差。信息矩阵就是协方差矩阵的逆。手写实现时你不需要真的去实现一个概率模型类只需要对每个残差项保存“残差向量 信息矩阵”然后在目标函数里累加即可。用一个生活化的类比如果每个传感器都是一个人各自向你报告“物体大约在这里”后端就是把这些报告做一次加权综合。但它不是直接把坐标平均而是在“约束级别”做综合——因为每个报告之间还互相依赖比如同一张地图点会被多个关键帧看到。你可以把它理解成一群人合伙记账每个人对同一笔支出记的金额略有偏差后端要找出一个最合理的账本让所有人手里的记录都能对得上。对VIO来说为什么一定要提“最大后验”而不是“最大似然”因为IMU和边缘化会给系统带来先验信息。尤其是边缘化之后系统里存在一部分已经固化下来的约束它们本质上就是贝叶斯公式里的先验项。如果你只做最大似然等于丢弃了那些历史信息轨迹自然会飘。1.2 状态量清单位姿、速度、零偏、逆深度写后端第一件事是先把“我在优化哪些量”定义清楚。一个典型的VIO滑动窗口里状态量大概分成这么几类状态类别常用符号含义维度关键帧位姿T_i (R_i, p_i)第i帧相机在世界坐标系下的位姿6速度v_i第i帧在IMU坐标系下的速度3加速度计零偏b_a_iIMU加速度计零偏3陀螺仪零偏b_g_iIMU陀螺仪零偏3路标点λ_i 或 [x, y, z]观测到的地图点位置每点1或3关于路标点用逆深度还是三维坐标这里值得多说一句。单目视觉存在尺度不确定性如果直接优化三维坐标 [x, y, z]远处点的深度值可能从几米到几百米跨越好几个数量级数值上很容易病态。而把路标点参数化为“anchor帧观测方向 逆深度1/d”后逆深度在近处和远处的误差尺度比较均匀优化起来也更容易收敛。所以主流的做法都是逆深度参数化。你可能要说“我先把三维坐标跑通之后再改成逆深度行不行”行但你要有心理准备。三维坐标实现简单但滑动窗口里一旦出现尺度扰动或深度较大的远点收敛性会明显变差。如果你只是想验证后端流程可以用三维坐标如果你打算做一个能长时间运行的VIO早点换成逆深度参数化更省事。1.3 后端需要前端给它喂哪些“原材料”后端不会自己提取特征也不会自己重新积分IMU。所以手写时接口设计比后端本身的数学更早决定成败。我建议你把后端的输入明确限定为四样东西关键帧位姿初值通常来自前端的帧间估计或上一轮优化的结果特征跟踪关系哪些路标点在哪几帧被观测到各自的归一化坐标是多少帧间IMU预积分结果ΔR、Δv、Δp以及对应的协方差矩阵边缘化的先验信息如果已经在跑滑动窗口后端还要接收上一轮边缘化产生的先验项。我在手写时踩过一个很深的坑一开始没有把这四类输入封装成独立的数据结构而是散落在各种数组里。结果前端一改接口后端整个重构白白浪费了两天。听我一句劝先把接口设计好后面加边缘化、加关键帧筛选才不会被重构拖垮。2. 残差的两条腿视觉重投影误差与IMU预积分误差的构建后端的目标函数归根结底就是两类残差视觉的、IMU的。这两类残差一个管“图像观测和几何投影对不对得上”一个管“IMU积分出来的运动和状态变量对不对得上”。它们在数学形式上完全不一样但最终都会被非线性优化器扔进同一个H矩阵里。2.1 视觉重投影误差预计位置和实际打卡位置的差视觉残差的思想其实很简单。假设某个路标点在第 i 帧一般叫anchor帧第一次被观测到我们用该帧相机坐标系下的方向向量和逆深度 λ 来表示这个点。现在想预测它在第 j 帧的像在哪里只需要把那个点从第 i 帧坐标系变换到第 j 帧坐标系再投影到归一化平面r_v z_j - π( T_cw_j * T_wi * P_i )其中 z_j 是第 j 帧实际匹配到的归一化坐标π 是针孔投影函数就是归一化到z1平面。P_i 由anchor帧方向向量乘上 1/λ 得到。这里有两个细节值得注意。第一为什么在归一化平面上算残差而不是直接在像素平面上算因为内参和畸变应该提前在特征提取阶段就处理掉如果残差建模在像素平面你的雅可比里还要多一环内参矩阵的链式求导纯属给自己找麻烦。第二视觉残差的量级很小通常是 1e-3 到 1e-1 的水平因为归一化坐标本身的数值就在 0 到 1 之间。一个非常实用的调试技巧在后端跑起来之后把某一帧所有特征点的重投影误差画出来。如果误差呈标准正态分布、集中在零附近那视觉部分基本没问题如果误差系统性偏大比如所有点都往一个方向偏移那多半是位姿初值有问题或者特征跟踪时间戳根本没对齐。这个技巧能帮你把“优化算法的问题”和“前端数据的问题”快速分开。2.2 IMU预积分残差别在每次迭代时重新积分如果每一轮优化都把IMU从第 i 帧原始测量开始积分到第 j 帧那计算量会随着优化迭代次数线性爆炸实时性根本无从谈起。预积分的核心思路是把两帧之间的IMU运动增量在优化前就预先算好。优化迭代过程中位姿变了只需要在预积分量附近做一个一阶修正不用重新积分。预积分残差一般由三部分组成旋转残差δφ_ij Log( ΔR_ij^T * R_i^T * R_j )用对数映射把旋转误差变成三维向量速度残差Δv_ij gΔt - (v_j - v_i)再扣除零偏带来的修正项位置残差Δp_ij v_iΔt 0.5gΔt² - (p_j - p_i)同样要扣除零偏修正项。这里最容易忽略的是零偏修正。预积分量是在某个零偏假设下算出来的优化过程中bias一旦更新预积分量本身也要跟着变。工程上通常的做法是在预积分类里保存预积分量对零偏的偏导矩阵然后每次只用一阶近似去更新而不是重新积分。如果你之前只看过预积分的公式没手写过你可能会觉得这个残差有点绕。但实际操作起来它的代码结构比视觉残差更固定——就是对着公式逐行翻译难的是把各个雅可比矩阵的符号弄对。后面我会专门讲雅可比验证的方法这里先记住一句话IMU残差的量纲是米、米每秒、弧度和视觉残差的“尺子”完全不一样权重配平稍有不慎就会“一边倒”。2.3 两类残差的量级差异与权重初值手写VIO后端时残差权重不是随便拍的。权重本质上是信息矩阵也就是对测量可信度的估计。视觉残差和IMU残差的典型量级如下残差项表达式典型量级信息来源视觉重投影归一化坐标之差1e-3 ~ 1e-1特征跟踪预积分位置位移偏差1e-3 ~ 1e-1 米IMU积分预积分速度速度偏差1e-2 ~ 1e-1 米/秒IMU积分预积分旋转角度偏差1e-4 ~ 1e-2 弧度IMU积分我自己的做法是第一次跑通时先把视觉标准差设成由特征跟踪残差统计出来的真实值IMU噪声按传感器手册的数量级给然后打印两类残差的平均范数。如果某一类残差的总量比另一类大一个数量级以上先别急着怀疑算法先去检查权重和信息矩阵是不是给错了。很多新手在代码里写死了一个 weight1.0然后就一头扎进调参地狱。我见过最夸张的情况是视觉权重小了三个数量级导致系统基本只用IMU预积分做短时间预测轨迹看起来也“挺平滑”但一转弯就偏得离谱。所以第一轮调试一定要先让两类残差在同一音量上说话再谈其他。3. 线性化与H矩阵的稀疏结构手写求解器前必须先看懂的“形状”后端优化走的是迭代路线在某个线性化点附近把残差展开成一阶泰勒近似然后求解一个局部子问题。这个过程中真正决定性能的不是你用的是高斯牛顿还是LM而是H矩阵的稀疏结构是不是被正确利用。很多人第一次写VIO后端直接用稠密矩阵把H存下来结果跑十几帧就开始卡然后怀疑自己算法写错了。其实算法没写错是数据结构选错了。3.1 雅可比从哪来链式法则的工程化写法残差对状态的导数雅可比矩阵是非线性优化里最绕的部分但也是最能“作弊”验证的部分。视觉重投影误差对位姿的雅可比就是投影函数对相机坐标系坐标的导数再乘上相机坐标系坐标对位姿扰动的导数。IMU残差对位姿、速度、零偏的雅可比则是对残差表达式逐项求偏导再叠加上预积分量对零偏的修正项。我强烈建议手写时准备一个数值雅可比函数来做交叉验证。数值雅可比实现起来就是最原始的差分法for each 状态维度 d: x_plus X ε * e_d x_minus X - ε * e_d 雅可比[:, d] (r(x_plus) - r(x_minus)) / (2ε)把解析雅可比和数值雅可比放在同一个优化迭代里比较如果两者差异超过 1e-5那你的解析公式里大概率有符号或坐标变换方向的问题。我自己的血泪教训是旋转扰动方向上的符号错了优化结果看起来“还行”但精度始终差一截怎么调参都救不回来。后来就是靠数值雅可比一列一列比对才发现某个负号写反了。这个验证步骤看起来笨却是手写VIO后端能跑通的唯一捷径。3.2 H矩阵的稀疏性大对角带加稀疏十字有了残差和雅可比H矩阵就是 Σ J^T W J。你会发现H不是稠密的它只在“由某个残差项连接起来的两个状态”之间形成非零块。视觉残差连接的是anchor帧、当前帧、路标点三个节点IMU残差连接的是相邻两个关键帧以及各自的零偏和速度。于是整个H矩阵的形状可以描述成一个大致的带状对角矩阵对角线上的每个块都对应一个关键帧内部的稠密状态非对角线只有存在因子关系的帧之间才有块。路标点那一大块则是稀疏散布的十字形结构因为每个路标点只被少数几帧看到。这个结构对求解性能意味着什么路标点的数量通常是关键帧数量的几十倍如果你把所有路标点块都当成稠密矩阵处理内存和时间都瞬间爆掉。所以后端的工程实现核心就是“识别稀疏性利用稀疏性”。你不需要自己实现一个稀疏线性代数库但你要能在脑子里把H矩阵的非零图案画出来知道哪些块可以预计算、哪些块必须动态填充。3.3 Schur消元的完整步骤先把海量路标点“消”掉因为路标点数量太大了直接求解完整的正规方程 H Δx b 不现实。所以标准做法是用Schur消元把路标点先消掉。把所有状态分成两类相机状态帧位姿、速度、零偏记为 x_c路标点状态记为 x_m。那么方程可以分块写成[ H_cc H_cm ] [Δx_c] [b_c] [ H_mc H_mm ] [Δx_m] [b_m]对路标点那一块做Schur补得到只关于相机状态的小规模系统S H_cc - H_cm * H_mm^{-1} * H_mc g b_c - H_cm * H_mm^{-1} * b_m然后先解 S Δx_c g求出相机状态的增量再回代到下面的方程里求出路标点增量。这个过程翻译成代码其实就是几行矩阵运算1. 组装 H 和 b按相机状态/路标点分块 2. 求解 H_mm^{-1}通常是对角块稀疏非常快 3. 计算舒尔补矩阵 S H_cc - H_cm * H_mm^{-1} * H_mc 4. 计算右侧 g b_c - H_cm * H_mm^{-1} * b_m 5. 用稠密求解器解 S * Δx_c g 6. 回代求 Δx_m H_mm^{-1} * (b_m - H_mc * Δx_c)这里有一个很容易搞错的细节LM迭代时要在H矩阵对角线上加阻尼项λI。如果是先加阻尼再Schur消元那S对角线上的阻尼会被H_mm^{-1}那一步“污染”一部分导致实际问题不是你想解的那个阻尼最小二乘。所以我自己的经验是阻尼项加入H_cc和H_mm的对角块之后再做Schur消元保证S正定。4. 滑动窗口与边缘化VIO的“记忆管理哲学”后端不可能永远保留所有状态。你把所有历史帧都存下来每轮都做全局BA时间和内存会随运行时长线性甚至超线性增长。所以在实际VIO系统里后端只维护一个长度有限的滑动窗口窗口之外的旧信息不能简单扔掉而是要“打包”成一个先验项继续影响后续优化这个过程就是边缘化。4.1 为什么全局BA救不了实时VIO全局BA从理论上看是最优的——所有历史信息都不丢。但在线运行场景里轨迹持续增长关键帧和地图点数量只会越来越多每一轮优化要处理的矩阵维度都在膨胀。你可能在实验室跑前10秒飞快跑3分钟后每一轮优化都要一两秒那就完全没法落地到实时设备。更重要的是VIO本身只需要短期一致性。你要的不是“把半小时前的地图精确重建成米级模型”而是“过去几秒内的位姿轨迹足够平滑、足够准”。所以滑动窗口是工程和精度的折中窗口足够容纳当前时刻周围的有效约束丢弃更早的信息也不会对短时位姿估计产生致命影响。窗口大小怎么定我建议从10帧开始。8~12帧是一个常见的可选范围帧太少IMU约束不足精度下降帧太多计算量上涨还可能把较弱的旧约束拖累当前优化。等你把整个流程跑通再根据实际平台的CPU占用去调。4.2 关键帧策略什么时候把新帧放进窗口滑动窗口里并不是每帧都放。前端会先判关键帧常见的依据是当前帧和上一关键帧之间特征视差超过阈值比如10~15像素时间间隔超过一定长度比如0.5秒当前帧跟踪质量明显下降。如果所有帧都塞进窗口后端很快就会因为冗余帧之间几乎没有视差导致H矩阵奇异。反过来说关键帧太稀疏预积分跨度变大IMU线性化误差也会变大。所以关键帧判定是VIO的“节拍器”需要根据场景动态调整。窗口满的时候要选择一帧移出。我的经验是选“最老且与当前帧共视最少”的帧避免把一个强约束的关键帧边缘化掉。这一步听起来像是工程细节但选错帧会直接影响边缘化先验的质量。4.3 边缘化的实现细节把被移出帧变成“先验税单”边缘化是被移出窗口的旧帧的“离职交接”。你不能直接把被移出帧的证据当垃圾扔掉因为那些证据已经通过视觉和IMU约束和保留状态绑定在一起了。正确的做法是把与该帧相关的所有残差在当前线性化点展开成二次型然后对这些残差做一次Schur补把被移出状态消掉剩下一项只关于保留状态的先验项。这个先验项表现为一组“先验信息矩阵 H_prior”和一个“先验右侧向量 b_prior”。之后每一轮优化目标函数里都要加上这一项。注意边缘化的先验是逐帧累加的每移出一帧就叠加一次。如果你把同一个观测既留在普通残差集合里又放进先验里等于把同一份证据用了两遍结果会偏得很难看。我在实际写边缘化时首先被难住的是“哪些残差要参与边缘化”这个问题。答案很简单只要残差里包含被移出状态的全部参与。参与完以后这些残差就要从普通残差集合中移除后续迭代不再单独计算它们。4.4 FEJ边缘化阶段最容易埋雷的“线性化点锁定”边缘化有个非常隐蔽但致命的坑叫First Estimate Jacobians。简单说边缘化发生的那一刻我们对某个状态 X0 做了线性化得到先验项。之后优化迭代会不断更新 X0如果普通残差也随着新的 X0 重新线性化而先验项还停留在旧的 X0 上两者描述的就不是同一个状态点H矩阵会变得不一致严重时优化发散或者长跑之后出现系统性漂移。FEJ的工程做法是对于被边缘化先验“锁定”的那些状态后续计算普通残差雅可比时也使用边缘化发生时的线性化点而不是当前更新后的点。这通常需要一个小的状态记录结构在雅可比函数里查一下“这个状态是否被固定”。你可以把FEJ理解成一个常见的生活场景离职员工交接时把一份重要文档按当时的版本存档了后来项目文档改了但那份存档不能跟着改因为它记录的是“当时那个时间点”的状态。如果你在后续迭代里用新状态和旧先验混着算就是让两个不同版本的信息互相打架。说句实在话FEJ是最容易让手写VIO后端心态崩的地方。小数据集上不实现FEJ优化往往也能收敛但长轨迹跑起来就会出现“说不上来哪里错但轨迹就是慢慢偏”的症状。所以我会建议你哪怕先实现一个简单的FEJ版本也比不做强。5. 手写VIO后端的调参、调试与常见崩溃现场就算数学推导都对了代码写完了后端也一定会出各种幺蛾子。这一章我把常见的崩溃现场和经验性的排查顺序写出来不一定让你一次跑通但至少能让你少走几个弯路。5.1 先让残差“按同一音量说话”权重问题的外在表现是某一类残差主导了优化方向另一类形同虚设。最直接的验证方式是在每个迭代里打印所有残差的平均范数看视觉和IMU的量级是否在同一个数量级附近。如果视觉残差普遍为0.001IMU残差普遍为0.1那IMU其实已经把视觉完全淹没了哪怕视觉权重设得再高H矩阵里视觉项的贡献也微乎其微。给一组参考初值参数初值建议调太大的影响调太小的影响视觉标准差1~2像素对应到归一化平面视觉约束弱轨迹容易飘视觉约束太强容易被误匹配带偏IMU白噪声按传感器手册数量级IMU约束弱短时预测不准零偏容易被过拟窗口大小10帧计算量上涨精度下降如果你发现优化在两三种参数组合下都发散那大概率不是调参问题而是下面这些更基础的问题。5.2 发散时的排查顺序先查四元数再查零偏最后查边缘化我把最常见的崩溃原因按检查优先级排个序四元数更新以后有没有规范化四元数在迭代更新后很容易丢掉单位范数约束导致旋转误差越来越离谱。这是最基础、也最容易犯的错。零偏更新量是不是太大预积分线性化成立的前提是零偏变化很小。如果零偏增量一次更新好几倍标准差预积分量的“一阶修正”假设就崩盘了。LM阻尼是不是太小高斯牛顿在远离极值点时容易震荡LM的阻尼系数初值可以设到1e-6到1e-3之间再根据实际下降增益去调。H矩阵是不是奇异特征点在某帧完全没有观测、某个状态没有任何约束时H会奇异。即使加了阻尼也可能出现数值问题去检查矩阵求解器返回的警告。边缘化后是否出现负特征值先验矩阵在数值上应当是对称正定的。出现负特征值十有八九是FEJ没做对或者先验累加时把同一个观测重复计了。5.3 阶段式调试策略从纯视觉到完整后端逐层叠加最后分享一个我认为最重要的调试策略不要一上来就把视觉残差、IMU残差、滑动窗口、边缘化全部打开。失败时你不知道该查哪一块。我的做法是把后端拆成四个阶段每个阶段单独验证阶段A只用视觉残差固定零偏加一个很小的先验先把BA优化跑通阶段B加入IMU残差检查预积分和雅可比是否一致阶段C加入滑动窗口但先不边缘化直接丢弃旧帧看精度损失有多大阶段D加入边缘化和FEJ和阶段C对比轨迹差异。每一阶段都能拿出一个可以运行的版本再往后叠。这样如果阶段D出了问题你至少知道它一定来自边缘化那一堆代码而不是又要从头看起。我在实际手写时阶段C和阶段D的轨迹对比是最直观的没有边缘化时前几十秒还好越往后越飘加上边缘化之后长轨迹的稳定性明显变好。这个对比本身就说明边缘化的价值。写到这里就是我这轮手写VIO后端最想留下的几句话后端不是把公式背出来塞进代码就完事它考验的是你对“状态”“残差”“信息”三个概念的理解以及当你面对发散轨迹时手里有没有一套系统的排查方法。希望这篇总结能给你省下几个调试到凌晨的夜晚。