七、VINS-mono 代码解析——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化
文章目录
前言
本文主要对紧耦合后端非线性优化的理论进行了详细的推导。主要借鉴了{VINS-Mono理论学习——后端非线性优化，VINS-Mono 理论详细解读——紧耦合后端非线性优化 IMU+视觉的残差residual、Jacobian、协方差、基于舒尔补的边缘化}，
并参考了崔博的《VINS论文推导与代码解析》、深蓝学院的VIO课程内容。主要想对目标函数中视觉残差和IMU残差，以及对应的雅可比、协方差进行推导。}等资源。
紧耦合后端非线性优化系统框架
一、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、视觉误差函数部分（滑动窗口中特征点在相机下视觉重投影残差）
二、视觉约束
这部分要拟合的目标可以通过重投影误差约束，求解的是对同一个路标点的观测值和估计值之间的误差，注意是在归一化平面上表示。
1.视觉重投影误差residual
注意：（归一化平面）
相关自变量是：该路标点被两帧观察到的相机帧位姿、Cam和IMU之间相对位姿、路标点在第一帧的逆深度。
当某路标点在第i帧观测到并进行初始化操作得到路标点逆深度，当其在第j帧也被观测到时，估计其在第j帧中的坐标为：
此时的视觉残差为：（左侧为根据i帧反推估计的位置，右侧为观测值）
逆深度参数化
逆深度作为参数原因：
1）观测到的特征点深度可能非常大，难以进行优化；
2）可以减少实际优化的参数变量；
3）逆深度更加服从高斯分布。这里特征点的逆深度在第i帧初始化操作中得到。
论文中：
上面的公式是传统的针孔相机模型，这里用的是单位半球体的相机观测残差。是一个鱼眼相机。
在相机的归一化平面上比较残差，再将视觉残差投影到单位球面的正切平面上。由于视觉残差的*度是2，所以我们将残差向量投影到切平面上。
这两种相机模型在VINS-Mono的论文中都有用到：
#ifdef UNIT_SPHERE_ERROR
residual = tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
double dep_j = pts_camera_j.z();
residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif
在第i帧第一次观测到第l个路标点，在第j帧中对该路标点进行观测的残差为：
第一个式子就是残差的表达式，第二个式子是鱼眼相机反投影函数将观测到的像素坐标转换成单位向量的观测值数据，b1和b2是此单位向量的切平面上的一组基。第三个式子是重投影估计模型。其实VINS代码中也可以使用普通的针孔相机模型。
2、优化变量
包括两个时刻的状态量、外参，以及逆深度
值得注意的是：和纯视觉不同的是，待优化的状态量中的旋转平移都是IMU系到w系，而不是Camera到w系。
状态量特征：
1、待优化量和IMU约束中待优化量有重叠，再次体现了紧耦合。
2、和IMU不同的是，IMU每次优化的状态量是相邻两帧的，但是视觉优化的2帧不一定是相邻的，因此用i，j表示。
3、Jacobian
求解Jacobian就是用视觉残差对上述7个状态变量进行求导。
方法一：崔博直接给出答案
方法二：深蓝学院详细推导
4、协方差
void Estimator::setParameter()
{
for (int i = 0; i < NUM_OF_CAM; i++)
{
tic[i] = TIC[i];
ric[i] = RIC[i];
}
f_manager.setRic(ric);
ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
td = TD;
}
三、IMU约束
（之前有介绍的，这里就不详细的推导了）
1、残差：
状态量传播预测与IMU预积分的残差：
2、优化变量：
IMU的第i、j时刻下的p位置，v速度，Q旋转（PVQ），两个偏置ba,bw
3、IMU测量残差公式推导
简单来说，如果每一帧IMU都要相对于第一帧求位姿，计算量很大；考虑将PVQ积分公式改为相对第i时刻的姿态。
4、残差对状态量的Jacobian
这里直接给出结果，如果想看详细推导，参考:三、VINS-mono 代码解析——IMU预积分的残差、Jacobian和协方差
对应位置补充上这个J的矩阵块就行了，其他位置还是0。一共分为四大模块：
5、残差对状态量的协方差
四、基于舒尔补的边缘化
为什么进行边缘化操作？
如果仅仅从前后两帧图像计算相机位姿，速度快但是精度低；但是采用全局优化BA，连接所有图像帧，精度高但是复杂度高。
采用滑动窗，固定数量的帧进行优化，这样能够保证精度和速度。既然是滑动窗，在滑动的过程中会有新的图像进来，旧的图像离开，所谓边缘化就是为了删除图像，但是把图像信息约束保留下来。
1、论文部分
为了防止pose和特征的个数的复杂度随着时间不断增长，引入边缘。
化，在移除位姿时将关联的约束转化为先验放入优化问题中。
为了限制基于优化的VIO计算复杂度，引入边缘化。有选择地从滑动窗口中将IMU状态xK和特征λ1边缘化，同时将对应于边缘状态的测量值转换为先验。
分为两种情况，
1、一种是倒数第二帧如果是关键帧的话，将最旧的pose移出Sliding Window，将最旧帧关联的视觉和惯性数据边缘化掉。把第一个老关键帧及其测量值被边缘化；Margin_Old作为先验值。
2、如果倒数第二帧不是关键帧的话，那么就只剔除倒数第二帧的视觉观测，而不剔除它的IMU约束。原因是边缘化保证关键帧之间有足够视差而能够三角化足够多的地图点。并且保证了IMU预积分的连贯性。
为了保持系统的稀疏性，我们不会边缘化非关键帧的所有测量值。
2、基本公式
我们根据运动模型和观测模型建立H矩阵（高斯牛顿法中的JJT）的过程其实就是根据概率图模型（多元高斯分布）建立各个节点变量间的信息矩阵（协方差矩阵的逆）的过程，而边缘化则是去掉概率图中的某一个节点后信息矩阵会发生怎样的变换的问题。
基于高斯牛顿的非线性优化理论可知，H*delta_x=b可以写成：
其中，delta_xa和delta_xb分别是希望marg掉的部分和保留部分。
VINS中需要边缘化滑动窗口中的最老帧，目的是希望不再计算这一帧的位姿或者与其相关的路标点，但是希望保留该帧对窗口内其余帧的约束关系。我们基于与移除状态相关的所有边缘化测量值构造一个先验。新的先验项被添加到现有的先验项中。
3、舒尔补
4、marg后形成的先验
xa为需要marg的变量，假设为相机pose,我们更关心如何求解希望保留的xb，而不再求解xa（即marg的变量改为0，左乘时左上是0），这里是要变化为上三角。
注意：去掉了x1,但是之前和x1相连的所有量x2 x3 x4 x5 在marg掉x1后变得两两相连。
5 具体例子
5.1之前的信息矩阵H的构成
5.2 舒尔补后形成新的信息矩阵new_H,并构造为先验
5.3 新测量信息和先验构成新的系统
参考文章：
try_again_later的博客
VINS-Mono理论学习——后端非线性优化 by Manii
VINS-Mono之后端非线性优化 (目标函数中视觉残差和IMU残差，及其对状态量的雅克比矩阵、协方差递推方程的推导) by Hansry
深蓝学院手写VIO课程
https://blog.csdn.net/iwanderu/article/details/104729332