★ FEATURED ARTICLE 手写VIO系列走到第五篇终于轮到后端了。前几篇把前端跟踪、IMU预积分、初始化都过了一遍到了这里其实才是整个系统真正“出活”的地方——所有的观测数据、状态变量、约束关系最终都要在后端汇到一个优化问题里统一求解。后端是整个VIO的定海神针前端给什么吃进去、后端吐什么出来决定了系统的精度和稳定性天花板。这篇我会把后端拆成四个层面来聊整体架构与选型思路、残差模型与状态向量怎么组织、边缘化的原理和代码落地、求解器配置与调优。内容主要面向已经具备SLAM基础、准备手写或正在手写VIO后端的人也适合想彻底搞懂滑动窗口后端细节的读者。代码思路以常见的平滑建图系统为参考结合我自己的工程实践来展开。1. 后端整体架构与技术选型1.1 后端的定位状态收敛的最终裁决者VIO系统的流水线可以粗略分成三段前端负责帧间相关性比如特征提取、光流跟踪、描述子匹配初始化负责在系统启动阶段拿到尺度、重力方向、速度初值后端则接收前两段的输出在一段时间窗口内对所有状态做联合优化输出平滑且一致的位姿轨迹、IMU零偏估计和特征深度。后端解决的核心问题用大白话说就是在所有观测特征点像素坐标、帧间IMU读数给定的情况下找到一组最优的状态变量让所有残差项的总和最小。这里的状态变量包括滑窗内每一帧的位姿、速度、IMU零偏以及被观测特征点的逆深度。输出则是优化后的轨迹、零偏校准结果和地图点坐标。很多人会把前端跟踪出来的位姿直接当成最终结果用这在短时间、弱纹理、小场景下勉强能跑但一旦出现遮挡、加速度突变、长时间运动纯前端的结果会累积漂移。后端的价值就在于把多帧之间的几何约束和IMU物理约束拉通误差被分摊而不是一级级滚雪球尤其在中长距离、快速运动、光照变化这些场景下优势非常明显。1.2 手写后端为什么不用现成优化库直接怼在讨论技术选型时我经常被问后端不就是个非线性最小二乘问题吗直接用Ceres这类库不就行了为什么还要手写我的回答是如果你只是要跑通一个VIO Demo当然可以直接用优化库省时省力。但如果你想真正理解VIO后端在优化什么、边缘化到底对信息矩阵做了什么、为什么某些状态不可观、为什么尺度会飘手写一遍几乎是绕不开的路径。手写后端不等于推翻所有库。矩阵运算用Eigen求解线性方程组也基于Eigen的稀疏求解器但残差函数、Jacobian推导、滑窗管理、边缘化逻辑全部自己实现。这样做的优势有三个残差构建透明出问题时能直接定位到是Jacobian的问题还是残差尺度的问题边缘化和信息矩阵更新逻辑完全可控可以自由实现FEJ、先验注入等细节便于移植和定制必要时可以嵌入到自己的框架里不受优化库API限制。代价也很明显Debug难度大数学功底要求高很多细节需要耐心踩坑。但走完一遍你对后端心理会特别有底。对比维度直接用优化库手写后端开发速度快慢可定制性受API限制完全可控理解深度偏黑盒全面透明Debug难度较低较高调优空间有限大2. 状态向量与残差模型构建2.1 滑动窗口里的状态变量组织写后端代码的第一步是把优化变量具体定义出来。我采用滑动窗口的方案窗口大小一般设10到15帧帧太多计算量爆炸帧太少约束不足容易飘。窗口内每个关键帧的状态定义如下每一帧的状态由位置、姿态四元数、速度、加速度计零偏、陀螺仪零偏组成共15维。这里姿态用单位四元数表示但优化时用最小化的右扰动或左扰动更新不能直接对四元数四个分量求导。特征点的参数化是另一个容易踩坑的地方。一个3D路标点用三维坐标表示相比用逆深度表示在滑窗场景下有明显劣势当某个特征点只在当前窗口内被部分帧看到且相机位姿变化不大时三维坐标的深度方向不确定性很大优化容易把点推到相机后方去。逆深度只用一个参数表示深度倒数天然避免了“深度为负”的无效解也让每个特征点对状态维度的贡献最小。于是整个系统待优化变量就是X [x_0, x_1, ..., x_n, l_1, l_2, ..., l_m]其中 x_i 是第i帧的15维状态l_j 是第j个特征的逆深度。这么做的好处是视觉残差函数对逆深度的Jacobian非常简单而且滑窗滚动时特征点的增删只对应一个标量参数代码上很容易管理。2.2 视觉重投影残差从像素到几何视觉残差是在说一个很简单的事一个3D路标点按照当前估计的相机位姿投影到图像上应该和实际观测到的像素位置一致。不一致的部分就是残差。残差构建链路是这样的路标点先变换到相机坐标系然后投影到归一化平面再做畸变修正最后通过内参矩阵转到像素坐标。完整链路下来Jacobian的推导要经历链式法则每一步对位姿、逆深度、相机内外参求导都要单独处理。我自己写代码时把投影函数分成接口和内部实现两层。接口层只接收状态和三维点坐标内部先检查深度值是否为正、是否在图像边界内过滤掉异常观测。这个过滤看似简单实际对优化稳定性帮助极大因为一个被错误匹配带进来的外点会让Huber核疯狂工作甚至在视觉退化场景下把位姿推飞。观测噪声的建模也很关键。视觉残差的噪声通常假设为像素坐标上的高斯噪声信息矩阵取基于1个像素标准差的逆。不要把这个值设得过于自信比如主观设到0.1像素否则视觉信息会被放大到压过IMU约束导致轨迹在快速旋转时剧烈抖动。2.3 IMU预积分残差把惯性约束压缩到帧间IMU的原始测量频率很高通常有100Hz到200Hz而关键帧之间的间隔有几百毫秒。如果直接在每次优化迭代时把这一段时间内所有IMU读数重新积分计算量完全不能接受而且每次位姿更新后初始条件变了全部都要重算。预积分就是为了解决这个问题出现的。它在两个关键帧之间把IMU测量的相对增量提前计算出来比如相对位置增量、速度增量、姿态增量成了一个只与帧间初始状态和零偏相关的常数。后续优化迭代时直接把两个关键帧的状态差和预积分量作差就是IMU残差。需要特别注意的是预积分量包含了重力影响。因为IMU加速度计测量的线性加速度是“比力”包含重力分离问题所以在残差公式里必须有重力项参与。这也解释了为什么VIO纯单目前端无法估计绝对尺度重力向量和尺度之间存在耦合。零偏在预积分里的处理方式我采用的是对预积分量做零偏的一阶近似修正。每次零偏估计变化后不需要重新预积分全部数据只需要根据预积分量对零偏的雅可比做线性修正精度足够。这个技巧能节省大量重复积分时间是工程上必不可少的一步。3. 边缘化的原理与代码级实现3.1 为什么需要边缘化滑窗滚动时信息不能丢滑动窗口的大小固定新的关键帧进来旧的必然要被移出去。有人会说直接丢掉不就行了吗不行的原因在于被移出的帧对当前窗口内的其他状态还保有着几何约束和信息贡献这些信息是实打实的观测积累直接丢弃会让约束变弱轻则精度下降重则系统退化。边缘化本质上是一种信息压缩把待删除变量所携带的约束变成关于剩余变量的先验信息附加到后续优化中。这样系统既控制了计算规模又不会忘记历史观测。我常打一个比方滑窗好比一扇只能看到前后几帧的小窗边缘化相当于把窗外看到的景色浓缩成一张高动态范围照片贴在新窗框上窗外的风景还在以另一种形式参与构图只是你不能再去直接调整窗外的景物了。3.2 Schur补与信息矩阵更新边缘化在数学上对应高斯牛顿迭代时对信息矩阵的Schur补操作。假设某个时刻待移除变量集合为a剩余变量集合为b则增量方程可以分块写成H_aa * δx_a H_ab * δx_b b_a H_ba * δx_a H_bb * δx_b b_b把 δx_a 消掉后剩余b的更新信息矩阵变为Λ H_bb - H_ba * H_aa^{-1} * H_ab这个 Λ 就是边缘化完成后的先验信息矩阵会与新的残差项叠加。从代码层面看关键不是去求大矩阵的逆而是利用Schur补的形式只对 H_aa 做分解然后用块操作完成更新。Eigen的Block操作在这里非常顺手我通常先把变量重排成“待删块 保留块”的顺序再一次性完成消元和回代。边缘化时还有个细节被边缘化的变量可能是位姿、速度、零偏、特征逆深度的混合体。对于不同的被边缘化帧保留块可能各不相同所以重排逻辑要写得非常通用否则调试起来会想撞墙。3.3 FEJ避免先验信息的重复计算边缘化把历史信息变成先验后下一个窗口的优化会同时包含两类约束一类是边缘化得到的先验信息矩阵一类是新观测构建的残差信息。问题来了这两类约束可能对同一个状态变量进行了线性化但线性化点不同。如果对同一个状态在不同线性化点求Jacobian相当于两个“不同视角”的信息被强行加在了一起会造成虚假信息严重时导致系统过置信轨迹出现人为的“笔直化”趋势。这就是所谓的double-counting问题。解决这个问题的标准手段是FEJ对所有会被边缘化先验和新残差共同影响的状态统一使用它在首次被纳入优化时的线性化点来求Jacobian。在代码实现上我会为滑窗维护一个首估Jacobian数组每次构建残差前检查该状态的线性化点是否已经固定过如果固定过就用固定点求导。FEJ不是所有系统都愿意严格实现因为它需要额外的存储和管理复杂度。但如果你发现你的VIO在长时间运行后轨迹存在系统性的收缩或尺度漂移除了零偏发散等原因外很可能就是边缘化的一致性处理没做好。4. 求解器配置与性能调优4.1 稀疏求解利用结构来换速度后端最终要解一个大规模非线性最小二乘问题。幸运的是H矩阵有非常明显的稀疏结构位姿和位姿之间只通过共视路标间接连接而路标与位姿的关联则是局部的。这意味着大部分分块位置都是零正是SLAM后端的核心加速点。求解时我采用两阶段策略先在位姿层面做Schur消元把路标变量边缘化掉得到只含有位姿状态的增广线性系统求解该小规模问题后再回代求路标。这样做的好处是主求解规模只和滑窗帧数相关和路标数量无关计算负担大幅下降。稀疏线性方程组的求解我选Eigen内置的稀疏求解器配合LLT分解。在N到2N帧规模、几千路标的典型场景下单次迭代耗时从毫无优化的几百毫秒降到几十毫秒左右能够满足频率要求。如果窗口进一步扩大就要准备换用性能更强的稀疏Cholesky实现但手写VIO场景下没必要。4.2 鲁棒核函数与初始值两把保护伞现实中的视觉观测不可能全是干净的误匹配、遮挡边界、动态物体都会产生大残差。如果直接当成高斯噪声硬解一个巨大残差就能把整个状态更新拉偏。这里的对策是核函数残差小于阈值时保持原样大于阈值时用根号形式压缩其影响也就是常见的Huber核。我在代码里为每个残差项都配置了核函数并对信息矩阵做相应的调整。核函数的阈值要根据特征噪声水平和实际运行场景设定太小会浪费有效约束太大又起不到抗外点作用。初始值的重要性容易被新手低估。后端优化本质上是局部算法初始值如果太离谱迭代会发散或收敛到错误极值。因此初始化模块输出的一定要是一个相对合理的位姿序列、速度和重力方向而不仅仅是能跑通就行。我在后端迭代前还会做一次粗配准和残差预检查如果初次残差中位数超过经验阈值直接重置初始化从头来过。4.3 参数调优滑窗大小、信息权重与关键帧策略后端系统的表现和一堆敏感参数有关。滑窗大小直接决定计算量和信息量的平衡我自己一般固定10个关键帧在室内小场景和室外中距离场景表现都稳定。要在更大场景跑建议逐步加大窗口并配对稀疏求解器。视觉和IMU之间的信息权重也需要反复调。视觉信息太弱系统容易在快速运动时跟丢太强则会在弱纹理环境下被噪声主导。最直接的办法是观察残差云图中两类残差的数值尺度让它们在接近同一量级。IMU零偏的先验也不能完全放开否则零偏估计会吸收视觉残差中的系统性误差导致位姿失真。关键帧选取策略同样影响后端效果。我只在视差足够大或者IMU激励足够明显时插入新关键帧不搞固定帧率。视差小的帧直接丢弃避免冗余约束拖垮优化规模和稀释有效信息。5. 常见问题与调试经验实录5.1 信息矩阵奇异求解器说算不了症状非常直白求解器报矩阵不可逆或条件数超大。这类问题多半发生在某段滑窗内视觉约束几乎为零比如相机对着白墙或纯色天花板平移又恰好那几帧IMU零偏完全不可观。这时候即使理论上有IMU约束身体也会提出抗议。排查手段是打印当前H矩阵的最小特征值和条件数。如果最小特征值低于某个数量级就要去找约束薄弱的帧查看该帧附近是否有足够的特征点覆盖和视差。长期修法有两个收紧关键帧的最小视差策略或者给零偏加弱先验让系统在视觉退化时也不会直接崩溃。5.2 边缘化导致的不一致轨迹在窗口滚动时跳变这个坑我调试很久才真正理解。现象是优化结果在单帧内平滑但每次滑窗滚动后新窗口估计的轨迹与上一窗口尾部轨迹之间存在一个突然的跳变。跳变幅度不大但长时间运行下来会让整条轨迹呈现出“阶梯”形态。本质上就是边缘化先验与新增约束在线性化点上不一致造成的。修复思路是严格执行FEJ让被边缘化状态和新状态在同一线性化点下共享Jacobian。另外边缘化后的先验残差在每次滚动后都要重新计算一次确保先验残差里的状态值已经更新而不是沿用边缘化时刻的旧值。5.3 抖动与漂移优化结果不稳定如果前端跟踪很稳但优化得到的位姿在帧间有明显抖动大概率是残差权重配比不对。视觉噪声设得太小残差会被哈到无法呼吸位姿随特征噪声抖动IMU权重过低则快速运动时轨迹会有气垫感。调整思路是分别记录两类残差的RMS让它们在典型场景下处于同一尺度。漂移问题则需要同时检查零偏估计是否收敛、重力方向是否被错误更新、边缘化先验是否有过强的记忆效应。我常用一个技巧来区分把真实轨迹和优化轨迹同时可视化如果漂移主要出现在旋转后重点查陀螺仪零偏如果漂移主要出现在直线加速段重点查加速度计零偏和尺度耦合。最后再分享一个我手写后端过程中最大的经验真正难的不是那些公式推导而是把数学结构和代码结构一一对应起来。残差对应一个类Jacobian对应一个函数信息矩阵对应一个稀疏块边缘化对应一次块消元。一旦这个对应关系在你的代码里变得清晰可控整个后端的调试和调优就都顺了。写完这个后端再回去看各种优化库的源码和使用文档你会发现所有概念都串起来了那种感觉才是真的值。
阅读完成 · 觉得有帮助？
