从零学习VINS-Mono/Fusion源代码（六）：后端优化
本节解析VINS后端优化，包括优化变量、残差约束的构建，ceres求解器等。
VINS-Mono/Fusion代码学习系列：
从零学习VINS-Mono/Fusion源代码（一）：主函数
从零学习VINS-Mono/Fusion源代码（二）：前端图像跟踪
从零学习VINS-Mono/Fusion源代码（三）：IMU预积分公式推导
从零学习VINS-Mono/Fusion源代码（四）：误差卡尔曼滤波
从零学习VINS-Mono/Fusion源代码（五）：VIO初始化
从零学习VINS-Mono/Fusion源代码（六）：后端优化
从零学习VINS-Mono/Fusion源代码（六）：后端优化
- 1 ceres自动求导与解析求导
- 2 后端优化流程
- 2.1 定义待优化的参数块
- 2.2 通过残差约束来添加残差块
- （1）IMU预积分约束
- （2）视觉重投影约束
- （3）边缘化先验
- 3 信息矩阵、FEJ、零空间漂移
- 3.1 信息矩阵
- 3.2 FEJ
- 3.3 零空间漂移
1 ceres自动求导与解析求导
-
VIO初始化利用ceres自动求导方式来构造loss_function，这种方法比较简单，只需要构造优化变量和残差求解式，不需要手动计算雅克比；缺点是求解速度较慢.
-
后端优化利用ceres解析求导，需要手动定义优化变量、残差及雅克比，通过这种方式提高计算速度，以满足里程计的实时性要求.
2 后端优化流程
2.1 定义待优化的参数块
problem.AddParameterBlock()
| 参数块 | 维数 | 代码命名 |
|---|---|---|
| 滑窗中的位姿 | 7 | para_Pose[WINDOW_SIZE + 1][SIZE_POSE] |
| 速度和零偏 | 9 | para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS] |
| 特征点深度 | 上限1000，维度1 | para_Feature[NUM_OF_F][SIZE_FEATURE] |
| 外参 | 7 | para_Ex_Pose[NUM_OF_CAM][SIZE_POSE] |
| 回环有关的位姿 | 7 | para_Retrive_Pose[SIZE_POSE] |
| 时间戳校正 | 1 | para_Td[1][1] |
| 没用到 | 1 | para_Tr[1][1] |
- 对于旋转，需要重新定义参数加法方式
- ceres参数块都是double数组类型，需要把初始化求解的eigen类型对象转为double，通过vector2double()实现
2.2 通过残差约束来添加残差块
残差约束：先验、IMU、视觉、回环
problem.AddResidualBlock()
（1）IMU预积分约束
由预积分公式引出，当位姿速度得到优化调整后，公式（5）就不再为等号. 同时，考虑到调整后的结果仍然不能过分偏离预积分，基于此，构造IMU残差.
其中，bias也得到了优化，利用雅克比矩阵，对预积分量进行补偿.
IMU预积分残差计算公式：
IMU预积分雅克比公式：
预积分残差r是一个15x1维的向量，状态量涉及位姿、速度和bias，需要对k和k+1时刻的para_Pose（7维）和para_SpeedBias（9维）求导，所以它的雅克比矩阵形式为（15x7, 15x9, 15x7, 15x9）
（2）视觉重投影约束
在第i帧和第j帧构建特征点的重投影误差，特征点以逆深度形式表示，本质上是把第i帧下的特征点转换到j帧下面去，跟观测到的归一化坐标作差，从而构建视觉重投影误差。
残差计算公式：
视觉残差雅克比：
重投影误差中涉及到的状态量有旋转外参Rbc，平移外参pbc，特征点逆深度，还有i和j两个时刻的位姿。采用链式求导的思想，依次计算残差对归一化坐标点的导数、归一化坐标点对状态量的导数。
（3）边缘化先验
对于滑窗中旧的信息，采用舒尔补的方式，边缘化掉旧的关键帧和地图点。
关于为什么要边缘化所有的地图点，是因为边缘化的过程会导致原本稀疏的矩阵变得稠密，不利于计算，所以vins边缘化所有的地图点，以减小矩阵的维度。
如下公式，我们要留下 δxb\delta {x_b}δxb，边缘化δxa\delta {x_a}δxa，所以要把左下方矩阵块化为0矩阵。
这里借用白巧克力亦唯心大佬的博客：https://blog.csdn.net/heyijia0327/article/details/52822104
本质上在求解 δxb\delta {x_b}δxb的过程中，保留了 δxa\delta {x_a}δxa中的部分约束信息。
3 信息矩阵、FEJ、零空间漂移
3.1 信息矩阵
通常认为信息矩阵等于协方差的逆，是对误差不确定性的一种衡量，在滑窗中用于对不同误差量统一量纲。
推导可以看这篇论文《Relationship between the Hessian and Covariance Matrix for Gaussian Random Variables》
需要注意的是，ceres求解器中并没有定义信息矩阵，所以vins代码在处理的过程中，对信息矩阵采用了LLT分解，再乘上雅克比矩阵。
3.2 FEJ
FEJ（First Estimated Jacobian）
在做边缘化之后，新的残差项和旧的信息矩阵，它们求解雅克比矩阵用的线性化点不同，会导致信息矩阵的零空间发生变化，可能导致零空间里的解变成了固定的一组解。
所以，不同残差对同一个状态求雅克比时，线性化点必须一致，避免零空间退化。
3.3 零空间漂移
VIO一般是四自由度不可观，yaw+xyz，roll和pitch能够通过重力确定绝对方向，尺度能够通过加速度计确定。所以在滑窗优化的过程中，整个滑窗图像帧可能会发生整体的漂移。
采取的方法通常有两种：
1、固定第一帧不优化，这样其他帧也就不会过分偏离第一帧
2、漂移之后，计算第一帧漂移了多少，把所有帧都移回去
VINS中采取的是第二种方法