七、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、视觉误差函数部分（滑动窗口中特征点在相机下视觉重投影残差）

二、视觉约束

这部分要拟合的目标可以通过重投影误差约束，求解的是对同