VINS-MONO理论学习---紧耦合后端非线性优化

基于滑动窗的紧耦合后端非线性优化：将视觉约束、IMU约束、闭环约束放到一个大的目标函数中进行非线性优化，求解出滑动窗口中所有帧的PVQ、bias等。

在视觉约束和IMU约束中，基本思想是找到优化状态向量，然后通过视觉残差和IMU测量残差分别对状态向量求导，获得视觉和IMU预积分的Jacobian和协方差矩阵。

一、VIO残差函数的构建

1、需要优化的状态向量：
状态向量包括滑动窗口内的所有相机状态（位置P、旋转Q、速度V、加速度偏置ba、陀螺仪偏置bw）、相机到IMU的外参、所有3D点的逆深度：

第一个式子是滑动窗口内所有状态量，n是关键帧数量，m是滑动窗内所有观测到的路标点总数，维度是15*n+6+m。特征点逆深度为了满足高斯系统。

第二个式子xk是在第k帧图像捕获到的IMU状态，包括位置，速度，旋转（PVQ）和加速度偏置，陀螺仪偏置。

第三个式子是相机外参。

注意：xk只与IMU项和Marg有关；特征点深度也只与camera和Marg有关；

2、目标函数为：

视觉惯性BA：这三项分别为边缘化的先验信息、IMU的测量残差、视觉的重投影误差

BA优化模型分为三部分：

1、Marg边缘化残差部分（滑动窗口中去掉位姿和特征点约束）代码中使用Google开源的Ceres solver解决。

2、IMU残差部分（滑动窗口中相邻帧间的IMU产生）

3、视觉误差函数部分（滑动窗口中特征点在相机下视觉重投影残差）

二、视觉约束

这部分要拟合的目标可以通过重投影误差约束，求解的是对同一个路标点的观测值和估计值之间的误差，注意是在归一化平面上表示。（视觉测量残差就是重投影误差，定义为一个特征点在归一化相机坐标系下的估计值与观测值的差。估计值即特征点的三维空间坐标(x,y,z)^T ，观测值为其在相机归一化平面的坐标。）

1.视觉重投影误差residual（归一化平面）

相关自变量是：该路标点被两帧观察到的相机帧位姿、Cam和IMU之间相对位姿、路标点在第一帧的逆深度。

当某路标点在第i帧观测到并进行初始化操作得到路标点逆深度，当其在第j帧也被观测到时，估计其在第j帧中的坐标为：

此时的视觉残差为：（左侧为根据i帧反推估计的位置，右侧为观测值）

逆深度参数化

逆深度作为参数原因：1）观测到的特征点深度可能非常大，难以进行优化；2）可以减少实际优化的参数变量；3）逆深度更加服从高斯分布。这里特征点的逆深度在第i帧初始化操作中得到。

以上是对针孔相机模型的视觉残差，而在VINS论文中其实显示的是对一般相机模型的视觉残差，即使用了广角相机的球面模型。用的是单位半球体的相机观测残差。是一个鱼眼