VINS-Mono这套代码很多人从入门到放弃基本都卡在后端线程这一关。前面几节我们把前端特征提取、光流追踪、IMU预积分都过了一遍那些东西虽然琐碎但只要耐心对着一行行看总能慢慢捋清楚。到了thd_BackEnd线程就不一样了它背后是整个视觉惯性导航系统里最核心、最绕的滑窗优化与边缘化机制。这一节我们就把这个线程彻底拆开从线程入口到优化器求解把每一段逻辑都讲明白。我最早看这段代码的时候绕了差不多一周才真正弄懂边缘化那几步。后来发现只要先把线程的整体定位搞清楚再把代码块按功能切成几段其实并没有想象中那么难。这篇就按我自己的学习路径来写尽量把“为什么这么做”也一并讲清楚而不是只贴一堆代码让人干瞪眼。1. 核心认知BackEnd线程在整个VIO系统里的定位1.1 三条线程各管一摊VINS-Mono在运行时会拉起三条主要的工作线程分工非常明确。前端线程专门处理图像做特征提取、光流跟踪以及关键帧的判断和特征点管理回环线程负责全局的闭环检测与位姿图优化它的作用是在系统跑了很久之后把漂移拉回来而我们这一节要看的后端线程就是在vins_estimator模块里不断处理滑窗优化的地方。这个后端线程是整个系统的“决策中心”。前端检测到的新图像、IMU积分出来的预测结果全部都要送到这里来统一处理。后端线程要做的事情有两个层面一是维护一个大小固定的滑动窗口二是基于窗口内的所有观测联合优化所有需要估计的状态量。最终算出来的位姿、速度、陀螺仪和加速度计偏置、特征点的深度全都从这里产出再被送去可视化、送去给回环检测使用。理解了这条线程的定位很多代码细节就顺了。它不负责“感知”原始数据也不负责“修正”全局轨迹它只负责一件事在窗口内把所有的误差项压到最小求出一组最优的状态估计。1.2 数据流转谁喂给后端后端的输出去哪VINS-Mono的模块之间是通过全局变量和回调函数通信的。图像数据经过前端处理后会以关键帧的形式进入后端的数据队列IMU数据则在回调里被累积下来等待后端线程取用。这里有个很容易忽略的点IMU的预积分并非在后端线程里从头开始算后端拿到的其实已经是“预积分好的增量”和“协方差矩阵”。拿IMU举例前端或者回调里连续收到多个IMU数据先通过预积分接口把加速度和角速度积分成两帧之间的相对旋转、速度和位置变化。后端线程再把这些结果作为因子放进优化问题。这样做的核心原因很实际IMU的频率通常是200Hz甚至更高几十上百个原始数据如果直接丢进滑窗优化计算量会爆炸。预积分本质上是一种“降采样”把一个时间段内的所有惯性测量压缩成一个增量约束损失的信息已经用协方差矩阵显式地表达出来了。后端的输出则五花八门。优化完一帧后位姿会被直接塞进一个专门用于可视化的全局路径队列这样RVIZ上才能画出实时的轨迹线。IMU的偏置估计也会被送回给预积分模块用最新的偏置重新校正预积分结果。回环线程则可能在合适时机读到后端最新算出的位姿用来做闭环匹配。2. 线程主循环wait与process的双人舞2.1 run()与process()的执行逻辑精简版的后端线程实现主循环其实非常朴素核心就是一段不断调度的代码。这里我先贴一段极简化的示意结构帮大家建立整体印象void BackendThread::run() { while (true) { process(); std::this_thread::sleep_for(std::chrono::milliseconds(1)); } }看到这个是不是有点失落说好的复杂多线程呢。别急真正的逻辑全都被藏进了process()里面。process函数要做的事情可以拆成两幕先看有没有新数据到来有就处理没有就按照设计的节奏继续空转等待。这里的“等待”并不是简单的sleep。在VINS的原版实现里后端线程往往靠一个标志位或者条件变量来感知新数据是否就绪。某个全局变量维护着当前窗口内最新的一帧图像和对应的IMU预积分结果前端线程更新这个变量时会把一个状态置为“有新的输入”。后端线程的process()检测到这个状态后就会进入真正的数据消费流程。这个设计看起来简单实际工程中却很稳。因为优化本身是耗时操作如果每次都等所有数据齐了再处理实时性根本保证不了。这里采用的就是经典的“生产-消费”模型前端负责把数据准备好后端在空闲周期里检查并消费两者之间不需要太强的同步关系。2.2 初始化分支为什么优先在后端线程的process()里第一个判断往往是系统是否已经完成初始化。如果是第一帧或者前几帧系统还没有稳定的位姿、速度、偏置和深度估计这时候如果直接进入优化整个问题就是病态的。VINS的做法是在初始化完成之前后端线程会先跳过优化主逻辑而专注于把初始状态“建立”出来。初始化阶段会做两件关键的事情通过视差判断特征点是否有足够的平移观测然后联合视觉和IMU数据估计出初始的位姿、速度和尺度因子。尤其要注意的是VIO系统里单目视觉加IMU存在一个最核心的痛点单目相机本身无法直接恢复尺度整个轨迹的真实比例关系需要由IMU的重力反馈来确定。这一步一旦算错后面的所有优化都会在错误的基础上打转。很多人在看代码时直接跳过初始化去看滑窗优化。我建议反过来先把初始化逻辑完整读一遍因为后面的状态维度、雅可比矩阵的排列方式很大程度上是初始化阶段定下来的。简而言之初始化是地基地基没打好上面盖的楼再漂亮也没有用。3. 滑窗优化与状态管理3.1 滑窗多大、里面装了谁VINS默认的窗口大小是11帧代码里的常量一般是WINDOW_SIZE 10。这个“10”指的是窗口内除了当前最新帧之外还保留历史的10帧。每进来一帧新图像窗口就要把某个成员移出去同时腾出位置给新帧。为什么不能保留所有历史帧呢因为BA优化也就是Bundle Adjustment每增加一帧要优化的未知量就会线性增长而计算复杂度和内存占用会涨得很快最终导致实时性崩溃。滑窗里究竟装了哪些状态可以看下面这个状态向量// 状态向量分布示意 [ frame0位姿, frame0速度, frame0偏置, frame1位姿, frame1速度, frame1偏置, ... frame10位姿, frame10速度, frame10偏置, 特征点逆深度xN ]每一帧的状态包含了位姿的旋转和平移、速度、陀螺仪偏置和加速度计偏置。旋转部分一般不直接用欧拉角而是用四元数平移是三维向量。速度为什么也要放进优化因为在IMU和视觉紧耦合的框架下速度与位姿的导数之间有明确的约束关系把它当状态量估计能获得更一致的轨迹。特征点的深度则是用“逆深度”来表示的。这个细节非常关键。直接估计深度值当特征点很远时深度值可能非常大数值量级也不稳定优化时容易造成数值问题。而逆深度只是深度的倒数范围一般落在零点几到几之间数值分布合理得多。同时逆深度在数学上也更适合和高斯分布假设配合这是VINS在设计之初就做过取舍的。3.2 图像帧分类关键帧逻辑不是每一帧图像都会进入滑窗优化。前端线程会做一次“关键帧判断”最简单的方式是看和上一关键帧之间的平均视差。如果视差太小说明视角变化不够新增的图像帧带来的信息增量太低直接丢弃如果视差足够大才会成为关键帧进入后端队列。VINS的滑窗策略针对不同情况有两种边缘化方案对应的标志位分别叫MARGIN_OLD和MARGIN_SECOND_NEW。当新来的帧可以作为关键帧时滑窗会移除最老的一帧这叫MARGIN_OLD当新来的帧不被算作关键帧时窗口里则会移除次新帧也就是第二新的那一帧这叫MARGIN_SECOND_NEW。为什么要有这么个区分这是我觉得VINS设计里非常聪明的一点。如果最新的帧不是关键帧说明它和上一帧的视角太接近但它与最新的关键帧之间时间间隔又太短这时如果直接丢掉最老帧可能导致窗口内特征点的视差整体太大。视觉SLAM和VIO里适度的视差对于特征三角化至关重要。保留更老的帧再把次新帧丢掉能让窗口内的视角变化保持一个比较健康的梯度。这两条策略直接体现在代码里就决定了优化完成后要调用哪个边缘化分支。很多刚开始读源码的人看到marginalization_flag时一头雾水不知道为什么要分这么细。理解上述原因后再回头看代码逻辑就非常顺了。4. 边缘化最难啃的一块4.1 为什么要边缘化滑窗的大小是固定的所以每滑出一帧就要把这一帧携带的所有信息从优化问题里“移除”。但“移除”如果只是简单地把对应的矩阵行和列删掉损失的信息就太多了。被移除的帧也是通过大量特征点和其他关键帧建立过观测关系的这些约束信息一旦被粗暴抛弃系统的精度会肉眼可见地下滑。边缘化在数学上做了这样一件事对被移除状态对应的变量我们不直接丢弃而是把它的信息“压缩”到仍然留在窗口里的变量上。具体实现使用的是Schur补操作它通过条件概率分解把被删除变量的信息投影到保留变量上形成一个新的先验项。这样系统虽然不再直接优化被删除的那一帧但它的历史影响仍然留在优化问题里。这里可以用一个生活化的类比。搬家公司搬家时旧房子里有一堆家具不可能全搬走但也不能啥都不带走。边缘化就相当于把旧家具拍成照片、量好尺寸登记在册然后把这些信息全部放进一本册子里交给新房主。以后新房主布置屋子时参考册子就行不再需要旧家具本身。4.2 代码里的四步走VINS边缘化实现的紧凑性在这几行里体现得淋漓尽致。完整过程可以分为四个步骤顺序固定缺一不可。第一步是addResidualBlockInfo()把所有需要参与边缘化的残差块登记进来。这里的残差包括三类IMU预积分残差、视觉重投影残差以及上一轮优化留下的先验残差。这一步的关键是把每个残差块关联的“参数块ID”记录下来因为后面做矩阵分块时需要知道哪些变量是要被边缘化的哪些是要保留的。第二步是preMarginalize()对每个残差块计算线性化后的雅可比和残差。注意这里不是直接在整个大问题里做全局求解而是先把局部残差块的数值都准备好。代码里会调用每个残差块的Evaluate()把残差值、雅可比矩阵以及参数块之间的相对位置全部暂存起来。第三步是marginalize()这是最核心的一步里面有不少Eigen的稠密矩阵运算。系统会把所有线性化方程拼成一个大的信息矩阵H以及对应的误差向量b。然后把状态变量拆成两部分要边缘化掉的分块和要保留的分块。通过Schur补得到一个新的信息矩阵H_prior和残差b_prior这个结果就被当作下一轮优化里的先验项。第四步叫addr_shift()做的事情比较工程化但也不容小觑。因为滑窗里各帧的位置会发生变化老的帧被移出新的帧被挪到对应索引上。边缘化得到的先验残差块内部记录着参数块的地址必须同步更新这些地址否则下次优化求解时ceres会拿到悬空的指针直接段错误崩溃。我自己在第一次抄写这段代码的时候就栽在addr_shift这一步上始终忘了更新地址结果整个程序每隔几分钟就崩溃一次。后来打开GDB才定位到是访问了已经被释放的内存。这块的教训是边缘化不只是数学问题更是内存管理问题。4.3 FEJ与线性化点的一致性提到边缘化就不得不说First Estimate Jacobians简称FEJ。这个概念如果不理解算出来的结果往往会对不齐系统精度比预期差很多。问题是这样的在优化迭代过程中状态的估计值会不断更新。一般残差的雅可比应该基于当前最新的状态估计来计算。但是边缘化生成先验时信息的“方向”是基于当时的线性化点算出来的。如果后续迭代里其他残差块的雅可比却使用了新的线性化点两边就会对同一个状态产生不同的切空间近似等价于把一个信息反复使用了多次最终导致系统的尺度失真。VINS的解法比较直接对于所有参与了边缘化的参数块在后续的优化中计算雅可比时强制使用“第一次估计”时的状态值也就是在边缘化发生之前保存下来的那一版。这样所有残差块的线性化点都保持一致信息就不会被重复计算。FEJ是VINS代码里相对难啃的一环但也是保证整个系统长时间运行不发散的基石。面试里也经常被问到理解这个对整条代码的理解深度会提升一大截。5. 从理论到工程优化求解的细节5.1 Ceres求解器与残差块组织整个滑窗优化最终是交给Ceres库来解算的。在调用优化之前代码会先做一件很重要的事情把边缘化得到的先验项构造成一个特殊的残差块这个残差块在代码里往往对应一个单独定义的成本函数类。这个类的内部会保存边缘化得到的信息矩阵H_prior然后在Evaluate()里把误差向量算出来。这个残差块的维度就是窗口内所有“保留状态”的维度之和通常几百维所以它的雅可比矩阵是一个稠密的小方块。每次优化迭代时这个先验残差块负责把历史信息约束住不让新帧的观测把已经收敛过的状态扯得太远。除了先验残差每个关键帧之间还会加入IMU残差每个被跟踪到的特征点会加入视觉重投影残差。这里视觉残差通常会配上Huber Loss。为什么因为光流追踪不是每次都能稳定跟踪偶尔会出现一个明显的异常点比如匹配错误、遮挡边缘的特征它们的重投影误差会非常大。如果不加鲁棒核这些离群点会把整个优化带偏这是视觉SLAM里常见的坑。IMU残差一般不需要加鲁棒核。因为IMU是物理传感器只要有值就是连续的不会像视觉匹配那样出现明显的“野值”。加了鲁棒核反而可能把真实观测权重压低得不偿失。5.2 逆深度参数化的价值前面提到逆深度参数化这里展开讲一下好处。对于特征点如果直接优化三维坐标参数空间是三自由度的而且深度方向上的量级可能从零点几米到几百米不等优化的数值稳定性很差。使用逆深度后一个特征点只需要一个参数也就是逆深度值其他两个方向由归一化平面的坐标来决定。这种参数化的另一个好处是它天然地适合处理“远点”。一个特征点距离相机很远时它的深度不确定性很大但逆深度的不确定性反而很小更加接近一个高斯分布。高斯分布是卡尔曼滤波和BA问题的基本假设在这个假设下做优化收敛速度和稳定性都要好一些。代码里对视觉残差的实现就充分利用了这个特性。每个特征点在滑窗里有一个“anchor帧”也就是第一次被观测到的那一帧。后续所有帧对这个特征点的观测都会通过这个anchor帧的逆深度值来重投影形成残差。这个设计大大减少了状态维度100个特征点只增加100维状态而不是300维。5.3 常见崩溃与排查实录读代码是一回事真正跑起来调试又是另一回事。我在实践和帮助其他人排查时遇到过不少共通的问题这里整理成一张速查表。现象可能原因排查方法程序跑一会儿就段错误崩溃addr_shift后参数地址没更新到位开启AddressSanitizer或用GDB看崩溃栈是否在边缘化残差块的Evaluate里优化迭代次数很多但误差不下降信息矩阵H_prior被重复添加导致先验过强检查代码里是否对旧先验因子做了析构或者是否重复addResidualBlock轨迹有明显漂移但单步误差不大边缘化掉关键帧后视差约束被削弱重点关注MARGIN_OLD与MARGIN_SECOND_NEW的触发条件是否符合预期初始化完成后系统立刻发散初始scale估计不准IMU偏置初值乱跳检查初始化阶段的重力对齐和尺度因子估计是否收敛必要时调大初始化帧数程序跑着跑着突然变慢CPU占用飙升图像特征太多Ceres求解规模爆炸限制窗口内特征点数量比如控制在150-200个以内时间戳同步异常导致优化时所有残差都异常大IMU和图像时间戳未对齐检查数据集的时戳是否一致必要时在代码入口加时戳对齐逻辑这张表里每一条都是我或者身边朋友踩过的坑。特别是在仿真数据集上跑得好好的换到自己的传感器上就崩溃十有八九是时戳对齐和边缘化地址更新这两个问题。6. 从代码阅读到真正掌握的方法6.1 建议的阅读顺序很多初学VINS的人一上来就直接读estimator.cpp最底下的几段结果被边缘化的Eigen矩阵运算劝退。我建议的阅读顺序跟代码文件的组织顺序反着来。先读头文件把里面定义的成员变量和状态量含义搞清楚再去读processImage函数了解整个滑窗的流转流程最后才碰optimize和marginalize。optimize里面有一大半是在组织Ceres的Problem对象这部分主要是“填空题”难度不高。难的是marginalize里的矩阵分块和Schur补但这部分只要理解了数学原理再对照代码看一遍就能豁然开朗。如果基础相对薄弱建议先不看完整版而是用代码精简版作为骨架来对照。精简版把很多状态机的跳转逻辑抽掉了只保留最核心的优化链路读起来会舒服很多。等把精简版读透了再回到完整版去补细节难度会降一个台阶。6.2 一个最小闭环的实践路径只看不练始终不能真正掌握。我建议直接把滑窗里所有状态的维度打印出来在每次优化前输出一次。这样你能直观看到每一帧优化前后位姿的变化量。第一次跑通的时候你会看到它的轨迹非常平滑地贴在地面真值上那种成就感是看多少论文都换不来的。然后可以尝试一个小改动比如把边缘化这一步注释掉不让它生成先验再跑同一个数据集。对比前后的轨迹误差你会实实在在地体会到边缘化的重要性比你读十遍理论都管用。到了这个阶段再回去看边缘化的数学推导每一行基本都能和代码对应上整个thd_BackEnd线程对你就没有秘密了。我自己的体会是这段线程代码是典型的“先怕后爱”。最初看它觉得边缘化就是一堆莫名其妙的矩阵运算但真正跑通、调通之后再看滑窗优化和边缘化的配合你会忍不住感叹这套设计的精巧。后面你再去接触其他VIO系统会发现大框架基本换汤不换药。把这一节啃下来等于打通了整个VIO任督二脉。