从零手写VIO学习记录 ——系列五：【后端优化】（学习记录 | 作业 | 代码）

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

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

本节主要使用Eigen库手撕求解器，非线性最小二乘求解，滑动窗口算法实现。

非线性最小二乘求解：

1. 对上一节构建的VIO误差进行非线性最小二乘求解。流程如图：

详细参考：
1、Giorgio Grisetti et al. “A tutorial on graph-based SLAM”. In: IEEE Intelligent Transportation Systems Magazine 2.4
(2010), pp. 31–43.
2、Rainer Kümmerle et al. “g 2 o: A general framework for graph optimization”. In: 2011 IEEE International Conference on
Robotics and Automation. IEEE. 2011, pp. 3607–3613.
3、Manolis IA Lourakis and Antonis A Argyros. “SBA: A software package for generic sparse bundle adjustment”. In: ACM
Transactions on Mathematical Software (TOMS) 36.1 (2009), p.

2. 求解过程中的疑问：
   1. 信息矩阵H不满秩，求解时该如何操作？
      答： 使用LM算法，加阻尼因子使系统满秩。但是求得的结果可能会往零空间变化;
      添加先验约束，增加系统的可观性。
   2. ORBSLAM2，SVO等求单目BA问题时，fix一个相机pose和一个特征点，或者fix两个相机pose，也是为了限定优化值不乱飘。代码如何实现fix？
      答：添加超强先验，使得对应的信息矩阵巨大（如，$10^{1}5$，就能使得$\Delta x=0$）;
      设定对应雅可比矩阵为0，意味着残差等于0，求解方程为$(0+\lambda \mathbf{I}=0)$，只能$\Delta x=0$。

滑动窗口算法

1. 这一块核心主要时矩阵块的对应关系，以及如何拼接信息矩阵。
2. 大概滑窗步骤就是：（详细参考上一节从零手写VIO学习记录 | 作业 | 代码 | 深蓝学院VIO课程——系列四：VIO滑动窗口算法）
   1. 构建先验
      
   2. 先验+新观测信息=新的信息矩阵
      

本次作业主要使填充矩阵块运算及拼接信息矩阵：