VINS-Mono理论学习——后端非线性优化

前言

本文主要介绍VINS状态估计器模块（estimator）在完成了系统初始化后，对视觉与IMU信息进行基于滑动窗口的紧耦合过程中所用到的非线性优化理论。这部分对应论文第六章（VI. TIGHTLY-COUPLED MONOCULAR VIO），并参考了崔博的《VINS论文推导与代码解析》、深蓝学院的VIO课程内容。主要想对目标函数中视觉残差和IMU残差，以及对应的雅可比、协方差进行推导。

一、状态向量与代价函数

状态向量包括滑动窗口内的所有相机状态（位置P、旋转Q、速度V、加速度偏置ba、陀螺仪偏置bw）、相机到IMU的外参、所有3D点的逆深度：
$$\mathcal{X}=\left[{\mathbf{x}}_0,{\mathbf{x}}_1,\cdots {\mathbf{x}}_n,{\mathbf{x}}_c^b,{\lambda }_0,{\lambda }_1,\cdots {\lambda }_m\right]$$

$${\mathbf{x}}_k=\left[{\mathbf{p}}_{b_k}^w,{\mathbf{v}}_{b_k}^w,{\mathbf{q}}_{b_k}^w,{\mathbf{b}}_a,{\mathbf{b}}_g\right],k\in [0,n]$$

$${\mathbf{x}}_c^b=\left[{\mathbf{p}}_c^b,{\mathbf{q}}_c^b\right]$$

目标函数为
$$\underset{\mathcal{X}}{\min }\{{\Vert {\mathbf{r}}_p-{\mathbf{H}}_p\mathcal{X}\Vert }^2+\sum _{k\in \mathcal{B}}{\Vert {\mathbf{r}}_{\mathcal{B}}\left({\hat{\mathbf{z}}}_{b_{k+1}}^{b_k},\mathcal{X}\right)\Vert }_{{\mathbf{P}}_{b_{k+1}}^{b_k}}^2+\sum _{(l,j)\in \mathcal{C}}\rho \left({\Vert {\mathbf{r}}_{\mathcal{C}}\left({\hat{\mathbf{z}}}_l^{c_j},\mathcal{X}\right)\Vert }_{{\mathbf{P}}_l^{c_j}}^2\right)\}$$
这三项分别为边缘化的先验信息、IMU的测量残差、视觉的重投影误差。其中边缘化过程及其产生的先验将单独进行讨论。

另外有一点需要指出的是，因为实际优化的是：$min(d)=min(r^T{P}^{-1}r)$，这里r代表残差，P代表协方差。而Ceres只接受最小二乘优化，即$min(e^{T}e)$，因此在代码中，需要把信息矩阵$P^{-1}$做LLT分解，即$L{L}^T=P^{-1}$，代码中对应为sqrt_info，这样最后优化的$$min(d)=min(r^T{P}^{-1}r)=min((L^{T}r{)}^T(L^{T}r))=min(r^{\prime T}{r}^{\prime })$$

因此在下面所有的残差和Jacobian中，代码在最后都乘上了sqrt_info。

二、视觉约束

1、视觉测量残差

视觉测量残差就是重投影误差，定义为一个特征点在归一化相机坐标系下的估计值与观测值的差。估计值即特征点的三维空间坐标$(x,y,z{)}^T$，观测值为其在相机归一化平面的坐标。
$${\mathbf{r}}_c=\left[\begin{array}{l}\frac{x}{z}-u\\ \frac{y}{z}-v\end{array}\right]$$

逆深度参数化：采用逆深度$\lambda$来表示特征点在归一化相机坐标系下的坐标：
$$\left[\begin{array}{l}x\\ y\\ z\end{array}\right]=\frac{1}{\lambda }\left[\begin{array}{l}u\\ v\\ 1\end{array}\right]$$

以逆深度作为参数的原因，一是因为一些观测到的特征点深度值可能会非常大，难以进行优化；二是可以减少实际优化的参数变量；三是逆深度更加服从高斯分布。

对于第l个路标点P，其在第i帧中被第一次观测到，其归一化相机坐标系的坐标$\left[\begin{array}{c}{u}_{c_i}\\ v_{c_i}\\ 1\end{array}\right]$
转换到第j帧的坐标为：
$$\left[\begin{array}{c}{x}_{c_j}\\ y_{c_j}\\ z_{c_j}\\ 1\end{array}\right]={\mathbf{T}}_{bc}^{-1}{\mathbf{T}}_{w{b}_j}^{-1}{\mathbf{T}}_{w{b}_i}{\mathbf{T}}_{bc}\left[\begin{array}{c}\frac{1}{\lambda }{u}_{c_i}\\ \frac{1}{\lambda }{v}_{c_i}\\ \frac{1}{\lambda }\\ 1\end{array}\right]$$

这里因为我们优化的状态量是IMU坐标系（Body坐标系）的位姿，因此相较于视觉SLAM的重投影误差，还需加上两项从相机到IMU的外参。

而观测值即P在第j帧被观测到的归一化相机坐标系下的坐标为$\left[\begin{array}{c}{u}_{c_j}\\ v_{c_j}\\ 1\end{array}\right]$

因而得到视觉残差为：

$${\mathbf{r}}_c=\left[\begin{array}{l}\frac{x_{c_j}}{z_{c_j}}-u_{c_j}\\ \frac{y_{c_j}}{z_{c_j}}-v_{c_j}\end{array}\right]$$

将重投影方程拆成三维坐标形式：
$${\mathcal{P}}_l^{c_j}=R_b^c\{R_w^{b_j}[R_{b_i}^w(R_c^b\frac{1}{\lambda }\left[\begin{array}{c}{u}_{c_i}\\ v_{c_i}\\ 1\end{array}\right]+p_c^b)+p_{b_i}^w]+p_w^{b_j}\}+p_b^c$$

因为对于变换矩阵有：$R_b^c=(R_c^b{)}^T，p_b^c=-(R_c^b{)}^T{p}_c^b$，所以

$${\mathcal{P}}_l^{c_j}=R_b^c\{R_w^{b_j}[R_{b_i}^w(R_c^b\frac{1}{\lambda }\left[\begin{array}{c}{u}_{c_i}\\ v_{c_i}\\ 1\end{array}\right]+p_c^b)+p_{b_i}^w]-R_w^{b_j}{p}_{b_j}^w\}-R_b^c{p}_c^b$$

$$=R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b\frac{1}{\lambda }\left[\begin{array}{c}{u}_{c_i}\\ v_{c_i}\\ 1\end{array}\right]+R_b^c(R_w^{b_j}(R_{b_i}^w{p}_c^b+p_{b_i}^w-p_{b_j}^w)-p_c^b)$$

---

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

最后定义的视觉残差为：
$${\mathbf{r}}_{\mathcal{C}}\left({\hat{\mathbf{z}}}_l^{c_j},\mathcal{X}\right)=\left[\begin{array}{l}{\mathbf{b}}_1\\ {\mathbf{b}}_2\end{array}\right]\left({\widehat{\overline{\mathcal{P}}}}_l^{c_j}-\frac{{\mathcal{P}}_l^{c_j}}{\Vert {\mathcal{P}}_l^{cj}\Vert }\right)$$

$${\widehat{\overline{\mathcal{P}}}}_l^{c_j}={\pi }_c^{-1}\left(\left[\begin{array}{c}{\hat{u}}_l^{c_j}\\ {\hat{v}}_l^{c_j}\end{array}\right]\right)$$

这里将重投影和测量值都归一化为单位向量，将视觉残差为两个单位向量的差。因为视觉残差的自由度为2，再将视觉残差投影到正切平面上，b1b2为正切平面的两个正交基。

这两种相机模型在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

2、优化变量

包括两个时刻的状态量、外参，以及逆深度。
$$[p_{b_i}^w,q_{b_i}^w]、[p_{b_j}^w,q_{b_j}^w]、[p_c^b,q_c^b]、{\lambda }_l$$

3、Jacobian

这里我们采用针孔相机的（归一化相机平面）视觉残差来进行推导，而相机球面模型的视觉残差推导过程也相似。

$${\mathbf{r}}_c=\left[\begin{array}{l}\frac{x_{c_j}}{z_{c_j}}-u_{c_j}\\ \frac{y_{c_j}}{z_{c_j}}-v_{c_j}\end{array}\right]$$

这里先定义：

$$f_{c_i}=\frac{1}{{\lambda }_l}{\overline{P}}_l^{c_i}=\frac{1}{\lambda }\left[\begin{array}{c}{u}_{c_i}\\ v_{c_i}\\ 1\end{array}\right]$$

$f_{c_j}$即为原来的${\mathcal{P}}_l^{c_j}$：

$$f_{c_j}={\mathcal{P}}_l^{c_j}=R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b{f}_{c_i}+R_b^c(R_w^{b_j}(R_{b_i}^w{p}_c^b+p_{b_i}^w-p_{b_j}^w)-p_c^b)$$

因为根据链式法则，对Jacobian的计算可以分成：
1）视觉残差对重投影3D点$f_{c_j}$求导。

$$\frac{\partial {\mathbf{r}}_c}{\partial {\mathbf{f}}_{c_j}}=\left[\begin{array}{ccc}\frac{1}{z_{c_j}}& 0& -\frac{x_{c_j}}{z_{c_j}^2}\\ 0& \frac{1}{z_{c_j}}& -\frac{y_{c_j}}{z_{c_j}^2}\end{array}\right]$$

2）$f_{c_j}$对各个优化变量求导。（这里采用了崔博的推导结果）

$$J[0{]}^{3\times 7}=\left[\frac{\partial f_{c_j}}{\partial p_{b_i}^w},\frac{\partial f_{c_j}}{\partial q_{b_i}^w}\right]=\left[R_b^c{R}_w^{b_j},-R_b^c{R}_w^{b_j}{R}_{b_i}^w{\left(R_c^b\frac{1}{{\lambda }_l}{\overline{P}}_l^{c_i}+p_c^b\right)}^{\wedge }\right]$$

$$J[1{]}^{3\times 7}=\left[\frac{\partial f_{c_j}}{\partial p_{b_j}^w},\frac{\partial f_{c_j}}{\partial q_{b_j}^w}\right]=\left[-R_b^c{R}_w^{b_j},R_b^c{\left\{R_w^{b_j}\left[R_{b_i}^w\left(R_c^b\frac{{\overline{P}}_l^{c_i}}{{\lambda }_l}+p_c^b\right)+p_{b_i}^w-p_{b_j}^w\right]\right\}}^{\wedge }\right]$$

$$J[2{]}^{3\times 7}=\left[\frac{\partial f_{c_j}}{\partial p_c^b},\frac{\partial f_{c_j}}{\partial q_c^b}\right]={\left[\begin{array}{c}{R}_b^c\left(R_w^{b_j}{R}_{b_i}^w-I_{3\times 3}\right)\\ -R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b{\left(\frac{{\overline{P}}_l^{c_i}}{{\lambda }_l}\right)}^{\wedge }+{\left(R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b\frac{{\overline{P}}_l^{c_i}}{{\lambda }_l}\right)}^{\wedge }+{\left\{R_b^c\left[R_w^{b_j}\left(R_{b_i}^w{p}_c^b+|p_{b_i}^w-p_{b_j}^w\right)-p_c^b\right]\right\}}^{\wedge }\end{array}\right]}^T$$

$$J[3{]}^{3\times 1}=\frac{\partial f_{c_j}}{\partial {\lambda }_l}=-R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b\frac{{\overline{P}}_l^{c_i}}{{\lambda }_l^2}$$

这一部分的具体代码实现在projection_factor.cpp 的 bool ProjectionFactor::Evaluate() 函数中，由于数学公式很容易找到对应，这里就不对代码做解释了。

---

下面是对这些Jacobian的证明：
推导结果把崔博和VIO课程的结果都放上去了，两者其实是一样的
再把$f_{c_j}$写一遍以看的更清楚

$$f_{c_j}=R_b^c\{R_w^{b_j}[R_{b_i}^w(R_c^b{f}_{c_i}+p_c^b)+p_{b_i}^w-p_{b_j}^w]-p_c^b\}$$

• 对i时刻位移$p_{b_i}^w$

$$\frac{\partial f_{c_j}}{\partial p_{b_i}^w}=R_b^c{R}_w^{b_j}={\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }$$

• 对i时刻旋转$q_{b_i}^w$

$$\begin{gathered} \frac{\partial f_{c_j}}{\partial q_{b_i}^w}=\frac{\partial f_{c_j}}{\partial \delta {\theta }_{b_i}}=\frac{\partial R_b^c\{R_w^{b_j}[R_{b_i}^{w}exp([\delta {\theta }_{b_i}{]}_{\times })(R_c^b{f}_{c_i}+p_c^b)+p_{b_i}^w-p_{b_j}^w]-p_c^b\}}{\partial \delta {\theta }_{b_i}} \\ =\frac{\partial R_b^c{R}_w^{b_j}{R}_{b_i}^{w}exp([\delta {\theta }_{b_i}{]}_{\times })(R_c^b{f}_{c_i}+p_c^b)}{\partial \delta {\theta }_{b_i}} \\ =\frac{\partial R_b^c{R}_w^{b_j}{R}_{b_i}^w([\delta {\theta }_{b_i}{]}_{\times })(R_c^b{f}_{c_i}+p_c^b)}{\partial \delta {\theta }_{b_i}} \\ =-R_b^c{R}_w^{b_j}{R}_{b_i}^w[R_c^b{f}_{c_i}+p_c^b{]}_{\times }=-{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\left[{\mathbf{f}}_{b_i}\right]}_{\times } \end{gathered}$$

• 对j时刻位移$p_{b_j}^w$

$$\frac{\partial f_{c_j}}{\partial p_{b_j}^w}=-R_b^c{R}_w^{b_j}=-{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }$$

• 对j时刻旋转$q_{b_j}^w$

$$\begin{gathered} \frac{\partial f_{c_j}}{\partial q_{b_j}^w}=\frac{\partial f_{c_j}}{\partial \delta {\theta }_{b_j}}=\frac{\partial R_b^c\{(R_{b_j}^{w}exp([\delta {\theta }_{b_j}{]}_{\times }){)}^{-1}[R_{b_i}^w(R_c^b{f}_{c_i}+p_c^b)+p_{b_i}^w-p_{b_j}^w]-p_c^b\}}{\partial \delta {\theta }_{b_i}} \\ =\frac{\partial R_b^{c}exp(-[\delta {\theta }_{b_j}{]}_{\times })R_w^{b_j}[R_{b_i}^w(R_c^b{f}_{c_i}+p_c^b)+p_{b_i}^w-p_{b_j}^w]}{\partial \delta {\theta }_{b_i}} \\ =\frac{\partial R_b^c(-[\delta {\theta }_{b_i}{]}_{\times })R_w^{b_j}[R_{b_i}^w(R_c^b{f}_{c_i}+p_c^b)+p_{b_i}^w-p_{b_j}^w]}{\partial \delta {\theta }_{b_i}} \\ =R_b^c\{R_w^{b_j}[R_{b_i}^w(R_c^b{f}_{c_i}+p_c^b)+p_{b_i}^w-p_{b_j}^w]{\}}_{\times }={\mathbf{R}}_{bc}^{\top }{\left[{\mathbf{f}}_{b_j}\right]}_x \end{gathered}$$

• 对外参的平移$p_c^b$

$$\begin{gathered} \frac{\partial f_{c_j}}{\partial p_c^b}=\frac{\partial R_b^c\{R_w^{b_j}[R_{b_i}^w(R_c^b{f}_{c_i}+p_c^b)+p_{b_i}^w-p_{b_j}^w]-p_c^b\}}{\partial p_c^b} \\ =R_b^c(R_w^{b_j}{R}_{b_i}^w-I)={\mathbf{R}}_{bc}^{\top }\left({\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}-{\mathbf{I}}_{3\times 3}\right) \end{gathered}$$

• 对外参的旋转$q_c^b$
  $$f_{c_j}={\mathcal{P}}_l^{c_j}=R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b{f}_{c_i}+R_b^c(R_w^{b_j}(R_{b_i}^w{p}_c^b+p_{b_i}^w-p_{b_j}^w)-p_c^b)=f_{c_j}^1+f_{c_j}^2$$

分成两项分别求导。

$$\begin{gathered} \frac{\partial f_{c_j}^1}{\partial q_c^b}=\frac{\partial (R_c^{b}exp([\delta {\theta }_c^b{]}_{\times }){)}^{-1}{R}_w^{b_j}{R}_{b_i}^w{R}_c^{b}exp([\delta {\theta }_c^b{]}_{\times })f_{c_i}}{\partial \delta {\theta }_c^b} \\ =\frac{\partial (1-[\delta {\theta }_c^b{]}_{\times })R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b(1+[\delta {\theta }_c^b{]}_{\times })f_{c_i}}{\partial \delta {\theta }_c^b} \\ =\frac{\partial (-[\delta {\theta }_c^b{]}_{\times })R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b{f}_{c_i}+R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b([\delta {\theta }_c^b{]}_{\times })f_{c_i}+o^2(\delta {\theta }_c^b)}{\partial \delta {\theta }_c^b} \\ =[R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b{f}_{c_i}{]}_{\times }-R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b[f_{c_i}]\times \\ =-{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_k}{\mathbf{R}}_{bc}{\left[{\mathbf{f}}_{c_i}\right]}_x+{\left[{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}\right]}_x \end{gathered}$$
$$\begin{gathered} \frac{\partial f_{c_j}^2}{\partial q_c^b}=\frac{\partial (R_c^{b}exp([\delta {\theta }_c^b{]}_{\times }){)}^{-1}(R_w^{b_j}(R_{b_i}^w{p}_c^b+p_{b_i}^w-p_{b_j}^w)-p_c^b)}{\partial \delta {\theta }_c^b} \\ =\frac{\partial (-[\delta {\theta }_c^b{]}_{\times })R_b^c(R_w^{b_j}(R_{b_i}^w{p}_c^b+p_{b_i}^w-p_{b_j}^w)-p_c^b)}{\partial \delta {\theta }_c^b} \\ =[R_b^c(R_w^{b_j}(R_{b_i}^w{p}_c^b+p_{b_i}^w-p_{b_j}^w)-p_c^b){]}_{\times } \\ ={\left[{\mathbf{R}}_{bc}^{\top }\left({\mathbf{R}}_{w{b}_j}^{\top }\left(\left({\mathbf{R}}_{w{b}_i}{\mathbf{p}}_{bc}+{\mathbf{p}}_{w{b}_i}\right)-{\mathbf{p}}_{w{b}_j}\right)-{\mathbf{p}}_{bc}\right)\right]}_{\times } \end{gathered}$$

• 对逆深度$\lambda$
  $$\frac{\partial f_{c_j}}{\partial \lambda }=\frac{\partial f_{c_j}}{\partial f_{c_i}}\frac{\partial f_{c_i}}{\partial \lambda }=-R_b^c{R}_w^{b_j}{R}_{b_i}^w{R}_c^b\frac{1}{{\lambda }^2}\left[\begin{array}{c}{u}_{c_i}\\ v_{c_i}\\ 1\end{array}\right]=-\frac{1}{\lambda }{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}$$

---

4、协方差

视觉约束的协方差与标定相机内参时的重投影误差有关。VINS在代码中sqrt_info取1.5个像素，对应到归一化相机平面还需除以焦距f，最后得到的信息矩阵：（这里真正的信息矩阵其实是sqrt_info的平方）
$$sqrt({\Omega }_{vis})=sqrt({\Sigma }_{vis}^{-1})={\left(\frac{1.5}{f}{I}_{2\times 2}\right)}^{-1}=\frac{f}{1.5}{I}_{2\times 2}$$

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 测量残差

在IMU预积分中我们已经推导了：

$$R_w^{b_k}{p}_{b_{k+1}}^w=R_w^{b_k}(p_{b_k}^w+v_{b_k}^w\Delta t_k-\frac{1}{2}{g}^w\Delta t_k^2)+{\alpha }_{b_{k+1}}^{b_k}$$

$$R_w^{b_k}{v}_{b_{k+1}}^w=R_w^{b_k}(v_{b_k}^w-g^w\Delta t_k)+{\beta }_{b_{k+1}}^{b_k}$$

$$q_w^{b_k}\otimes q_{b_{k+1}}^w={\gamma }_{b_{k+1}}^{b_k}$$

因此其IMU的残差可以写成：

$${\mathbf{r}}_B({\mathbf{z}}_{b_{k+1}}^{b_k},\mathcal{X})=\left[\begin{array}{c}{\mathbf{r}}_p\\ {\mathbf{r}}_q\\ {\mathbf{r}}_v\\ {\mathbf{r}}_{ba}\\ {\mathbf{r}}_{bg}\end{array}\right]=\left[\begin{array}{c}{\mathbf{q}}_{b_{i}w}\left({\mathbf{p}}_{w{b}_j}-{\mathbf{p}}_{w{b}_i}-{\mathbf{v}}_i^w\Delta t+\frac{1}{2}{\mathbf{g}}^w\Delta t^2\right)-{\alpha }_{b_i{b}_j}\\ 2{\left[{\mathbf{q}}_{b_j{b}_i}\otimes \left({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}\\ {\mathbf{q}}_{b_{i}w}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)-{\beta }_{b_i{b}_j}\\ {\mathbf{b}}_j^a-{\mathbf{b}}_i^a\\ {\mathbf{b}}_j^g-{\mathbf{b}}_i^g\end{array}\right]$$

2、优化变量

优化变量主要包括第i、j时刻的p、q、v、ba、bg：

$$\left[p_{b_k}^w,q_{b_k}^w\right],\left[v_{b_k}^w,b_{a_k},b_{{\omega }_k}\right],\left[p_{b_{k+1}}^w,q_{b_{k+1}}^w\right],\left[v_{b_{k+1}}^w,b_{a_{k+1}},b_{{\omega }_{k+1}}\right]$$

3、Jacobian

这部分的结果建议直接看VINS的源码，在imu_factor.h中的class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> 的函数virtual bool Evaluate()中实现，其中parameters[0~3]分别对应了以上4组优化变量的参数块。

---

以下是部分证明：

对于pqv的求导可以直接采用对误差增量进行计算，而对ba、bg求导，因为i时刻的bias相关的预积分计算是通过迭代一步步递推的，直接求导太复杂，这里直接对预积分量在i时刻的bias附近用一阶泰勒展开来近似，而不是真的取迭代计算。

$$\begin{array}{rl}{\mathbf{\alpha }}_{b_i{b}_j}& ={\mathbf{\alpha }}_{b_i{b}_j}+{\mathbf{J}}_{b_i^a}^{\alpha }\delta {\mathbf{b}}_i^a+{\mathbf{J}}_{b_i^g}^{\alpha }\delta {\mathbf{b}}_i^g\\ {\mathbf{\beta }}_{b_i{b}_j}& ={\mathbf{\beta }}_{b_i{b}_j}+{\mathbf{J}}_{b_i^a}^{\beta }\delta {\mathbf{b}}_i^a+{\mathbf{J}}_{b_i^g}^{\beta }\delta {\mathbf{b}}_i^g\\ {\mathbf{q}}_{b_i{b}_j}& ={\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i^g}^q\delta {\mathbf{b}}_i^g\end{array}\right]\end{array}$$

关于bias的Jacobian可以根据IMU预积分讨论的协方差递推公式，一步步递推获得。
递推公式为：
$$J$$