1. 视觉惯性里程计后端优化到底在解决什么问题视觉惯性里程计VIO走到第六章基本上就进入了整个系统最核心也最硬核的部分——后端优化。前面的章节你可能已经搭好了前端的光流跟踪、IMU预积分、初始化流程但那些更多是在“搭架子”真正决定整个系统精度和鲁棒性的是后端如何把视觉观测和惯性观测融合在一起做联合优化。我先说清楚这一章要干什么。简单讲VIO后端要解决的是一个最大后验估计问题给定一串IMU测量和若干帧图像特征观测估计出每一时刻的位姿、速度、零偏以及路标点的三维坐标。这个问题的数学本质是一个非线性最小二乘通常用滑动窗口的方式控制计算量窗口内做联合优化窗口外做边缘化。为什么后端这么重要因为前端光流跟踪会漂IMU积分会飘单独拿出来都撑不住长时间运行。后端的作用就是把这些有噪声的观测放在一个统一的目标函数里通过迭代优化让所有状态量互相“拉扯”到一个自洽的解。你可以理解为前端负责提供线索后端负责当法官把所有线索综合起来给出最合理的判决。这一章的作业通常要求你手写一个基于滑动窗口的VIO后端包括构建信息矩阵、求解增量方程、处理边缘化先验。听起来吓人但拆开来看就是几个模块的拼接。下面我按实际写代码的顺序把每个模块的设计思路、公式推导和实现细节都过一遍。1.1 为什么选择滑动窗口而不是全局优化全局BA当然精度最高但计算量随时间线性增长跑几十秒就卡死了。滑动窗口的思路是只保留最近N帧的状态老的状态通过边缘化转成先验信息塞进信息矩阵里。这样计算量恒定同时保留了历史信息对当前估计的约束。这里有个关键取舍窗口越大精度越好但越慢窗口越小越快但容易丢信息。实际工程里通常取10到20帧具体看你的算力和场景动态程度。我自己的经验是室内低速场景10帧够用室外高速或者剧烈运动建议15帧以上。边缘化的数学操作是Schur补。把状态向量分成两部分要保留的当前窗口内的状态和要丢弃的滑出窗口的老状态。对信息矩阵做Schur补就能把老状态的影响“凝聚”成一个先验项加到保留状态上。这个操作会引入填充fill-in让信息矩阵变稠密这是滑动窗口不可避免的代价。注意边缘化时一定要保证被边缘化的状态和保留状态之间的约束被正确传递否则会丢失信息导致估计不一致。很多初学者在这里出错表现为优化结果逐渐漂移甚至发散。2. 状态向量定义与信息矩阵构建的完整推导2.1 状态向量到底包含哪些量在VIO后端里状态向量通常定义为$$ \mathbf{x} [\mathbf{x}_0, \mathbf{x}_1, \dots, \mathbf{x}_N, \lambda_0, \lambda_1, \dots, \lambda_M] $$其中每个IMU状态 $\mathbf{x}_k$ 包含$$ \mathbf{x}_k [\mathbf{p}_k, \mathbf{q}_k, \mathbf{v}_k, \mathbf{b}_a, \mathbf{b}_g] $$分别是位置、旋转四元数、速度、加速度计零偏、陀螺仪零偏。路标点用逆深度 $\lambda$ 参数化这样数值上更稳定尤其是远距离点。为什么用逆深度而不是深度因为逆深度的不确定性更接近高斯分布优化时收敛更好。远距离点的深度可能从10米到无穷远用深度参数化的话初值很难给用逆深度的话初值给0就代表无穷远很自然。零偏为什么放在状态里估计而不是提前标定因为零偏会随温度和时间缓慢变化在线估计能实时补偿。但零偏也不能频繁大幅变化所以要在优化里加随机游走约束限制相邻时刻零偏的变化量。2.2 信息矩阵的块结构信息矩阵 $\mathbf{H}$ 是整个优化问题的核心它的结构直接决定了求解效率。对于VIO问题$\mathbf{H}$ 天然具有稀疏块结构对角线上是每个状态块的自身信息IMU约束连接相邻两帧的IMU状态视觉约束连接观测帧的IMU状态和对应的路标点这个稀疏性非常重要因为我们可以用Schur补先消去路标点得到一个只关于IMU状态的缩减矩阵然后再求解。这就是所谓的“边缘化路标点”能把求解规模从 (N×15 M) 降到 N×15M通常远大于N所以加速非常明显。具体操作是把 $\mathbf{H}$ 按IMU状态和路标点分块$$ \mathbf{H} \begin{bmatrix} \mathbf{H}{xx} \mathbf{H}{x\lambda} \ \mathbf{H}{\lambda x} \mathbf{H}{\lambda\lambda} \end{bmatrix} $$然后对 $\mathbf{H}_{\lambda\lambda}$ 求逆做Schur补$$ \mathbf{H}{xx} \mathbf{H}{xx} - \mathbf{H}{x\lambda} \mathbf{H}{\lambda\lambda}^{-1} \mathbf{H}_{\lambda x} $$右端项也做相应变换。由于 $\mathbf{H}_{\lambda\lambda}$ 是块对角矩阵每个路标点只和观测到它的帧有关求逆可以逐块进行效率很高。实操心得构建 $\mathbf{H}$ 矩阵时一定要用稀疏矩阵存储否则内存和计算都会爆炸。我一般用Eigen的SparseMatrix填充时用triplet列表最后统一构建比逐个insert快很多。3. IMU预积分与视觉重投影的残差构建3.1 IMU预积分的残差怎么算IMU预积分的核心思想是在两个关键帧之间把IMU测量积分成相对运动约束这样优化时改变关键帧状态不需要重新积分。预积分的残差定义为$$ \mathbf{r}_{\text{IMU}} \begin{bmatrix} \mathbf{r}_p \ \mathbf{r}q \ \mathbf{r}v \ \mathbf{r}{ba} \ \mathbf{r}{bg} \end{bmatrix} $$其中位置残差$$ \mathbf{r}_p \mathbf{q}_i^{-1}(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}i \Delta t \frac{1}{2}\mathbf{g}\Delta t^2) - \hat{\alpha}{ij} $$速度残差$$ \mathbf{r}_v \mathbf{q}_i^{-1}(\mathbf{v}_j - \mathbf{v}i \mathbf{g}\Delta t) - \hat{\beta}{ij} $$旋转残差$$ \mathbf{r}q 2[\hat{\gamma}{ij}^{-1} \otimes \mathbf{q}_i^{-1} \otimes \mathbf{q}j]{xyz} $$零偏残差就是相邻时刻零偏之差。这些残差对状态量的雅可比需要仔细推导尤其是旋转部分涉及四元数的扰动模型。我建议在纸上先推一遍再写代码否则很容易搞错符号。3.2 视觉重投影残差与鲁棒核函数视觉残差是路标点在归一化平面上的重投影误差$$ \mathbf{r}{\text{vis}} \mathbf{z}{ij} - \pi(\mathbf{q}_i^{-1}(\mathbf{p}_j - \mathbf{p}_i)) $$其中 $\mathbf{z}_{ij}$ 是归一化平面观测$\pi$ 是投影函数。这个残差对位姿和路标点的雅可比是标准推导注意链式法则别漏项。实际中一定要加鲁棒核函数比如Huber核来抑制外点的影响。外点可能来自误匹配或者动态物体不加核函数的话一个外点就能把整个优化带偏。Huber核的阈值一般取1到2个像素对应的归一化坐标值。注意视觉残差的信息矩阵要除以观测噪声方差这个方差通常假设为1到2个像素。如果实际跟踪精度差可以适当放大噪声让视觉约束权重降低。4. 增量方程求解与边缘化先验的处理4.1 用LM算法还是高斯牛顿增量方程 $\mathbf{H}\Delta\mathbf{x} \mathbf{b}$ 的求解我推荐用Levenberg-MarquardtLM而不是纯高斯牛顿。原因很简单VIO问题的初值不一定好纯高斯牛顿容易发散LM通过调节阻尼因子能在初值差的时候更稳。LM的阻尼因子更新策略如果这次迭代残差下降了就减小阻尼更接近高斯牛顿如果残差上升了就增大阻尼更接近梯度下降。实际实现时初始阻尼取1e-4左右下降因子0.1上升因子10。求解器方面因为 $\mathbf{H}$ 是稀疏的可以用Cholesky分解。Eigen的SimplicialLDLT对稀疏对称正定矩阵很合适。如果矩阵接近奇异可以加一个小的对角正则项。4.2 边缘化先验的构建细节边缘化是滑动窗口里最容易出错的地方。具体操作步骤确定要边缘化的状态通常是窗口里最老的那一帧IMU状态以及只被这一帧观测到的路标点。构建完整的 $\mathbf{H}$ 和 $\mathbf{b}$包括所有约束。把 $\mathbf{H}$ 按保留状态和边缘化状态分块。对边缘化状态做Schur补得到先验信息矩阵 $\mathbf{H}{\text{prior}}$ 和先验向量 $\mathbf{b}{\text{prior}}$。把先验加到下一次优化的 $\mathbf{H}$ 和 $\mathbf{b}$ 里。这里有个关键点先验的雅可比是在上一次线性化点处计算的下次优化时线性化点变了先验需要做一阶近似修正。具体做法是用一阶泰勒展开把先验残差从旧线性化点搬到新线性化点。这个操作叫“先验的fejFirst Estimate Jacobian”处理不做的话会导致估计不一致。实操心得边缘化时一定要检查 $\mathbf{H}_{\lambda\lambda}$ 是否可逆。如果某个路标点只被一帧观测到它的信息矩阵可能是奇异的需要加一个小的正则项或者直接丢弃这个路标点。5. 常见问题排查与调试技巧实录5.1 优化不收敛或者发散怎么办这是最常见的问题排查顺序如下问题现象可能原因排查方法残差震荡不下降雅可比符号错误用数值雅可比对比解析雅可比优化后位姿跳变边缘化先验错误关闭边缘化测试是否正常逐渐漂移零偏估计发散检查零偏随机游走约束权重直接发散初值太差或外点太多加鲁棒核检查初始化数值雅可比对比是我最常用的调试手段。写一个函数用有限差分算雅可比和解析雅可比逐项对比误差应该在1e-6以内。如果某项差很多基本就是推导错了。5.2 边缘化导致的填充问题边缘化会让信息矩阵变稠密尤其是路标点被边缘化后原本不相关的IMU状态之间会出现新的约束。这个填充会随时间累积最终让矩阵变得很稠密求解变慢。缓解方法有几个一是控制窗口大小别太大二是边缘化时尽量选择观测少的帧三是可以用一些稀疏化技巧比如只保留先验的块对角部分。但稀疏化会损失信息需要权衡。我自己的做法是窗口取10帧边缘化时把只被老帧观测到的路标点一起边缘化掉这样填充可控。实测下来跑几分钟矩阵稀疏度还能接受。5.3 时间同步与标定误差的影响VIO对时间同步很敏感IMU和相机的时间戳差1毫秒就可能引入明显误差。如果发现优化结果有系统性偏移先检查时间同步。可以在状态里加一个时间偏移量在线估计但会增加复杂度。外参标定误差也会影响精度尤其是相机和IMU之间的旋转。如果外参不准视觉和惯性约束会互相矛盾优化结果会折中表现为精度下降但不发散。建议先用标定工具把外参标准再跑VIO。注意在线估计外参虽然方便但和外参、零偏、速度等状态耦合严重容易收敛到局部最优。除非标定条件很差否则建议离线标定好固定住。6. 代码实现中的关键细节与性能优化6.1 稀疏矩阵的构建与求解Eigen的SparseMatrix构建时用triplet列表是最快的std::vectorEigen::Tripletdouble triplets; triplets.reserve(estimated_nonzeros); // 填充triplets for (int i 0; i num_blocks; i) { // 添加块 } Eigen::SparseMatrixdouble H(num_params, num_params); H.setFromTriplets(triplets.begin(), triplets.end());求解用Eigen::SimplicialLDLTEigen::SparseMatrixdouble solver; solver.compute(H); Eigen::VectorXd dx solver.solve(b);如果H接近奇异可以加对角正则H.diagonal().array() 1e-8;6.2 雅可比的解析推导与验证每个残差块的雅可比都要手推。以视觉残差为例对位姿的雅可比需要用到链式法则$$ \frac{\partial \mathbf{r}}{\partial \delta \xi} \frac{\partial \mathbf{r}}{\partial \mathbf{p}} \frac{\partial \mathbf{p}}{\partial \delta \xi} $$其中 $\mathbf{p}$ 是相机坐标系下的点。这部分推导要仔细符号很容易错。推完后用数值雅可比验证确认无误再集成。6.3 滑动窗口的状态管理窗口管理包括新帧进来时把新状态加入窗口老帧滑出时触发边缘化。状态在内存里的排列要固定方便索引。我一般用一个结构体数组存IMU状态路标点用哈希表存键是特征ID。新帧加入时要用IMU预积分把新帧和上一帧之间的约束建好。如果新帧和上一帧之间没有IMU数据要插值或者等待。实际中IMU频率远高于相机一般不会缺。7. 实测效果与精度评估方法7.1 用什么指标评估VIO精度常用指标是ATE绝对轨迹误差和RPE相对位姿误差。ATE衡量全局一致性RPE衡量局部精度。计算时先把估计轨迹和真值轨迹对齐通常用Umeyama算法做相似变换对齐然后算均方根误差。如果没有真值可以用闭环检测或者回环误差来间接评估。也可以把VIO估计的轨迹和纯视觉、纯惯性的结果对比看融合是否带来提升。7.2 不同场景下的表现差异室内低速场景VIO通常能跑到厘米级精度因为特征丰富、运动平缓。室外高速场景特征跟踪容易丢IMU积分误差累积快精度会下降到分米级甚至米级。剧烈运动比如快速旋转陀螺仪量程可能饱和零偏估计也会变差。我实测下来在走廊场景跑100米ATE大概在0.1到0.3米之间取决于纹理和光照。如果地面纹理少视觉约束弱精度会明显下降。这时候可以适当增大IMU权重但也不能太大否则视觉就白加了。7.3 参数调优的经验关键参数就几个窗口大小、鲁棒核阈值、IMU噪声、视觉噪声、零偏随机游走噪声。调参顺序建议先调噪声参数让残差量级合理再调鲁棒核阈值抑制外点最后调窗口大小平衡精度和速度。IMU噪声参数可以从数据手册查但实际值往往偏大因为还有未建模的误差。我一般把手册值乘以2到5倍作为初值然后根据优化结果微调。视觉噪声取1到2像素比较稳。实操心得调参时一次只改一个参数改完跑同一段数据对比ATE。同时改多个参数的话根本不知道是哪个起了作用。记录每次实验的参数和结果形成表格方便回溯。8. 从作业到工程的差距在哪里作业通常只要求跑通一个仿真或者小数据集但工程落地要考虑的东西多得多。首先是鲁棒性作业数据干净实际数据有各种异常图像模糊、IMU饱和、时间戳跳变。这些都要有异常检测和处理机制。其次是计算资源作业在PC上跑工程可能跑在嵌入式平台上算力有限。这时候要优化矩阵求解比如用定点数、降低窗口大小、减少迭代次数。还要考虑内存占用稀疏矩阵的存储也要优化。最后是长期稳定性作业跑几十秒工程要跑几小时。零偏的缓慢变化、外参的温漂、地图的累积误差这些在长时间运行中都会暴露。需要设计在线标定和零偏重估计机制。我个人体会是把作业跑通只是入门真正难的是让它在各种条件下都稳定工作。建议做完作业后找一些有挑战性的公开数据集跑一跑看看在快速运动、弱纹理、光照变化下的表现这样能更快发现自己的实现有哪些不足。后续可以尝试加入回环检测和全局优化把滑动窗口的漂移进一步消除形成一个完整的SLAM系统。