企业官网建设流程全解析

这个系列写到第五篇，终于到了我最不想写、但也最绕不开的部分：手写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滑动窗口里，状态量大概分成这么几类：

关于路标点用逆深度还是三维坐标，这里值得多说一句。单目视觉存在尺度不确定性，如果直接优化三维坐标 [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 帧实际匹配到的归一化坐标，π 是针孔投影函数（就是归一化到z=1平面）。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残差的典型量级如下：

我自己的做法是：第一次跑通时，先把视觉标准差设成由特征跟踪残差统计出来的真实值，IMU噪声按传感器手册的数量级给，然后打印两类残差的平均范数。如果某一类残差的总量比另一类大一个数量级以上，先别急着怀疑算法，先去检查权重和信息矩阵是不是给错了。

很多新手在代码里写死了一个 weight=1.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.001，IMU残差普遍为0.1，那IMU其实已经把视觉完全淹没了，哪怕视觉权重设得再高，H矩阵里视觉项的贡献也微乎其微。

给一组参考初值：

如果你发现优化在两三种参数组合下都发散，那大概率不是调参问题，而是下面这些更基础的问题。

5.2 发散时的排查顺序：先查四元数，再查零偏，最后查边缘化

我把最常见的崩溃原因，按检查优先级排个序：

1. 四元数更新以后有没有规范化？四元数在迭代更新后很容易丢掉单位范数约束，导致旋转误差越来越离谱。这是最基础、也最容易犯的错。
2. 零偏更新量是不是太大？预积分线性化成立的前提是零偏变化很小。如果零偏增量一次更新好几倍标准差，预积分量的“一阶修正”假设就崩盘了。
3. LM阻尼是不是太小？高斯牛顿在远离极值点时容易震荡，LM的阻尼系数初值可以设到1e-6到1e-3之间，再根据实际下降增益去调。
4. H矩阵是不是奇异？特征点在某帧完全没有观测、某个状态没有任何约束时，H会奇异。即使加了阻尼也可能出现数值问题，去检查矩阵求解器返回的警告。
5. 边缘化后是否出现负特征值？先验矩阵在数值上应当是对称正定的。出现负特征值，十有八九是FEJ没做对，或者先验累加时把同一个观测重复计了。

5.3 阶段式调试策略：从纯视觉到完整后端逐层叠加

最后分享一个我认为最重要的调试策略：不要一上来就把视觉残差、IMU残差、滑动窗口、边缘化全部打开。失败时你不知道该查哪一块。我的做法是把后端拆成四个阶段，每个阶段单独验证：

• 阶段A：只用视觉残差，固定零偏，加一个很小的先验，先把BA优化跑通；
• 阶段B：加入IMU残差，检查预积分和雅可比是否一致；
• 阶段C：加入滑动窗口，但先不边缘化，直接丢弃旧帧，看精度损失有多大；
• 阶段D：加入边缘化和FEJ，和阶段C对比轨迹差异。

每一阶段都能拿出一个可以运行的版本，再往后叠。这样如果阶段D出了问题，你至少知道它一定来自边缘化那一堆代码，而不是又要从头看起。我在实际手写时，阶段C和阶段D的轨迹对比是最直观的：没有边缘化时，前几十秒还好，越往后越飘；加上边缘化之后，长轨迹的稳定性明显变好。这个对比本身就说明边缘化的价值。

写到这里，就是我这轮手写VIO后端最想留下的几句话：后端不是把公式背出来塞进代码就完事，它考验的是你对“状态”“残差”“信息”三个概念的理解，以及当你面对发散轨迹时，手里有没有一套系统的排查方法。希望这篇总结能给你省下几个调试到凌晨的夜晚。