1. 从零手写VIO后端求解器的整体设计思路1.1 为什么非要自己手写求解器很多刚接触VIOVisual-Inertial Odometry视觉惯性里程计的朋友学到后端优化这一块时第一反应是直接上Ceres或者g2o。这两个库确实成熟稳定调几个接口就能跑出一个像样的BABundle Adjustment结果。但如果你真的想搞明白VIO后端到底在干什么我强烈建议你至少手写一次求解器。原因很简单调库的时候你只需要关心“残差怎么定义”但手写的时候你必须把“残差怎么变成增量方程、增量方程怎么解、解出来的步长怎么更新状态”这一整条链路全部走通。我自己第一次手写的时候踩的最大的坑就是搞不清楚H矩阵和b向量到底是怎么从一个个残差块里拼出来的。看论文觉得“不就是J^T J和J^T r嘛”真到写代码的时候发现维度对不上、稀疏性没利用上、LM的lambda更新逻辑写反了各种问题全冒出来。所以这一节的核心目标很明确不依赖任何优化库从零实现一个能跑通VIO后端BA的高斯牛顿/LM求解器。这个求解器要能处理的核心问题就是给定一堆视觉重投影残差和IMU预积分残差构建一个大规模稀疏的非线性最小二乘问题然后迭代求解。听起来吓人但拆开来看无非就是几个矩阵运算加上一个线性方程组求解。1.2 求解器的整体架构拆解在动手写代码之前先把整个求解器的架构理清楚。我习惯把它分成四层残差层负责定义每一种残差的计算方式包括残差向量和雅可比矩阵。VIO后端里主要就是视觉重投影残差和IMU预积分残差两种。信息矩阵层每种残差都有自己的信息矩阵也就是权重的逆视觉残差通常用像素噪声的逆IMU残差用预积分协方差的逆。装配层把所有残差块的雅可比和信息矩阵按照状态变量的顺序装配成全局的H矩阵和b向量。这一步是手写求解器最容易出错的地方。求解层对H矩阵进行分解求解得到增量delta_x然后更新状态变量。这里涉及到稀疏矩阵的存储和分解方式的选择。这四层里装配层和求解层是手写求解器的核心难点。残差层其实在之前的几节里已经写过了信息矩阵层也比较直接但装配层涉及到大量的索引计算和稀疏性处理求解层则涉及到线性代数库的选型和参数调优。提示如果你之前没有写过任何优化相关的代码建议先用一个小规模的曲线拟合问题练手把高斯牛顿的流程走通一遍再来写VIO后端。直接上手VIO后端很容易被维度问题搞晕。1.3 状态变量的组织方式VIO后端的状态变量通常包括滑动窗口内所有关键帧的位姿位置旋转、速度、IMU零偏以及所有路标点的逆深度。假设滑动窗口有N个关键帧M个路标点那么状态向量的维度大概是每个关键帧位置3 旋转3用so3扰动 速度3 加速度零偏3 陀螺仪零偏3 15维每个路标点逆深度1维总维度 15N M。如果N10M200那就是350维。这个规模不算大但H矩阵的稀疏结构非常明显——每个视觉残差只连接一个关键帧和一个路标点每个IMU残差只连接相邻两个关键帧。我自己的做法是把状态变量分成两部分位姿部分包括速度零偏和路标点部分。这样在装配H矩阵的时候可以分块处理也方便后面利用Schur补进行边缘化。这个分块思路在后面求解的时候会带来很大的便利因为路标点部分的对角块结构非常规整求逆很快。2. 残差块与雅可比矩阵的逐行实现2.1 视觉重投影残差的雅可比推导视觉重投影残差是最直观的一种残差。假设第i个关键帧观测到了第j个路标点观测到的像素坐标是(u, v)那么残差就是r_visual [u_obs - u_pred, v_obs - v_pred]其中u_pred和v_pred是通过位姿和路标点投影计算出来的。这个残差对位姿和路标点的雅可比需要分两步推导先对相机坐标系下的点求导再对位姿和路标点求导。具体来说假设路标点在相机坐标系下的坐标是P_c [X, Y, Z]那么投影过程是u_pred fx * X / Z cx v_pred fy * Y / Z cy对P_c的雅可比是dr/dP_c [-fx/Z, 0, fx*X/Z^2; 0, -fy/Z, fy*Y/Z^2]然后P_c对位姿的雅可比涉及到旋转矩阵的扰动模型。这里我建议用左乘扰动的方式推导起来比较清晰。P_c对路标点逆深度的雅可比也需要单独推导因为逆深度是路标点的参数化方式。注意视觉残差的雅可比推导里最容易出错的是符号问题。特别是旋转扰动那块左乘和右乘的雅可比形式不一样一定要在代码里注释清楚用的是哪种。2.2 IMU预积分残差的雅可比结构IMU预积分残差比视觉残差复杂得多因为它涉及到相邻两帧之间的所有IMU测量。预积分残差通常包括位置残差、速度残差、旋转残差以及零偏的随机游走残差。每一项残差对两个关键帧的位姿、速度、零偏都有雅可比。我自己的经验是IMU预积分的雅可比推导虽然长但结构很规整。位置残差对第一个关键帧的位置雅可比是负的单位矩阵对第二个关键帧的位置雅可比是正的单位矩阵乘以旋转矩阵的转置。旋转残差的雅可比涉及到so3的右雅可比这个在之前的预积分章节里应该已经推导过了。写代码的时候我建议把IMU残差的雅可比单独封装成一个函数输入是两个关键帧的状态和预积分量输出是残差向量和雅可比矩阵。这样在装配层调用的时候只需要关心索引不需要关心内部细节。2.3 信息矩阵的构建与权重分配信息矩阵决定了每种残差在优化中的权重。视觉残差的信息矩阵通常是像素噪声协方差的逆一般取一个对角矩阵对角线元素是1/sigma_pixel^2。IMU残差的信息矩阵是预积分协方差的逆这个协方差是通过预积分过程中的递推计算出来的。这里有一个实操中的小技巧信息矩阵不要直接求逆而是用Cholesky分解或者LDLT分解来解。因为预积分协方差矩阵可能接近奇异直接求逆数值不稳定。我一般用Eigen的ldlt()来解。另外视觉残差的信息矩阵可以根据特征点的跟踪质量动态调整。比如跟踪时间长的特征点给更高的权重新加入的特征点给低一点的权重。这个在实际系统中对鲁棒性提升很明显。3. 全局H矩阵的装配与稀疏性处理3.1 状态变量索引管理装配H矩阵的第一步是给每个状态变量分配索引。我通常用一个哈希表或者结构体数组来管理每个关键帧的位姿、速度、零偏分别占哪些维度每个路标点的逆深度占哪个维度。这里有一个容易忽略的点状态变量的顺序会影响H矩阵的稀疏结构。如果把位姿和路标点混在一起排H矩阵的带宽会变大分解效率降低。我一般把位姿部分排在前面路标点部分排在后面这样H矩阵呈现明显的块对角加边结构。索引管理我建议写一个专门的类提供getPoseIndex(frame_id)、getLandmarkIndex(landmark_id)这样的接口。每次装配残差块的时候通过这个类查询索引然后往H矩阵的对应位置累加。3.2 残差块到全局矩阵的映射每个残差块贡献的雅可比是一个小矩阵需要按照索引映射到全局H矩阵的对应位置。具体来说对于残差块r如果它涉及到状态变量x_i和x_j那么它贡献的H块是H_ii J_i^T * Omega * J_i H_ij J_i^T * Omega * J_j H_ji J_j^T * Omega * J_i H_jj J_j^T * Omega * J_j b_i J_i^T * Omega * r b_j J_j^T * Omega * r这个过程看起来简单但实际写代码的时候索引计算非常容易出错。我的做法是写一个通用的accumulateHessian函数输入是残差块涉及的变量索引列表、雅可比列表、信息矩阵和残差向量输出是往全局H和b的累加。这样每种残差只需要调用这个函数不需要重复写累加逻辑。提示装配完成后建议打印一下H矩阵的稀疏图案确认和预期的结构一致。我遇到过因为索引写错导致H矩阵出现异常非零块的情况通过稀疏图案一眼就能看出来。3.3 利用稀疏性加速装配VIO后端的H矩阵稀疏性非常强如果直接用稠密矩阵存储内存和计算都会浪费。我一般用Eigen的SparseMatrix或者自己实现一个块稀疏的存储结构。Eigen的SparseMatrix用起来方便但装配的时候需要先统计非零块的位置再逐个插入。如果不想这么麻烦可以用triplet列表的方式先把所有非零元素收集起来最后一次性构建。这种方式在装配阶段会慢一点但代码简单不容易出错。如果追求极致性能可以自己实现一个块稀疏矩阵每个非零块是一个固定大小的矩阵。VIO后端里位姿-位姿块是15x15位姿-路标块是15x1路标-路标块是1x1。这种固定块大小的结构非常适合块稀疏存储。4. 线性方程组求解与LM算法实现4.1 高斯牛顿与LM的取舍高斯牛顿法在残差接近零的时候收敛很快但在初始值不好的时候容易发散。LM算法通过引入阻尼因子lambda在高斯牛顿和梯度下降之间切换鲁棒性更好。VIO后端的初始值通常不会太差因为有前端提供但为了保险起见我还是推荐用LM。LM的核心是在H矩阵的对角线上加上lambda(H lambda * I) * delta_x -blambda的更新策略是如果这次迭代残差下降了就减小lambda如果残差上升了就增大lambda并重新求解。这个逻辑写起来简单但有一个细节lambda的初始值和增减系数需要根据问题规模调整。我一般初始lambda取1e-4减小系数取0.1增大系数取10。4.2 线性方程组的求解方法选择H矩阵是对称正定的在LM的阻尼下所以可以用Cholesky分解。Eigen提供了SimplicialLDLT和SimplicialLLT两种稀疏分解前者更稳定后者更快。我一般用LDLT因为VIO后端的H矩阵有时候会接近奇异LLT容易失败。如果状态维度比较大直接分解整个H矩阵会比较慢。这时候可以用Schur补先边缘化路标点得到一个只包含位姿的缩减矩阵再对这个缩减矩阵做分解。Schur补的公式是H_reduced H_pp - H_pl * H_ll^{-1} * H_lp b_reduced b_p - H_pl * H_ll^{-1} * b_l其中H_ll是路标点部分的对角块求逆很快。这个操作可以大幅降低分解的维度是VIO后端加速的关键。4.3 状态更新与迭代终止条件求解出delta_x之后需要更新状态变量。位姿的更新要注意旋转部分不能用简单的加法要用so3的指数映射。我一般把delta_x里的旋转部分取出来构造一个so3的增量然后左乘到原来的旋转矩阵上。迭代终止条件通常有三个残差变化量小于阈值、delta_x的范数小于阈值、达到最大迭代次数。我一般设最大迭代次数为10残差变化阈值1e-6delta_x阈值1e-8。实际跑下来通常5次以内就收敛了。注意更新完状态之后一定要重新计算残差确认残差确实下降了。如果残差没有下降说明雅可比或者信息矩阵有问题需要回头检查。5. 实操中遇到的典型问题与排查技巧5.1 H矩阵维度不匹配的排查维度不匹配是手写求解器最常见的问题。表现是装配的时候Eigen报错或者求解出来的delta_x长度不对。排查思路是先打印每个残差块涉及的变量索引和雅可比维度确认和状态变量的维度定义一致。我遇到过因为路标点索引从1开始而状态变量从0开始导致整体偏移一位的情况。另一个常见原因是滑动窗口边缘化之后状态变量的索引没有更新。边缘化会移除一些关键帧和路标点剩下的变量索引需要重新映射。如果忘了这一步装配的时候就会访问到已经移除的变量。5.2 求解不收敛的常见原因求解不收敛的表现是残差震荡或者发散。我总结了几种常见原因问题现象可能原因排查方法残差震荡lambda更新逻辑写反检查残差上升时lambda是否增大残差发散雅可比符号错误用数值求导对比解析雅可比收敛很慢信息矩阵权重不对检查视觉和IMU残差的权重比例迭代后残差不变状态更新没生效检查delta_x是否被正确应用其中雅可比符号错误是最隐蔽的因为残差可能还是会下降只是收敛速度变慢。我一般会用数值求导来验证解析雅可比对每个状态变量加一个小扰动计算残差变化和解析雅可比对比。如果误差在1e-6以内说明解析雅可比是对的。5.3 数值稳定性的处理技巧VIO后端的H矩阵有时候会接近奇异特别是当某个路标点只被一个关键帧观测到的时候。这时候LDLT分解可能会失败或者求解出来的delta_x非常大。我的处理方式是在LM的阻尼基础上再加一个小的正则项。具体来说在H矩阵的对角线上加一个很小的常数比如1e-8。这个正则项不会影响收敛结果但能保证H矩阵正定。另外信息矩阵的构建也要注意数值稳定性。预积分协方差矩阵如果接近奇异求逆之前先做一次特征值分解把太小的特征值截断。这个操作在实际系统中对鲁棒性提升很明显。5.4 性能优化的几个实用手段手写求解器跑起来之后如果发现太慢可以从这几个方面优化利用稀疏性确保H矩阵用稀疏存储分解用稀疏分解。稠密分解在状态维度超过100之后会明显变慢。Schur补边缘化先边缘化路标点缩减矩阵维度。这个操作通常能减少一半以上的分解时间。雅可比计算并行化每个残差块的雅可比计算是独立的可以用OpenMP并行。我实测下来4核并行能提速2-3倍。避免重复计算比如视觉残差里的投影计算多个残差块可能用到同一个路标点的投影可以缓存起来。提示优化之前先用profiler定位瓶颈。我遇到过花大力气优化雅可比计算结果发现瓶颈在H矩阵分解的情况。6. 从作业到实际系统的扩展思路6.1 边缘化与先验信息的引入作业版本的求解器通常只处理滑动窗口内的残差没有考虑边缘化移除的状态变量带来的先验信息。实际系统中边缘化是必须的否则滑动窗口一移动之前的信息就丢了。边缘化的核心是把要移除的状态变量对应的H块通过Schur补转换成对剩余状态变量的先验约束。这个先验约束以H和b的形式加到下一次优化的H矩阵和b向量里。实现的时候要注意先验信息的H矩阵是稠密的会破坏原有的稀疏结构需要特殊处理。6.2 鲁棒核函数的接入实际系统中视觉残差经常会有外点直接用二次核函数会导致优化结果被外点带偏。这时候需要接入鲁棒核函数比如Huber核或者Cauchy核。鲁棒核函数的接入方式是在信息矩阵上乘以一个权重这个权重是残差的函数。残差大的时候权重小残差小的时候权重大。实现的时候只需要在计算信息矩阵的地方多乘一个标量权重即可改动很小。6.3 多传感器融合的扩展手写求解器的框架其实不限于视觉和IMU。如果要接入轮速计或者GPS只需要定义新的残差块和对应的雅可比然后按照同样的方式装配到H矩阵里。这也是手写求解器的一个优势扩展性完全可控不像调库那样受限于库的接口。我在实际项目中接过轮速计残差实现起来就是多了一个残差块装配逻辑完全复用。唯一需要注意的是不同传感器的信息矩阵量纲不一样需要做归一化否则优化会被某个传感器主导。6.4 代码组织与调试建议最后分享几个代码组织上的经验。我一般把求解器分成这几个文件residual_visual.h/cpp视觉残差residual_imu.h/cppIMU残差solver.h/cpp装配和求解state.h/cpp状态变量管理调试的时候我习惯先单独测试每种残差的雅可比确认无误后再测试装配最后测试整体求解。这样出问题的时候能快速定位到是哪一层的问题。另外建议写一个简单的可视化工具把每次迭代的残差变化、lambda变化、delta_x范数画出来。这些曲线能直观反映求解器的收敛行为比看日志高效得多。我个人在实际操作中的体会是手写求解器最大的价值不在于性能而在于你对整个优化流程的理解会有一个质的飞跃。调库的时候遇到问题只能猜手写的时候遇到问题可以一步步排查这种掌控感是调库给不了的。而且一旦你手写过一个完整的求解器再回头看Ceres或者g2o的源码会发现很多设计细节豁然开朗。