后端优化 | VINS-Mono 论文公式推导与代码解析分讲

3. 后端优化(紧耦合)

VIO 紧耦合方案的主要思路就是通过将基于视觉构造的残差项和基于IMU构造的残差项放在一起构造成一个联合优化的问题，整个优化问题的最优解即可认为是比较准确的状态估计。

为了限制优化变量的数目，VINS-Mono 采用了滑动窗口的形式，滑动窗口 中的 全状态量：

滑动窗口内 n+1 个所有相机的状态(包括位置、朝向、速度、加速度计 bias 和陀螺仪 bias)
Camera 到 IMU 的外参
m+1 个 3D 点的逆深度

优化过程中的误差状态量

进而得到系统优化的代价函数（Minimize residuals from all sensors）

其中三个残差项依次是
边缘化的先验信息
IMU 测量残差
视觉的观测残差

三种残差都是用 马氏距离（与量纲无关） 来表示的。

Motion-only visual-inertial bundle adjustment: Optimize position, velocity, rotation in a smaller windows, assuming all other quantities are fixed

3.1 IMU 测量残差
（1）IMU 测量残差
上面的IMU预积分（估计值 - 测量值），得到 IMU 测量残差

/**

• [evaluate 计算IMU测量模型的残差]

• @param Pi，Qi，Vi，Bai，Bgi [前一次预积分结果]

• @param Pj，Qj，Vj，Baj，Bgj [后一次预积分结果]
  */
  Eigen::Matrix<double, 15, 1> evaluate(
  const Eigen::Vector3d &Pi, const Eigen::Quaterniond &Qi, const Eigen::Vector3d &Vi, const Eigen::Vector3d &Bai, const Eigen::Vector3d &Bgi,
  const Eigen::Vector3d &Pj, const Eigen::Quaterniond &Qj, const Eigen::Vector3d &Vj, const Eigen::Vector3d &Baj, const Eigen::Vector3d &Bgj)
  {
  Eigen::Matrix<double, 15, 1> residuals;

  Eigen::Matrix3d dp_dba = jacobian.block<3, 3>(O_P, O_BA);
  Eigen::Matrix3d dp_dbg = jacobian.block<3, 3>(O_P, O_BG);

  Eigen::Matrix3d dq_dbg = jacobian.block<3, 3>(O_R, O_BG);

  Eigen::Matrix3d dv_dba = jacobian.block<3, 3>(O_V, O_BA);
  Eigen::Matrix3d dv_dbg = jacobian.block<3, 3>(O_V, O_