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]$$

将重投影方程拆成三维坐标形式：
P l c j = R b c { R w b j [ R b i w ( R c b 1 λ [ u c i v c i 1 ] + p c b ) + p b i w ] + p w b j } + p b c \mathcal{P}_{l}^{c_{j}}=R^c_b\{R^{b_j}_w[R^w_{b_i} (R^b_c \frac{1}{\lambda}\left[\begin{array}{c}{u_{c_{i}}} \\ {v_{c_{i}}} \\ {1}\end{array}\right] +p^b_c)+p^w_{b_i}]+p^{b_j}_w\}+p^c_b Plcj​​=Rbc​{Rwbj​​[Rbi​w​(Rcb​λ1​⎣⎡​uci​​vci​​1​⎦⎤​+pcb​)+pbi​w​]+pwbj​​}+pbc​

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

P l c j = R b c { R w b j [ R b i w ( R c b 1 λ [ u c i v c i 1 ] + p c b ) + p b i w ] − R w b j p b j w } − R b c p c b \mathcal{P}_{l}^{c_{j}}=R^c_b\{R^{b_j}_w[R^w_{b_i} (R^b_c \frac{1}{\lambda}\left[\begin{array}{c}{u_{c_{i}}} \\ {v_{c_{i}}} \\ {1}\end{array}\right] +p^b_c)+p^w_{b_i}]-R^{b_j}_wp_{b_j}^w\}-R_b^cp_c^b Plcj​​=Rbc​{Rwbj​​[Rbi​w​(Rcb​λ1​⎣⎡​uci​​vci​​1​⎦⎤​+pcb​)+pbi​w​]−Rwbj​​pbj​w​}−Rbc​pcb​

$$=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());
#elsedouble 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 } f_{c_j}=R^c_b\{R^{b_j}_w[R^w_{b_i} (R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]-p_c^b\} fcj​​=Rbc​{Rwbj​​[Rbi​w​(Rcb​fci​​+pcb​)+pbi​w​−pbj​w​]−pcb​}

• 对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$

∂ f c j ∂ q b i w = ∂ f c j ∂ δ θ b i = ∂ R b c { R w b j [ R b i w e x p ( [ δ θ b i ] × ) ( R c b f c i + p c b ) + p b i w − p b j w ] − p c b } ∂ δ θ b i = ∂ R b c R w b j R b i w e x p ( [ δ θ b i ] × ) ( R c b f c i + p c b ) ∂ δ θ b i = ∂ R b c R w b j R b i w ( [ δ θ b i ] × ) ( R c b f c i + p c b ) ∂ δ θ b i = − R b c R w b j R b i w [ R c b f c i + p c b ] × = − R b c ⊤ R w b j ⊤ R w b i [ f b i ] × \frac{\partial f_{c_j}}{\partial q_{b_{i}}^{w}}=\frac{\partial f_{c_j}}{\partial \delta \theta _{b_{i}}} = \frac{\partial R^c_b\{R^{b_j}_w[R^w_{b_i} exp([ \delta \theta _{b_{i}}]_{\times})(R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]-p_c^b\}}{\partial \delta \theta _{b_{i}}} \\ =\frac{\partial R^c_bR^{b_j}_wR^w_{b_i} exp([ \delta \theta _{b_{i}}]_{\times})(R^b_c f_{c_i} +p^b_c)}{\partial \delta \theta _{b_{i}}} \\=\frac{\partial R^c_bR^{b_j}_wR^w_{b_i} ([ \delta \theta _{b_{i}}]_{\times})(R^b_c f_{c_i} +p^b_c)}{\partial \delta \theta _{b_{i}}}\\ =-R^c_bR^{b_j}_wR^w_{b_i} [R^b_c f_{c_i} +p^b_c]_{\times}=-\mathbf{R}_{b c}^{\top} \mathbf{R}_{w b_{j}}^{\top} \mathbf{R}_{w b_{i}}\left[\mathbf{f}_{b_{i}}\right]_{ \times} ∂qbi​w​∂fcj​​​=∂δθbi​​∂fcj​​​=∂δθbi​​∂Rbc​{Rwbj​​[Rbi​w​exp([δθbi​​]×​)(Rcb​fci​​+pcb​)+pbi​w​−pbj​w​]−pcb​}​=∂δθbi​​∂Rbc​Rwbj​​Rbi​w​exp([δθbi​​]×​)(Rcb​fci​​+pcb​)​=∂δθbi​​∂Rbc​Rwbj​​Rbi​w​([δθbi​​]×​)(Rcb​fci​​+pcb​)​=−Rbc​Rwbj​​Rbi​w​[Rcb​fci​​+pcb​]×​=−Rbc⊤​Rwbj​⊤​Rwbi​​[fbi​​]×​

• 对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$

∂ f c j ∂ q b j w = ∂ f c j ∂ δ θ b j = ∂ R b c { ( R b j w e x p ( [ δ θ b j ] × ) ) − 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 } ∂ δ θ b i = ∂ R b c e x p ( − [ δ θ b j ] × ) 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 ] ∂ δ θ b i = ∂ R b c ( − [ δ θ b i ] × ) 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 ] ∂ δ θ 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 ] } × = R b c ⊤ [ f b j ] x \frac{\partial f_{c_j}}{\partial q_{b_{j}}^{w}}=\frac{\partial f_{c_j}}{\partial \delta \theta _{b_{j}}} = \frac{\partial R^c_b\{ (R_{b_j}^w exp([ \delta \theta _{b_{j}}]_{\times}))^{-1}[R^w_{b_i} (R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]-p_c^b\}}{\partial \delta \theta _{b_{i}}} \\ =\frac{\partial R^c_b exp(-[ \delta \theta _{b_{j}}]_{\times}) R^{b_j}_w[R^w_{b_i}(R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]}{\partial \delta \theta _{b_{i}}} \\=\frac{\partial R^c_b (-[ \delta \theta _{b_{i}}]_{\times}) R^{b_j}_w[R^w_{b_i}(R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]}{\partial \delta \theta _{b_{i}}}\\ =R^c_b \{ R^{b_j}_w[R^w_{b_i}(R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]\}_{\times}=\mathbf{R}_{b c}^{\top}\left[\mathbf{f}_{b_{j}}\right]_{x} ∂qbj​w​∂fcj​​​=∂δθbj​​∂fcj​​​=∂δθbi​​∂Rbc​{(Rbj​w​exp([δθbj​​]×​))−1[Rbi​w​(Rcb​fci​​+pcb​)+pbi​w​−pbj​w​]−pcb​}​=∂δθbi​​∂Rbc​exp(−[δθbj​​]×​)Rwbj​​[Rbi​w​(Rcb​fci​​+pcb​)+pbi​w​−pbj​w​]​=∂δθbi​​∂Rbc​(−[δθbi​​]×​)Rwbj​​[Rbi​w​(Rcb​fci​​+pcb​)+pbi​w​−pbj​w​]​=Rbc​{Rwbj​​[Rbi​w​(Rcb​fci​​+pcb​)+pbi​w​−pbj​w​]}×​=Rbc⊤​[fbj​​]x​

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

∂ f c j ∂ p c b = ∂ 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 } ∂ p c b = R b c ( R w b j R b i w − I ) = R b c ⊤ ( R w b j ⊤ R w b i − I 3 × 3 ) \frac{\partial f_{c_j}}{\partial p^b_{c}}=\frac{\partial R^c_b\{R^{b_j}_w[R^w_{b_i} (R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]-p_c^b\} }{\partial p^b_{c}}\\ = R^c_b(R^{b_j}_w R^w_{b_i}-I)=\mathbf{R}_{b c}^{\top}\left(\mathbf{R}_{w b_{j}}^{\top} \mathbf{R}_{w b_{i}}-\mathbf{I}_{3 \times 3}\right) ∂pcb​∂fcj​​​=∂pcb​∂Rbc​{Rwbj​​[Rbi​w​(Rcb​fci​​+pcb​)+pbi​w​−pbj​w​]−pcb​}​=Rbc​(Rwbj​​Rbi​w​−I)=Rbc⊤​(Rwbj​⊤​Rwbi​​−I3×3​)

• 对外参的旋转$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_{k+1}=F{J}_k，J_0=I$$

---

接下来推导关于第i、j帧PVQ的Jacobian：

由于${\mathbf{r}}_p$和${\mathbf{r}}_v$的推法基本相似，这里只推导${\mathbf{r}}_v$，且结果不为0的项：

$${\mathbf{r}}_v={\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}$$

• 对i时刻旋转的Jacobian

$$\begin{gathered} \frac{\partial {\mathbf{r}}_v}{\partial \delta {\theta }_{b_i}}=\frac{\partial ({\mathbf{R}}_{w{b}_i}exp([\delta {\theta }_{b_i}]\times ){)}^{-1}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)}{\partial \delta {\theta }_{b_i}} \\ =\frac{\partial (-[\delta {\theta }_{b_i}]\times ){\mathbf{R}}_{b_{i}w}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)}{\partial \delta {\theta }_{b_i}} \\ =[{\mathbf{R}}_{b_{i}w}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right){]}_{\times } \end{gathered}$$

• 对i时刻速度的Jacobian

$$\frac{\partial {\mathbf{r}}_v}{\partial \delta {\mathbf{v}}_i^w}=-{\mathbf{R}}_{b_{i}w}$$

• 对j时刻速度的Jacobian

$$\frac{\partial {\mathbf{r}}_v}{\partial \delta {\mathbf{v}}_j^w}={\mathbf{R}}_{b_{i}w}$$

---

然后再推${\mathbf{r}}_q$：

$${\mathbf{r}}_q=2{\left[{\mathbf{q}}_{b_j{b}_i}\otimes \left({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}$$

• 对i时刻旋转的Jacobian
  这边的写法可能不太严谨，能理解就好。

$$\begin{gathered} \frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{\theta }}_{b_i}}=\frac{\partial 2{\left[{\mathbf{q}}_{b_j{b}_i}\otimes \left({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_i}} \\ =\frac{\partial 2{\left[{\mathbf{q}}_{b_j{b}_i}\otimes {\left({\mathbf{q}}_{b_{i}w}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i}\end{array}\right]\right)}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_i}} \\ =\frac{\partial -2{\left[{\mathbf{q}}_{b_{j}w}\otimes \left({\mathbf{q}}_{b_{i}w}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i}\end{array}\right]\right)\otimes {\mathbf{q}}_{b_i{b}_j}\right]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_i}} \\ =-2\left[\begin{array}{ll}0& \mathbf{I}\end{array}\right]\frac{\partial {\mathbf{q}}_{b_{j}w}\otimes \left({\mathbf{q}}_{b_{i}w}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_i}\end{array}\right]\right)\otimes {\mathbf{q}}_{b_i{b}_j}}{\partial \delta {\mathbf{\theta }}_{b_i}} \\ =-2\left[\begin{array}{ll}0& \mathbf{I}\end{array}\right]{\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\right]}_L{\left[{\mathbf{q}}_{b_i{b}_j}\right]}_R\left[\begin{array}{c}0\\ \frac{1}{2}\mathbf{I}\end{array}\right] \end{gathered}$$

$$\frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{\theta }}_{b_i}}={\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\right]}_{L3\times 3}{\left[{\mathbf{q}}_{b_i{b}_j}\right]}_{R3\times 3}$$

• 对j时刻旋转的Jacobian

$$\begin{gathered} \frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{\theta }}_{b_j}}=\frac{\partial 2{\left[{\mathbf{q}}_{b_j{b}_i}\otimes \left({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_j}} \\ =\frac{\partial 2{\left[{\mathbf{q}}_{b_j{b}_i}\otimes \left({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}\left[\begin{array}{c}1\\ \frac{1}{2}\delta {\mathbf{\theta }}_{b_j}\end{array}\right]\right)\right]}_{xyz}}{\partial \delta {\mathbf{\theta }}_{b_j}} \\ =2\left[\begin{array}{ll}0& \mathbf{I}\end{array}\right]{\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right]}_L\left[\begin{array}{c}0\\ \frac{1}{2}\mathbf{I}\end{array}\right] \end{gathered}$$

$$\frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{\theta }}_{b_j}}={\left[{\mathbf{q}}_{b_i{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}^{\ast }\otimes {\mathbf{q}}_{w{b}_j}\right]}_{L3\times 3}$$

• 对i时刻陀螺仪bias的Jacobian

$$\begin{gathered} \frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{b}}_i^g}=\frac{\partial 2{\left[{\mathbf{q}}_{b_j{b}_i}\otimes \left({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}}{\partial \delta {\mathbf{b}}_i^g} \\ =\frac{\partial 2{\left[({\mathbf{q}}_{b_i{b}_j}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b_i}^q\delta {\mathbf{b}}_i^g\end{array}\right]{)}^{\ast }\otimes \left({\mathbf{q}}_{b_{i}w}\otimes {\mathbf{q}}_{w{b}_j}\right)\right]}_{xyz}}{\partial \delta {\mathbf{b}}_i^g} \\ =-2\left[\begin{array}{ll}0& \mathbf{I}\end{array}\right]{\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_j}\right]}_L\left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{J}}_{b_i^g}^q\end{array}\right] \end{gathered}$$

$$\frac{\partial {\mathbf{r}}_q}{\partial \delta {\mathbf{b}}_i^g}=-{\left[{\mathbf{q}}_{w{b}_j}^{\ast }\otimes {\mathbf{q}}_{w{b}_i}\otimes {\mathbf{q}}_{b_i{b}_j}\right]}_{L3\times 3}{\mathbf{J}}_{b_i^g}^q$$

4、协方差

协方差即为IMU预积分中迭代得到的IMU增量误差的协方差。
对IMU增量误差递推方程：

$${\delta z_{k+1}}^{15\times 1}=F^{15\times 15}{\delta z_k}^{15\times 1}+V^{15\times 18}{n}^{18\times 1}\text{\hspace{1em}(*)}$$

协方差迭代公式为：
$$P_{k+1}=F{P}_k{F}^T+VQ{V}^T，P_0=0$$

初始值$P_k=0$，噪声项的对角协方差矩阵Q为：$Q^{18\times 18}=({\sigma }_a^2,{\sigma }_w^2,{\sigma }_a^2,{\sigma }_w^2,{\sigma }_{b_a}^2,{\sigma }_{b_w}^2)$。

总结：其实对于VIO的非线性优化中残差的Jacobian的推导，包括IMU预积分增量递推方程的推导，看起来很复杂，其实就是在明确被求导的对象，以及关于什么状态量求导后，如果是和位姿（四元数）有关，就把旋转转换到李群上，用扰动模型去推，如果是其他的，就直接用状态量(或状态量的误差增量)求偏导。所以从实际操作上来说，可能就像高博说的那样，只需要准备一张比较长的纸就行了。