★ FEATURED ARTICLE 搞视觉惯性导航的朋友十有八九都跟VINS-Mono打过交道。但很多人在跑通公开数据集之后换上自己的设备或者自己录的数据系统就开始不干人事了——要么轨迹一会儿飘到天上要么跑着跑着直接发散要么前端明明在跟踪但地图越建越歪。我早期调试的时候经常把这些锅甩给后端优化反复调权重、调核函数折腾一两个星期没什么实质进展最后回头老老实实通读一遍初始化流程才发现问题几乎都出在这一步。VINS-Mono初始化流程是整个系统的基石。它解决的不只是“系统动起来之前需要预热”这种工程问题而是从数学上决定了后续状态估计器的尺度、重力方向、初始速度这些全局量能不能落在正确数值附近。这篇东西我打算掰开揉碎讲一遍初始化要解哪些未知量为什么非解不可每个子模块在干什么以及我实际调试中遇到过哪些坑、怎么定位。适合正在啃代码的人也适合那种“系统能跑但总感觉哪里不对劲”的排查型选手。1. 初始化在干什么从“无尺度视觉轨迹”到“有物理单位的系统状态”1.1 单目相机天生的“尺子缺失”问题先想一个最简单的问题单目相机拍两张照片通过特征点匹配和几何关系你能算出相机从位置A移动到位置B的方向也能算出一个等效的平移向量但这个向量乘上任意正数两张图的重投影关系完全一样。换句话说单目视觉给出的轨迹是一个只能确定到“相似变换等价类”的解单位是像素还是米它自己说不清楚。VINS-Mono的核心是融合视觉和惯性测量单元。惯性测量单元里的加速度计测的是比力陀螺仪测的是角速度这些数据天生带着物理单位米每秒平方、弧度每秒。但有意思的是加速度计数据只有在系统中存在加速度变化时才真正有用而视觉给出的轨迹虽然没尺度却有比较可靠的形状和方向。把两者拧到一起让视觉给惯性提供方向惯性给视觉补上尺度这就是初始化阶段的核心逻辑。类比一下你拿到一张人物照片构图匀称、姿势自然但你看不出这个人的实际身高。旁边放了一个体重秤——它不能直接告诉你照片里人有多高但如果照片里那个人正好在称重、或者你能连续几帧观察到脚底和地面的相对运动那你就能推断出一个真实的比例尺。惯性和视觉的关系差不多就是这么回事。1.2 初始化要交付的“货物清单”初始化不是一个单独的函数调用它是一整套解算过程。它最终要给出以下几个量输出量含义为什么必须由初始化来定重力向量在世界坐标系下的方向把加速度计读数从“机体坐标”搬到“世界坐标”时需要的基准后续所有加速度积分都依赖它尺度因子视觉轨迹与真实物理轨迹的比例关系没有它位置全是像素单位窗口内各帧的初始速度惯性积分需要初始状态后端优化时是重要状态量陀螺仪偏置的粗估计陀螺零偏导致旋转漂移后续紧耦合优化需要初始值视觉坐标系到惯性坐标系的外参校准结果两个传感器的安装关系整个系统都在这个变换基础上运行你可以把初始化理解成“给系统睁开眼睛的过程”。在睁开眼之前视觉在动、惯性也在动但它们各自各说各话初始化做完之后所有传感器数据被拉到同一个坐标系、同一个尺度、同一个物理世界里紧耦合优化才有意义。1.3 为什么必须用滑窗而不是攒一堆数据离线搞VINS-Mono是实时系统不可能把几十秒的数据攒下来做最小二乘。初始化实际运行在一个滑动窗口里常见配置是维持最近的约20帧关键帧。这个设计思路跟很多实时视觉里程计一致既要保证局部的几何约束足够稠密又要控制优化规模避免计算延迟把前端拖垮。滑动窗口带来的一个实际问题就是如果窗口里的运动不充分初始化解出来的量就不稳定。早年文档里经常强调“初始化前让设备缓慢平移”根因就在这。这个问题后面会专门展开。2. 纯视觉SfM先用几何把位姿骨架钉住2.1 为什么第一步反而是“纯视觉”而不是“惯性优化”刚接触VINS-Mono的人容易有个误解既然是视觉惯性融合初始化就应该把两类数据同时丢进一个优化问题里。实际流程不是这样。初始化第一步是用纯视觉的方式把窗口内每一帧的相机位姿和若干三维点恢复出来后续惯性数据才有资格掺和进来。原因很简单视觉几何的旋转信息非常可靠。对极几何给出的本质矩阵、基础矩阵其旋转部分不随尺度变化计算稳定性好而惯性数据在没有重力方向和速度初值前没法直接用于位姿求解。先用视觉把“形状”搞出来再用惯性把“物理刻度”加进去这种拆分降低了求解难度也让每一步的误差可解释。2.2 从帧间匹配到本质矩阵具体做法上初始化会从窗口内选择合适的帧对做特征匹配然后估计本质矩阵。这位置有几个细节值得留意特征点必须均匀分布不能全堆在画面某一个角落。实际实现里一般有网格化特征提取策略保证每个区域都有点。帧对选择上系统会考察特征视差视差太小说明两帧之间几乎没有几何约束算出来的本质矩阵噪声极大。本质矩阵有5个自由度一般用5点法或8点法求解实际工程中经常带有RANSAC把错误匹配压掉。这一步出来的是一堆“相对旋转”和“相对平移方向”。它们只代表形状不代表尺度先别急着拿这些数据去做任何跟米、秒有关的事。2.3 后续的位姿传播与三角化有了基准帧对的本质矩阵接下来就可以用类似PnP的方式逐渐恢复窗口内其余帧的位姿然后三角化出三维点。VINS-Mono这部分其实沿用了视觉里程计的经典处理管线先两两帧估计相对运动再用增量式PnP往外扩最后对窗口内的位姿和三维点做一次局部BA微调。这里有个工程上容易忽略的点三角化的深度恢复常常有浮点误差尤其是基线很短时。这会导致三维点深度方向的噪声被放大。一个通用做法是在BA中加一个逆深度参数化把深度的相对误差变小。VINS-Mono的滑动窗口优化里也类似逆深度比绝对深度好收敛得多。2.4 退化场景与单应矩阵的坑纯视觉几何有个著名的退化问题如果两帧之间只有纯旋转、或者场景近似平面比如对着白墙、桌面平移本质矩阵求解会退化甚至得到错误解。初始化阶段如果没处理好后面的惯性融合喂进去的视觉位姿骨架就是歪的系统从起点就输在了跑线上。因此这一阶段通常会有退化检测如果本质矩阵解出来的平移量接近零或者平面场景检测出单应矩阵更合理系统不会强行继续而是推迟初始化或者换帧对。这个检测逻辑非常关键我后面会结合实际调试过程再说一段。3. IMU预积分与陀螺仪偏置给视觉轨迹补上旋转参考3.1 预积分到底“预先”积了什么先说明一下为什么不能直接对惯性测量单元的原始加速度和角速度逐帧积分。你要是在每一帧都从上一帧的绝对状态开始重新积分那计算量会随窗口线性增长而且重复积分放大了数值误差。预积分的思路是把相邻两帧之间惯性测量单元数据产生的相对位移、相对速度、相对旋转提前算好作为“测量值”缓存起来跟全局绝对状态解耦。打个比方你坐火车从北京到上海车窗外的风景你在每一站都重新测量一次位置既浪费又难以保证一致性而预积分相当于你只在出发和到达时各记一次总位移中间怎么颠簸都折进一个整体观测里。具体在初始化里的应用是视觉SfM给出的是相对位姿预积分给的是相对惯性运动两者之间存在一串约束关系。初始化就是把这些约束关系组织起来求解。3.2 陀螺仪偏置估计靠旋转一致性反推这一步的数学目标非常干净。对窗口内任意相邻两帧视觉SfM给出了一个相对旋转惯性预积分也给出了一个相对旋转它依赖未知的陀螺仪偏置。真实世界只有一个旋转所以这两条渠道算出的相对旋转应当一致。把误差写成关于偏置的函数最小化所有帧对误差的平方和就得到了偏置的粗估计。这里有个让我当时琢磨了好一会儿的细节为什么用旋转就能稳定解偏置而平移暂时不能用原因在于旋转观测对偏置非常敏感。陀螺仪偏置直接影响角速度积分结果而角速度积分又是旋转的来源一丁点零偏积累几秒就会在旋转上显出明显漂移。视觉旋转没有这种累积所以两边一对比偏置就被“逼”出来了。3.3 为什么初始化阶段先放过加速度计偏置跟陀螺仪偏置不同加速度计偏置在初始化中通常被忽略或者只在后续紧耦合优化中一起估计。原因不是不能估计而是不好估计。加速度计偏置和重力在当前窗口的短期运动中是高度混淆的。传感器静止时加速度计读数是重力加偏置的合成系统在做匀加速运动时你很难区分到底是真实的线性加速度还是偏置引起的读数偏移。在初始化这么短的观测窗口里如果把加速度计偏置一起设成未知数大概率解出一堆互相纠缠的垃圾值。实际工程里加速度偏置一般假设很小属于小量相比之下重力和运动加速度是大量等后端优化用更长时间窗口再把偏置逐渐收敛出来。提示如果你在代码注释或者文档里看到“假设加速度计偏置在初始化阶段为零”别觉得这是草率这是权衡后的合理工程决策。4. 重力与尺度联合求解初始化的重头戏4.1 从预积分“位移”方程里凑出一个线性系统这是整条初始化流程里最核心、也最容易让初学者晕掉的一段。结论先行VINS-Mono把尺度、速度、重力方向全部塞进一个线性最小二乘框架里一次性解出来。怎么构造这个方程组我不打算把全部公式抄上来但可以把逻辑链路讲透。预积分给出了相邻两帧之间的相对速度变化、相对位移这些量由上一帧的速度、重力、加速度项组成。因为视觉SfM已经给出了位姿把每个时刻的相机位姿代进去剩下的未知量就是尺度因子、每一帧的速度、重力向量。对窗口内每一对相邻帧都列一个等式未知量堆成一个向量然后用最小二乘解算。这个过程中有一个非常典型的技巧把“尺度”和“重力”放在同一个方程里求解是因为它们在视觉惯性观测中彼此耦合。如果不联合估计先单独解尺度再单独解重力误差会很大。这种耦合本质上来自单目视觉缺尺度、惯性又缺重力方向两个缺陷正好互相补刀联合估计才能把两个量一起锁在正确值附近。4.2 重力细化解出方向之后还要“修模”线性最小二乘解出来的“重力向量”只是一个三维向量其模长通常不等于标准重力加速度值。而现实世界中重力加速度的数值是已知的模长大概在9.8左右。所以要做一步所谓重力细化。方法是把重力向量写成一个已知粗方向的标量乘积累加上一个扰动这个扰动被约束在垂直重力方向的小切平面上只有两个自由度。然后在这个参数化下重新求解再迭代。这一步会显著改善重力方向的精度进而改善整个系统在垂直方向的稳定性。我见过有人复现的时候跳过这一步理由是“反正后面紧耦合优化会修正”。实测结果就是初始化完成后轨迹在垂直分量上慢慢下沉越跑越偏。重力细化是很便宜的一步几十次迭代而已在该做的地方做掉后面能省一堆麻烦。4.3 速度恢复每个窗口时刻都补上初速度把尺度和重力确定之后窗口内每一帧的速度就是现成的了。因为预积分公式里只要知道初始速度、重力、每一帧的策略加速度就能直接算出来。速度初值直接影响惯性积分如果初始速度解错跳变点会在后端优化里表现为加速度突跳有时会被误认为传感器故障。这一步骤我自己的感觉是实现上没什么高深的东西但数必须要从前面那一个大方程组里带出来而不是重新另起炉灶再解一遍。VINS-Mono代码里顺序是先把重力、尺度、以及带速度的联合向量解出来再从这里直接切出各帧速度。理解了这个顺序看代码就不会乱。5. 坐标系切换与全局细化从视觉坐标系搬回惯性坐标系5.1 视觉坐标系和惯性坐标系之间的旋转初始化进行到这里视觉SfM给出的位姿还挂在纯视觉坐标系里惯性测量单元预积分数据又在惯性坐标系里。两者之间有一个固定的旋转和平移。这个外参在系统配置里一般有初始值初始化阶段要做的是在这个基础上修正或者至少确定可靠的旋转对齐。方法不复杂视觉轨迹提供了一系列旋转惯性预积分也提供了一系列旋转。在尺度无关的情况下两者的旋转序列应该差一个固定旋转这个固定旋转就是坐标系变换。所有相对旋转的加权平均对齐可以写成一个简单的最优化问题在实际实现中甚至可以利用四元数平均来解。5.2 尺度作用到整张视觉地图尺度因子解出来之后需要把视觉坐标系里的所有相机位置、三维点坐标全部乘以尺度因子转换到真实的物理单位下。这一操作要同步作用到位姿和地图点不然视觉和惯性数据会“错位”。这一步在工程上有一个容易漏掉的细节相机到惯性测量单元的平移向量也参与了尺度变换。很多人只记得给位姿乘尺度却忘了外参平移量也跟着换单位。如果这里没处理好系统前几十帧的重投影误差会莫名变大甚至直接触发异常检测。5.3 初始化成功与否怎么判VINS-Mono的初始化不是“跑一遍就一定成功”。它是带校验的如果某一阶段不收敛系统会放弃当前窗口、清空滑窗重新积累数据。所以看日志时如果发现窗口反复被清空别急着怀疑代码有bug大概率是运动激励不够或者数据质量太差。一些常见校验手段包括重投影误差是否小于阈值、尺度是否在一个合理范围、重力解算后的方向是否跟预期接近。还有一点系统会统计窗口内的视差大小视差太小直接拒绝进入初始化流程。这个机制导致了一个很常见的现象设备启动后先静止或者只做小幅晃动系统一直初始化失败反复“等数据”直到给了明显的平移运动才被唤醒。提示调试自己的机器时先做一段5到10秒的缓慢平移加一点旋转再观察日志确认初始化成功。很多“系统不工作”其实只是你没给它足够的初始化运动。6. 踩坑记录我复现初始化流程时遇到的典型问题6.1 静止或纯旋转开局初始化永远不成功这是新手最容易撞上的第一堵墙。我早期用某开发板测试时把设备放在桌上静止了十几秒才开始动往日志里一看初始化模块一直报告视差不足、本质矩阵退化窗口被反复丢弃。根本原因就是纯几何的固有限制没有平移就没有视差也就没有可靠的深度信息。纯旋转时对极几何是退化的连“方向”都解不准后面的惯性融合更是无源之水。解决办法谈不上什么高深技巧就是在工程上保证设备启动后有一个明确的平移分量不要指望系统能凭空恢复深度。6.2 特征点太少或被动态物体带偏另一个常见坑是初始化窗口内的特征点都被动态物体占了。比如对着人来人往的大厅初始化墙面、行人的特征点匹配一旦存在大量动态点本质矩阵的RANSAC就可能被带偏最后视觉骨架歪掉初始化出来尺度明显不对。定位方法也简单把初始化的视差信息、参与解算的帧对和匹配内点数量打印出来很容易看出某一段帧的特征点分布极端不平衡。解决思路是加特征点均匀化约束以及在视差统计阶段对特征点深度分布做一次过滤优先保留能够稳定观测到的静态点。6.3 初始化“成功”了但一进优化就发散更让人头疼的是日志显示初始化通过了姿态和位置也开始输出了结果跑个十几秒轨迹就朝着一个方向飘。我复盘过几次几乎都指向同一个来源陀螺仪偏置初始估计不准或者视觉SfM阶段出现帧对漂移。陀螺仪偏置不准时初始化阶段给不出可靠的旋转后续紧耦合优化里大量旋转残差会积累成系统性误差。这种问题往往在短时间测试里看着没问题时间一长就暴露。我的经验是碰到这种情况先看旋转残差序列是否存在明显的单调趋势如果有回头检查初始化阶段陀螺仪偏置求解的收敛条件和数据量是否充足。6.4 不同场景下的调参思路最后给点实际操作方面的思路。初始化相关参数主要集中在滑窗长度、帧间视差阈值、特征点数量、收敛判定阈值这几类。参数室内小场景倾向室外大场景倾向滑窗长度可以稍短视差更容易积累稍长避免单帧噪声过大视差阈值加大防止微小视差带偏适中保证远点参与约束特征点数量增加并强调均匀分布保持数量但过滤低视差特征收敛阈值收紧几何精度要求高放宽防止过于频繁失败不同场景的特性确实不一样。室内近处特征点多、视差大但逆深度噪声也大室外的远点可以稳住旋转但对平移估计贡献小。初始化阶段如果能根据场景特点调整参数成功率会有直观提升。VINS-Mono的初始化流程拆开看其实是五个相对独立的步骤纯视觉SfM打底、惯性预积分做桥、陀螺仪偏置对齐旋转、重力和尺度联合求解、坐标系切换收尾。每一段单独拿出来都不是特别复杂但错误会跨模块累积这也是为什么很多人直接从后端优化坑起永远查不出根因。我自己调试的习惯是先确认初始化是否稳定通过再看前端质量最后才碰后端权重。顺序反了多半白忙一场。
阅读完成 · 觉得有帮助？
