VINS-Mono-紧耦合VIO中IMU残差的优化原理和代码解析

前言

VINS-Mono算法中VIO部分是基于优化的方法来做的，其优化的目标函数为:
$$\underset{\mathbf{\chi }}{\min }\left\{{\Vert {\mathbf{r}}_p-{\mathbf{H}}_p\mathbf{\chi }\Vert }^2+{\sum _{k\in B}\Vert {\mathbf{r}}_B({\hat{\mathbf{z}}}_{b_{k+1}}^{b_k},\mathbf{\chi })\Vert }_{{\mathbf{P}}_{b_{k+1}}^{b_k}}^2+\sum _{(l,j)\in C}\rho ({\Vert {\mathbf{r}}_C({\hat{\mathbf{z}}}_l^{c_j},\mathbf{\chi })\Vert }_{{\mathbf{P}}_l^{c_j}}^2)\right\}$$
式中${\mathbf{r}}_B,{\mathbf{r}}_C$分别是相对于IMU和视觉测量的残差项，剩余一项是来自边缘化的先验信息．由于式中每一项残差的定义计算及代码实现的篇幅较大，我将分三篇博客来分析这三个残差项．本篇分析:
$${\sum _{k\in B}\Vert {\mathbf{r}}_B({\hat{\mathbf{z}}}_{b_{k+1}}^{b_k},\mathbf{\chi })\Vert }_{{\mathbf{P}}_{b_{k+1}}^{b_k}}^2$$

原理

对于非线性最小二乘优化问题，一般都会采用高斯牛顿迭代法将问题转换成如下增量求解的形式:
$${\mathbf{J}}^T\mathbf{J}\delta \mathbf{x}=-{\mathbf{J}}^T\mathbf{f}$$
其中$\mathbf{f}$对应残差方程，$\mathbf{J}$对应残差方程相对于状态量的雅克比矩阵．VINS-Mono中利用的是ceres_solver最小二乘求解库来实现上述计算过程，但需要自己定义残差方程，其次对于复杂问题推荐自己计算雅克比矩阵，这样能够保证优化过程的高效性．

定义残差方程

$${\mathbf{r}}_B={\left[\begin{array}{c}\delta {\mathbf{\alpha }}_{b_{k+1}}^{b_k}\\ \delta {\mathbf{\theta }}_{b_{k+1}}^{b_k}\\ \delta {\mathbf{\beta }}_{b_{k+1}}^{b_k}\\ \delta {\mathbf{b}}_a\\ \delta {\mathbf{b}}_g\end{array}\right]}_{15\times 1}=\left[\begin{array}{c}{\mathbf{R}}_w^{b_k}({\mathbf{p}}_{b_{k+1}}^w-{\mathbf{p}}_{b_k}^w+\frac{1}{2}{\mathbf{g}}^w\Delta t^2-{\mathbf{v}}_{b_k}^w\Delta t)-{\hat{\mathbf{\alpha }}}_{b_{k+1}}^{b_k}\\ 2[({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w{]}_{xyz}\\ {\mathbf{R}}_w^{b_k}({\mathbf{v}}_{b_{k+1}}^w+{\mathbf{g}}^w\Delta t-{\mathbf{v}}_{b_k}^w)-{\hat{\mathbf{\beta }}}_{b_{k+1}}^{b_k}\\ {\mathbf{b}}_{a_{b_{k+1}}}-{\mathbf{b}}_{a_{b_k}}\\ {\mathbf{b}}_{w_{b_{k+1}}}-{\mathbf{b}}_{w_{b_k}}\end{array}\right]\text{\hspace{1em}(1)}$$

计算雅克比矩阵

通过公式(1)可以看出参与优化的变量包括$b_k$时刻的状态量$[{\mathbf{p}}_{b_k}^w,{\mathbf{q}}_{b_k}^w,{\mathbf{v}}_{b_k}^w,{\mathbf{b}}_{a_k},{\mathbf{b}}_{w_k}]$和$b_{k+1}$时刻的状态量$[{\mathbf{p}}_{b_{k+1}}^w,{\mathbf{q}}_{b_{k+1}}^w,{\mathbf{v}}_{b_{k+1}}^w,{\mathbf{b}}_{a_{k+1}},{\mathbf{b}}_{w_{k+1}}]$，为了与代码保持一致，我们将这两组状态量分成四组，分别为$[{\mathbf{p}}_{b_k}^w,{\mathbf{q}}_{b_k}^w],[{\mathbf{v}}_{b_k}^w,{\mathbf{b}}_{a_k},{\mathbf{b}}_{w_k}],[{\mathbf{p}}_{b_{k+1}}^w,{\mathbf{q}}_{b_{k+1}}^w],[{\mathbf{v}}_{b_{k+1}}^w,{\mathbf{b}}_{a_{k+1}},{\mathbf{b}}_{w_{k+1}}]$，再分别计算残差方程相对于这四组变量的雅克比矩阵．

相对于$b_k$时刻位姿的jacobi

• 结果
  $$\begin{array}{rl}\mathbf{J}[0{]}^{15\times 7}& =\frac{\partial {\mathbf{r}}_B}{\partial ({\mathbf{p}}_{{\mathbf{b}}_{\mathbf{k}}}^{\mathbf{w}},{\mathbf{q}}_{{\mathbf{b}}_{\mathbf{k}}}^{\mathbf{w}})}=\left[\begin{array}{cc}-{\mathbf{R}}_w^{b_k}& {\lfloor {\mathbf{R}}_w^{b_k}({\mathbf{p}}_{b_{k+1}}^w-{\mathbf{p}}_{b_k}^w+\frac{1}{2}{\mathbf{g}}^w\Delta t^2-{\mathbf{v}}_{b_k}^w\Delta t)\rfloor }_{\times }\\ 0& -{\mathbf{Q}}^{+}[{{\mathbf{q}}_{b_{k+1}}^w}^{-1}\otimes {\mathbf{q}}_{b_k}^w]{\mathbf{Q}}^{-1}[{\mathbf{\gamma }}_{b_{k+1}}^{b_k}]\\ 0& {\lfloor {\mathbf{R}}_w^{b_k}({\mathbf{v}}_{b_{k+1}}^w-{\mathbf{v}}_{b_k}^w+{\mathbf{g}}^w\Delta t)\rfloor }_{\times }\\ 0& 0\\ 0& 0\end{array}\right]\end{array}\text{\hspace{1em}(0)}$$

• 推导：
  $$\begin{array}{rl}\frac{\partial \delta {\mathbf{\alpha }}_{b_{k+1}}^{b_k}}{\partial {\mathbf{q}}_{b_k}^w}& =\frac{\partial [{\mathbf{R}}_w^{b_k}({\mathbf{p}}_{b_{k+1}}^w-{\mathbf{p}}_{b_k}^w+\frac{1}{2}{\mathbf{g}}^w\Delta t^2-{\mathbf{v}}_{b_k}^w\Delta t)-{\hat{\mathbf{\alpha }}}_{b_{k+1}}^{b_k}]}{\partial {\mathbf{q}}_{b_k}^w}=\frac{\partial [{\mathbf{R}}_w^{b_k}{\mathbf{p}}^{\prime }-{\hat{\mathbf{\alpha }}}_{b_{k+1}}^{b_k}]}{\partial {\mathbf{q}}_{b_k}^w}\\ & =\frac{\partial [{{\mathbf{R}}_{b_k}^w}^{-1}{\mathbf{p}}^{\prime }]}{\partial {\mathbf{q}}_{b_k}^w}=\frac{[exp({\lfloor \delta \mathbf{\theta }\rfloor }_{\times }){\mathbf{R}}_{b_k}^w{]}^{-1}{\mathbf{p}}^{\prime }-{{\mathbf{R}}_{b_k}^w}^{-1}{\mathbf{p}}^{\prime }}{\delta \mathbf{\theta }}\\ & =\frac{(\mathbf{I}-{\lfloor \delta \mathbf{\theta }\rfloor }_{\times }){\mathbf{R}}_w^{b_k}{\mathbf{p}}^{\prime }-{\mathbf{R}}_w^{b_k}{\mathbf{p}}^{\prime }}{\delta \mathbf{\theta }}=\frac{-{\lfloor \delta \mathbf{\theta }\rfloor }_{\times }{\mathbf{R}}_w^{b_k}{\mathbf{p}}^{\prime }}{\delta \mathbf{\theta }}\\ & =\frac{{\lfloor {\mathbf{R}}_w^{b_k}{\mathbf{p}}^{\prime }\rfloor }_{\times }\delta \mathbf{\theta }}{\delta \mathbf{\theta }}={\lfloor {\mathbf{R}}_w^{b_k}{\mathbf{p}}^{\prime }\rfloor }_{\times }\\ & ={\lfloor {\mathbf{R}}_w^{b_k}({\mathbf{p}}_{b_{k+1}}^w-{\mathbf{p}}_{b_k}^w+\frac{1}{2}{\mathbf{g}}^w\Delta t^2-{\mathbf{v}}_{b_k}^w\Delta t)\rfloor }_{\times }\end{array}$$
  $\frac{\partial \delta {\mathbf{\beta }}_{b_{k+1}}^{b_k}}{\partial {\mathbf{q}}_{b_k}^w}$的推导与上述推导方法类似，利用左扰动模型进行求导．
  $$\begin{array}{rl}\frac{\partial \delta {\mathbf{\theta }}_{b_{k+1}}^{b_k}}{\partial {\mathbf{q}}_{b_k}^w}& =\frac{\partial (2[({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w{]}_{xyz})}{\partial {\mathbf{q}}_{b_k}^w}\\ & =2\underset{\delta {\mathbf{\theta }}_{b_k}^w\to 0}{\lim }\frac{({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes ({\mathbf{q}}_{b_k}^w\otimes \left[\begin{array}{c}1\\ \frac{\delta {\mathbf{\theta }}_{b_k}^w}{2}\end{array}\right]{)}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w-({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes ({\mathbf{q}}_{b_k}^w{)}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w}{\delta {\mathbf{\theta }}_{b_k}^w}\\ & =2\underset{\delta {\mathbf{\theta }}_{b_k}^w\to 0}{\lim }\frac{{\mathbf{Q}}^{-}[{{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w]{\mathbf{Q}}^{+}[({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}](\left[\begin{array}{c}0\\ -\frac{\delta {\mathbf{\theta }}_{b_k}^w}{2}\end{array}\right])}{\delta {\mathbf{\theta }}_{b_k}^w}\\ & =-2{\mathbf{Q}}^{-}[{{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w]{\mathbf{Q}}^{+}[({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}]\\ & =-2{\mathbf{Q}}^{+}[{{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w]{\mathbf{Q}}^{-}[{\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}]\end{array}$$
  上述推导过程中出现的${\mathbf{Q}}^{+},{\mathbf{Q}}^{-}$分别为四元素的左乘右乘矩阵，具体求解可参考博客:Quaternion kinematics for ESKF－预测中"一些四元素的性质"章节．

相对于$b_k$时刻偏置的jacobi

• 结果
  $$\mathbf{J}[1{]}^{15\times 9}=\frac{\partial {\mathbf{r}}_B}{\partial ({\mathbf{v}}_{b_k}^w,{\mathbf{b}}_{a_k},{\mathbf{b}}_{w_k})}=\left[\begin{array}{ccc}-{\mathbf{R}}_w^{b_k}\Delta t& -{\mathbf{J}}_{{\mathbf{b}}_a}^{\mathbf{\alpha }}& -{\mathbf{J}}_{{\mathbf{b}}_w}^{\mathbf{\alpha }}\\ 0& 0& -{\mathbf{Q}}^{+}[{{\mathbf{q}}_{b_{k+1}}^w}^{-1}\otimes {\mathbf{q}}_{b_k}^w\otimes {\mathbf{\gamma }}_{b_{k+1}}^{b_k}]{\mathbf{J}}_{{\mathbf{b}}_w}^{\mathbf{\gamma }}\\ -{\mathbf{R}}_w^{b_k}& -{\mathbf{J}}_{{\mathbf{b}}_a}^{\mathbf{\beta }}& -{\mathbf{J}}_{{\mathbf{b}}_w}^{\mathbf{\beta }}\\ 0& -\mathbf{I}& 0\\ 0& 0& -\mathbf{I}\end{array}\right]\text{\hspace{1em}(1)}$$
• 推导
  $$\begin{array}{rl}\frac{\partial \delta {\mathbf{\theta }}_{b_{k+1}}^{b_k}}{\partial {\mathbf{b}}_{w_k}}& =\frac{\partial (2[({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w{]}_{xyz})}{\partial {\mathbf{b}}_{w_k}}\\ & =2\underset{\delta {\mathbf{b}}_{w_k}\to 0}{\lim }\frac{({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_w}^{\mathbf{\gamma }}\delta {\mathbf{b}}_{w_k}\end{array}\right]{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w-({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w}{\delta {\mathbf{b}}_{w_k}}\\ & =2\underset{\delta {\mathbf{b}}_{w_k}\to 0}{\lim }\frac{\left[\begin{array}{c}1\\ -\frac{1}{2}{\mathbf{J}}_{b_w}^{\mathbf{\gamma }}\delta {\mathbf{b}}_{w_k}\end{array}\right]\otimes ({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w-({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w}{\delta {\mathbf{b}}_{w_k}}\\ & =2\underset{\delta {\mathbf{b}}_{w_k}\to 0}{\lim }\frac{\left[\begin{array}{c}0\\ -\frac{1}{2}{\mathbf{J}}_{b_w}^{\mathbf{\gamma }}\delta {\mathbf{b}}_{w_k}\end{array}\right]\otimes ({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w}{\delta {\mathbf{b}}_{w_k}}\\ & =-{\mathbf{Q}}^{-}[({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w]\left[\begin{array}{c}0\\ {\mathbf{J}}_{b_w}^{\mathbf{\gamma }}\end{array}\right]\\ & =-{\mathbf{Q}}^{+}[({\mathbf{q}}_{b_{k+1}}^w{)}^{-1}\otimes {\mathbf{q}}_{b_k}^w\otimes {\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}]\left[\begin{array}{c}0\\ {\mathbf{J}}_{b_w}^{\mathbf{\gamma }}\end{array}\right]\end{array}$$
• 说明
  式中$({\mathbf{J}}_{b_a}^{\mathbf{\alpha }},{\mathbf{J}}_{b_w}^{\mathbf{\alpha }},{\mathbf{J}}_{b_a}^{\mathbf{\beta }},{\mathbf{J}}_{b_w}^{\mathbf{\beta }},{\mathbf{J}}_{b_a}^{\mathbf{\gamma }},{\mathbf{J}}_{b_w}^{\mathbf{\gamma }})$分别为预积分结果相对于偏置的雅克比矩阵，在imu预积分过程中已经计算过了，这里就不需要再计算了．

相对于$b_{k+1}$时刻位姿的jacobi

• 结果
  $$\mathbf{J}[2{]}^{15\times 7}=\frac{\partial {\mathbf{r}}_B}{\partial ({\mathbf{p}}_{{\mathbf{b}}_{\mathbf{k}}}^{\mathbf{w}},{\mathbf{q}}_{{\mathbf{b}}_{\mathbf{k}}}^{\mathbf{w}})}=\left[\begin{array}{cc}{\mathbf{R}}_w^{b_k}& 0\\ 0& {\mathbf{Q}}^{+}[{{\mathbf{\gamma }}_{b_{k+1}}^{b_k}}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w]\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right]\text{\hspace{1em}(2)}$$
• 推导
  $$\begin{array}{rl}\frac{\partial \delta {\mathbf{\theta }}_{b_{k+1}}^{b_k}}{\partial {\mathbf{q}}_{b_{k+1}}^w}& =\frac{\partial (2[({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w{]}_{xyz})}{\partial {\mathbf{q}}_{b_{k+1}}^w}\\ & =\frac{2[({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_{k+1}}^w\end{array}\right]]-2[({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w]}{\delta {\mathbf{\theta }}_{b_{k+1}}^w}\\ & =\frac{({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w\otimes \left[\begin{array}{c}0\\ \delta {\mathbf{\theta }}_{b_{k+1}}^w\end{array}\right]}{\delta {\mathbf{\theta }}_{b_{k+1}}^w}\\ & ={\mathbf{Q}}^{+}[({\hat{\mathbf{\gamma }}}_{b_{k+1}}^{b_k}{)}^{-1}\otimes {{\mathbf{q}}_{b_k}^w}^{-1}\otimes {\mathbf{q}}_{b_{k+1}}^w]\end{array}$$

相对于$b_{k+1}$时刻偏置的jacobi

• 结果
  $$\mathbf{J}[3{]}^{15\times 9}=\frac{\partial {\mathbf{r}}_B}{\partial ({\mathbf{v}}_{b_{k+1}}^w,{\mathbf{b}}_{a_{k+1}},{\mathbf{b}}_{w_{k+1}})}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ {\mathbf{R}}_w^{b_k}& 0& 0\\ 0& \mathbf{I}& 0\\ 0& 0& \mathbf{I}\end{array}\right]\text{\hspace{1em}(3)}$$

代码解析

    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
    {

        // 第i帧位姿信息p_b(i)_w, q_b_(i)_w
        Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]);
        Eigen::Quaterniond Qi(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
		// 第i帧偏置信息v_b(i)_w, b_a(i), b_w(i)
        Eigen::Vector3d Vi(parameters[1][0], parameters[1][1], parameters[1][2]);
        Eigen::Vector3d Bai(parameters[1][3], parameters[1][4], parameters[1][5]);
        Eigen::Vector3d Bgi(parameters[1][6], parameters[1][7], parameters[1][8]);

        // 第i帧位姿信息p_b(j)_w, q_b_(j)_w
        Eigen::Vector3d Pj(parameters[2][0], parameters[2][1], parameters[2][2]);
        Eigen::Quaterniond Qj(parameters[2][6], parameters[2][3], parameters[2][4], parameters[2][5]);
		// 第i帧偏置信息v_b(j)_w, b_a(j), b_w(j)
        Eigen::Vector3d Vj(parameters[3][0], parameters[3][1], parameters[3][2]);
        Eigen::Vector3d Baj(parameters[3][3], parameters[3][4], parameters[3][5]);
        Eigen::Vector3d Bgj(parameters[3][6], parameters[3][7], parameters[3][8]);

        // 计算第i帧与第j帧之间的状态残差
        Eigen::Map<Eigen::Matrix<double, 15, 1>> residual(residuals);
        residual = pre_integration->evaluate(Pi, Qi, Vi, Bai, Bgi,
                                             Pj, Qj, Vj, Baj, Bgj);

        // 计算用马氏距离表示的残差
        Eigen::Matrix<double, 15, 15> sqrt_info = Eigen::LLT<Eigen::Matrix<double, 15, 15>>(pre_integration->covariance.inverse()).matrixL().transpose();
        //sqrt_info.setIdentity();
        residual = sqrt_info * residual;

        if (jacobians)
        {
            double sum_dt = pre_integration->sum_dt;
            // 获取imu预积分过程中计算的位置相对于偏置状态的雅克比矩阵
            Eigen::Matrix3d dp_dba = pre_integration->jacobian.template block<3, 3>(O_P, O_BA);
            Eigen::Matrix3d dp_dbg = pre_integration->jacobian.template block<3, 3>(O_P, O_BG);
			// 获取imu预积分过程中计算的姿态相对于偏置的雅克比矩阵
            Eigen::Matrix3d dq_dbg = pre_integration->jacobian.template block<3, 3>(O_R, O_BG);
			// 获取imu预积分过程中计算的速度相对于偏置的雅克比矩阵
            Eigen::Matrix3d dv_dba = pre_integration->jacobian.template block<3, 3>(O_V, O_BA);
            Eigen::Matrix3d dv_dbg = pre_integration->jacobian.template block<3, 3>(O_V, O_BG);

            if (pre_integration->jacobian.maxCoeff() > 1e8 || pre_integration->jacobian.minCoeff() < -1e8)
            {
                ROS_WARN("numerical unstable in preintegration");
                //std::cout << pre_integration->jacobian << std::endl;
                ///                ROS_BREAK();
            }

			// 对应公式(0)
            if (jacobians[0])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_i(jacobians[0]);
                jacobian_pose_i.setZero();

                jacobian_pose_i.block<3, 3>(O_P, O_P) = -Qi.inverse().toRotationMatrix();
                jacobian_pose_i.block<3, 3>(O_P, O_R) = Utility::skewSymmetric(Qi.inverse() * (0.5 * G * sum_dt * sum_dt + Pj - Pi - Vi * sum_dt));

#if 0
            jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Qj.inverse() * Qi).toRotationMatrix();
#else
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_pose_i.block<3, 3>(O_R, O_R) = -(Utility::Qleft(Qj.inverse() * Qi) * Utility::Qright(corrected_delta_q)).bottomRightCorner<3, 3>();
#endif

                jacobian_pose_i.block<3, 3>(O_V, O_R) = Utility::skewSymmetric(Qi.inverse() * (G * sum_dt + Vj - Vi));

                jacobian_pose_i = sqrt_info * jacobian_pose_i;

                if (jacobian_pose_i.maxCoeff() > 1e8 || jacobian_pose_i.minCoeff() < -1e8)
                {
                    ROS_WARN("numerical unstable in preintegration");
                    //std::cout << sqrt_info << std::endl;
                    //ROS_BREAK();
                }
            }
            // 对应公式(1)
            if (jacobians[1])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_i(jacobians[1]);
                jacobian_speedbias_i.setZero();
                jacobian_speedbias_i.block<3, 3>(O_P, O_V - O_V) = -Qi.inverse().toRotationMatrix() * sum_dt;
                jacobian_speedbias_i.block<3, 3>(O_P, O_BA - O_V) = -dp_dba;
                jacobian_speedbias_i.block<3, 3>(O_P, O_BG - O_V) = -dp_dbg;

#if 0
            jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -dq_dbg;
#else
                //Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                //jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * corrected_delta_q).bottomRightCorner<3, 3>() * dq_dbg;
                jacobian_speedbias_i.block<3, 3>(O_R, O_BG - O_V) = -Utility::Qleft(Qj.inverse() * Qi * pre_integration->delta_q).bottomRightCorner<3, 3>() * dq_dbg;
#endif

                jacobian_speedbias_i.block<3, 3>(O_V, O_V - O_V) = -Qi.inverse().toRotationMatrix();
                jacobian_speedbias_i.block<3, 3>(O_V, O_BA - O_V) = -dv_dba;
                jacobian_speedbias_i.block<3, 3>(O_V, O_BG - O_V) = -dv_dbg;

                jacobian_speedbias_i.block<3, 3>(O_BA, O_BA - O_V) = -Eigen::Matrix3d::Identity();

                jacobian_speedbias_i.block<3, 3>(O_BG, O_BG - O_V) = -Eigen::Matrix3d::Identity();

                jacobian_speedbias_i = sqrt_info * jacobian_speedbias_i;

                //ROS_ASSERT(fabs(jacobian_speedbias_i.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_speedbias_i.minCoeff()) < 1e8);
            }
            // 对应公式(3)
            if (jacobians[2])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[2]);
                jacobian_pose_j.setZero();

                jacobian_pose_j.block<3, 3>(O_P, O_P) = Qi.inverse().toRotationMatrix();

#if 0
            jacobian_pose_j.block<3, 3>(O_R, O_R) = Eigen::Matrix3d::Identity();
#else
                Eigen::Quaterniond corrected_delta_q = pre_integration->delta_q * Utility::deltaQ(dq_dbg * (Bgi - pre_integration->linearized_bg));
                jacobian_pose_j.block<3, 3>(O_R, O_R) = Utility::Qleft(corrected_delta_q.inverse() * Qi.inverse() * Qj).bottomRightCorner<3, 3>();
#endif

                jacobian_pose_j = sqrt_info * jacobian_pose_j;

                //ROS_ASSERT(fabs(jacobian_pose_j.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_pose_j.minCoeff()) < 1e8);
            }
            // 对应公式(4)
            if (jacobians[3])
            {
                Eigen::Map<Eigen::Matrix<double, 15, 9, Eigen::RowMajor>> jacobian_speedbias_j(jacobians[3]);
                jacobian_speedbias_j.setZero();

                jacobian_speedbias_j.block<3, 3>(O_V, O_V - O_V) = Qi.inverse().toRotationMatrix();

                jacobian_speedbias_j.block<3, 3>(O_BA, O_BA - O_V) = Eigen::Matrix3d::Identity();

                jacobian_speedbias_j.block<3, 3>(O_BG, O_BG - O_V) = Eigen::Matrix3d::Identity();

                jacobian_speedbias_j = sqrt_info * jacobian_speedbias_j;

                //ROS_ASSERT(fabs(jacobian_speedbias_j.maxCoeff()) < 1e8);
                //ROS_ASSERT(fabs(jacobian_speedbias_j.minCoeff()) < 1e8);
            }
        }

        return true;
    }

参考

https://github.com/StevenCui/VIO-Doc
https://github.com/HKUST-Aerial-Robotics/VINS-Mono

注释代码路径

https://github.com/chennuo0125-HIT/vins-note