从零手写VIO学习记录——系列三：【基于优化的IMU和视觉信息融合】（学习记录 | 作业 | 代码）

本系列主要根据深蓝学院《视觉SLAM进阶：从零开始手写VIO》。博客主要以本人在课程学习过程中的作业和修改的代码为主。如需详细视频及PPT，请联系深蓝学院相关人员。

代码已上传github: https://github.com/why-freedom/VIOLearning_Note_Code.git（有用话给个star >_<）

本节主要是VIO的残差构建及其雅可比推导，主要是VINS-Mono那一套，（其实本课程也都是基于VINS-Mono的）

视觉SLAM的BA：

• 状态量：特征点三维坐标、相机位姿
• 测量值：特征点在不同图像上的图像坐标
• 通过构建重投影误差，然后构建最小二乘最小化误差得到最优估计。

最小二乘求解：

• 最速下降法：梯度的负方向为最速下降方向。缺点：最优值附近震荡，收敛慢。适用于迭代开始阶段。
• 牛顿法：在局部最有点$x^{\ast }$附近，如果$x+\Delta x$是最优解，则损失函数对$\Delta x$的导数等于0。缺点：二阶导矩阵计算复杂。适用于最优值附近。
• 高斯牛顿法：
  

• Levenberg和Marquardt先后对高斯牛顿法改进，求解过程引入阻尼因子：
  

  • $\mu >0$，保证$（{\mathbf{J}}^T\mathbf{J}+\mu \mathbf{I}）$正定，迭代朝着下降方向进行。
  • $\mu$非常大，则接近最速下降法。
  • $\mu$非常小，则接近高斯牛顿法。
  • 阻尼因子策略一般是（$\tau$取$[10^{-8},1]$）：
    
  • 阻尼因子更新策略：参考：https://blog.csdn.net/boksic/article/details/79177055

• 最小二乘中遇到outlier怎么处理？引入鲁棒核函数，参考：https://zhuanlan.zhihu.com/p/62175983

VIO残差构建

• 视觉重投影误差：一个特征点在归一化相机坐标系下的估计值与观测值的差。
• IMU测量预积分误差：一段时间内IMU构建的预积分量作为测量值，对两时刻之间的状态两进行约束。参考：https://zhuanlan.zhihu.com/p/38009126
• 先验误差：主要是滑窗丢弃之前信息保留的状态约束。
• 雅可比推导：参照崔神的解析：https://github.com/StevenCui/VIO-Doc

本节作业主要是实现LM算法，并修改阻尼因子策略。推到两项雅可比。以及证明题：