VINS-Mono(二)——优化问题建模

 

---

Reference

1. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator.
2. Quaternion Kinematics for Error-State KF.

 

---

视觉部分的优化问题建模

视觉部分的问题就是一个BA问题，不过与纯视觉不同，这里涉及到两个坐标系之间的变换，分别是camera坐标系C和IMU坐标系（通常认为这个是机体坐标系）b，同时因为整个过程中使用的是逆深度表示空间中的Landmark，因此这个投影过程会变得稍微麻烦一些，但是整个过程还是比较清晰的。
$$\begin{array}{r}\{\begin{array}{ll}{P}_{ci}& =\frac{f_{ci}}{\rho }\\ P_{bi}& =R_c^b{P}_{ci}+t_c^b\\ P_w& =R_{bi}^w{P}_{bi}+t_{bi}^w\\ P_{bj}& =(R_{bj}^w{)}^T(P_w-t_{bj}^w)\\ P_{cj}& =(R_c^b{)}^T(P_{bj}-t_c^b)={\left[X_j,Y_j,Z_j\right]}^T\\ p_{cj}& ={\left[\frac{X_j}{Z_j},\frac{Y_j}{Z_j}\right]}^T\\ erro{r}_{ij}& ={\left[\frac{X_j}{Z_j}-u_j,\frac{Y_j}{Z_j}-v_j\right]}^T\end{array}\end{array}\text{\hspace{1em}(1)}$$
于是可以逐个求解Jacobian矩阵了：

对第 i 帧的位姿进行求导

$$\begin{array}{rl}\frac{\partial e_{ij}}{\partial T_{bi}^w}& =\left[\frac{\partial e_{ij}}{\partial t_{bi}^w},\frac{\partial e_{ij}}{\partial R_{bi}^w}\right]\\ & =\frac{\partial e_{ij}}{\partial p_{cj}}\frac{\partial p_{cj}}{\partial P_{cj}}\frac{\partial P_{cj}}{\partial P_{bj}}\frac{\partial P_{bj}}{\partial P_w}\left[\frac{\partial P_w}{\partial t_{bi}^w},\frac{\partial P_w}{\partial R_{bi}^w}\right]\\ & =I_{2\times 2}{\left[\begin{array}{ccc}\frac{1}{Z_j}& 0& -\frac{X_j}{Z_j^2}\\ 0& \frac{1}{Z_j}& -\frac{Y_j}{Z_j^2}\end{array}\right]}_{2\times 3}(R_c^b{)}_{3\times 3}^T(R_{bj}^w{)}_{3\times 3}^T{\left[I,-R_{bi}^w[P_{bi}{]}_{\times }\right]}_{3\times 6}\end{array}\text{\hspace{1em}(2)}$$

对第 j 帧的位姿进行求导

$$\begin{array}{rl}\frac{\partial e_{ij}}{\partial T_{bj}^w}& =\left[\frac{\partial e_{ij}}{\partial t_{bj}^w},\frac{\partial e_{ij}}{\partial R_{bj}^w}\right]\\ & =\frac{\partial e_{ij}}{\partial p_{cj}}\frac{\partial p_{cj}}{\partial P_{cj}}\frac{\partial P_{cj}}{\partial P_{bj}}\left[\frac{\partial P_{bj}}{\partial t_{bj}^w},\frac{\partial P_{bj}}{\partial R_{bj}^w}\right]\\ & =I_{2\times 2}{\left[\begin{array}{ccc}\frac{1}{Z_j}& 0& -\frac{X_j}{Z_j^2}\\ 0& \frac{1}{Z_j}& -\frac{Y_j}{Z_j^2}\end{array}\right]}_{2\times 3}(R_c^b{)}_{3\times 3}^T{\left[-(R_{bj}^w{)}^T,[P_{bj}{]}_{\times }\right]}_{3\times 6}\end{array}\text{\hspace{1em}(3)}$$

对逆深度$\rho$进行求导

$$\begin{array}{rl}\frac{\partial e_{ij}}{\partial \rho }& =\frac{\partial e_{ij}}{\partial p_{cj}}\frac{\partial p_{cj}}{\partial P_{cj}}\frac{\partial P_{cj}}{\partial P_{bj}}\frac{\partial P_{bj}}{\partial P_w}\frac{\partial P_w}{\partial P_{bi}}\frac{\partial P_{bi}}{\partial P_{ci}}\frac{\partial P_{ci}}{\partial \rho }\\ & =I_{2\times 2}{\left[\begin{array}{ccc}\frac{1}{Z_j}& 0& -\frac{X_j}{Z_j^2}\\ 0& \frac{1}{Z_j}& -\frac{Y_j}{Z_j^2}\end{array}\right]}_{2\times 3}(R_c^b{)}_{3\times 3}^T(R_{bj}^w{)}_{3\times 3}^T(R_{bi}^w{)}_{3\times 3}(R_c^b{)}_{3\times 3}(-\frac{f_i}{{\rho }^2}{)}_{3\times 1}\end{array}\text{\hspace{1em}(4)}$$

对IMU和Camera的外参进行求导

$$\begin{array}{rl}\frac{\partial e_{ij}}{\partial T_c^b}& =\left[\frac{\partial e_{ij}}{\partial t_c^b},\frac{\partial e_{ij}}{\partial R_c^b}\right]\\ & =\frac{\partial e_{ij}}{\partial p_{cj}}\frac{\partial p_{cj}}{\partial P_{cj}}\frac{\partial P_{cj}}{\partial T_c^b}\\ & =\frac{\partial e_{ij}}{\partial p_{cj}}\frac{\partial p_{cj}}{\partial P_{cj}}(\frac{\partial P_{cj}}{\partial T_c^b}+(R_c^b{)}^T\frac{\partial P_{bj}}{\partial T_c^b})\\ & =\frac{\partial e_{ij}}{\partial p_{cj}}\frac{\partial p_{cj}}{\partial P_{cj}}(\frac{\partial P_{cj}}{\partial T_c^b}+(R_c^b{)}^T\frac{\partial P_{bj}}{\partial P_w}\frac{\partial P_w}{\partial P_{bi}}\frac{\partial P_{bi}}{\partial T_c^b})\\ & =I_{2\times 2}{\left[\begin{array}{ccc}\frac{1}{Z_j}& 0& -\frac{X_j}{Z_j^2}\\ 0& \frac{1}{Z_j}& -\frac{Y_j}{Z_j^2}\end{array}\right]}_{2\times 3}{\left(\left[-(R_c^b{)}^T,[P_{cj}{]}_{\times }\right]+(R_c^b{)}^T(R_{bj}^w{)}^T{R}_{bi}^w\left[I,-R_c^b[P_{ci}{]}_{\times }\right]\right)}_{3\times 6}\\ & =I_{2\times 2}{\left[\begin{array}{ccc}\frac{1}{Z_j}& 0& -\frac{X_j}{Z_j^2}\\ 0& \frac{1}{Z_j}& -\frac{Y_j}{Z_j^2}\end{array}\right]}_{2\times 3}{\left(\left[(R_c^b{)}^T((R_{bj}^w{)}^T{R}_{bi}^w-I),([P_{cj}{]}_{\times }-(R_c^b{)}^T(R_{bj}^w{)}^T{R}_{bi}^w{R}_c^b[P_{ci}{]}_{\times })\right]\right)}_{3\times 6}\end{array}\text{\hspace{1em}(5)}$$

在VINS-Mono的代码中，作者把$[P_{ci}{]}_{\times }$给分解了，这块儿不太理解作者为什么要分解开，但是其实展开之后的结果是一样的。

 

---

惯导部分的优化问题建模

惯导部分的建模也是一个最小化误差的问题，该误差部分由两个部分组成，一部分是状态变量组成的帧间间隔，这部分与位置、角度、速度相关，与bias不相关：
$$\begin{array}{r}\{\begin{array}{ll}{p}_{bj}^{bi}& =(R_{bi}^w{)}^T\left[({}^w{p}_{bj}-{}^w{p}_{bi})-{}^w{v}_{bi}\Delta t+\frac{1}{2}{}^{w}g\Delta t^2\right]\\ v_{bj}^{bi}& =(R_{bi}^w{)}^T\left[({}^w{v}_{bj}-{}^w{v}_{bi})+{}^{w}g\Delta t\right]\\ q_{bj}^{bi}& =(q_{bi}^w{)}^{-1}\otimes (q_{bj}^w)\\ b{a}_j& =b{a}_i\\ b{g}_j& =b{g}_i\end{array}\end{array}\text{\hspace{1em}(6)}$$
另一部分是IMU的积分值 { p ‾ b j b i , v ‾ b j b i , q ‾ b j b i } \{\overline{p}^{bi}_{bj}, \overline{v}^{bi}_{bj}, \overline{q}^{bi}_{bj}\} {p​bjbi​,vbjbi​,q​bjbi​}，这部分因为是在$bi$系下的积分，因此与位置、角度、速度（均在世界坐标系下）无关，但是这部分与bias息息相关，所以这部分笔者认为不能叫观测值；

所以整个问题的误差为：
$$\begin{array}{r}\{\begin{array}{ll}{e}_p& =p_{bj}^{bi}-{\overline{p}}_{bj}^{bi}=(R_{bi}^w{)}^T\left[({}^w{p}_{bj}-{}^w{p}_{bi})-{}^w{v}_{bi}\Delta t+\frac{1}{2}{}^{w}g\Delta t^2\right]-{\overline{p}}_{bj}^{bi}=(R_{bi}^w{)}^T{}^w\Delta p-{\overline{p}}_{bj}^{bi}\\ e_q& =2[({\overline{q}}_{bj}^{bi}{)}^{-1}\otimes q_{bj}^{bi}{]}_{3\times 1}\\ e_v& =v_{bj}^{bi}-{\overline{v}}_{bj}^{bi}=(R_{bi}^w{)}^T({}^w{v}_{bj}-{}^w{v}_{bi}+{}^{w}g\Delta t)-{\overline{v}}_{bj}^{bi}=(R_{bi}^w{)}^T{}^w\Delta v-{\overline{v}}_{bj}^{bi}\\ e_{ba}& =b{a}_j-b{a}_i\\ e_{bg}& =b{g}_j-b{g}_i\end{array}\end{array}\text{\hspace{1em}(7)}$$
我这里稍微有一个问题就是在代码中的姿态误差$e_q$和论文中的公式并不相同，起初笔者还是比较在意这个事情的，但是后来个人认为只要整个推导过程中按照一个公式来就可以了。

所以根据公式（7）对状态变量进行求导（按照VINS-Mono中的组织形式）：

误差部分对 i 时刻状态量的求导

误差部分对于i 时刻位姿状态量的求导

$$\begin{array}{rl}\frac{\partial e}{\partial T_{bi}}& =\left[\frac{\partial e}{\partial p_{bi}},\frac{\partial e}{\partial {\theta }_{bi}}\right]\\ & =\left[\begin{array}{cc}\frac{\partial e_p}{\partial p_{bi}}& \frac{\partial e_p}{\partial {\theta }_{bi}}\\ \frac{\partial e_q}{\partial p_{bi}}& \frac{\partial e_q}{\partial {\theta }_{bi}}\\ \frac{\partial e_v}{\partial p_{bi}}& \frac{\partial e_v}{\partial {\theta }_{bi}}\\ \frac{\partial e_{ba}}{\partial p_{bi}}& \frac{\partial e_{ba}}{\partial {\theta }_{bi}}\\ \frac{\partial e_{bg}}{\partial p_{bi}}& \frac{\partial e_{bg}}{\partial {\theta }_{bi}}\end{array}\right]\\ & =\left[\begin{array}{cc}-(R_{bi}^w{)}^T& [(R_{bi}^w{)}^T\Delta p{]}_{\times }\\ 0& -{\left[[q_i^j{]}_{\mathrm{L}}[{\overline{q}}_j^i\otimes {\mathrm{J}}_{bg}^q\delta b{g}_i{]}_{\mathrm{R}}\right]}_{3\times 3}\\ 0& [(R_{bi}^w{)}^T{}^w\Delta v{]}_{\times }\\ 0& 0\\ 0& 0\end{array}\right]\end{array}\text{\hspace{1em}(8)}$$

这里简单推导一下姿态误差对姿态的求导：
$$\begin{array}{rl}e(\theta +\delta \theta )& =2[({\overline{q}}_j^i{)}^{-1}\otimes [(q_i^w\otimes q(\delta \theta ){)}^{-1}\otimes q_j^w]{]}_{3\times 1}\\ & =2[({\overline{q}}_j^i{)}^{-1}\otimes q(\delta \theta {)}^{-1}\otimes q_j^i{]}_{3\times 1}\\ & =2[[(q_j^i{)}^{-1}\otimes q(\delta \theta )\otimes {\overline{q}}_j^i){]}^{-1}{]}_{3\times 1}\\ & =2[[[q_i^j{]}_{\mathrm{L}}[{\overline{q}}_j^i{]}_{\mathrm{R}}\left[\begin{array}{c}1\\ \frac{1}{2}\delta \theta \end{array}\right]{]}^{-1}{]}_{3\times 1}\\ & =2[[q_i^j{]}_{\mathrm{L}}[{\overline{q}}_j^i{]}_{\mathrm{R}}\left[\begin{array}{c}1\\ -\frac{1}{2}\delta \theta \end{array}\right]{]}_{3\times 1}\\ & =2[[q_i^j{]}_{\mathrm{L}}[{\overline{q}}_j^i{]}_{\mathrm{R}}\left[\begin{array}{c}1\\ 0\end{array}\right]+[q_i^j{]}_{\mathrm{L}}[{\overline{q}}_j^i{]}_{\mathrm{R}}\left[\begin{array}{c}0\\ -\frac{1}{2}\delta \theta \end{array}\right]{]}_{3\times 1}\\ & =e(\theta )+[[q_i^j{]}_{\mathrm{L}}[{\overline{q}}_j^i{]}_{\mathrm{R}}\left[\begin{array}{c}0\\ -\delta \theta \end{array}\right]{]}_{3\times 1}\end{array}$$
这部分其实看到中间取了一个求逆操作，主要是为了和VINS-Mono中的公式对上。

所以采用求导的原始定义有：
$$\frac{\partial e_q}{\partial \theta }=\mathrm{l}\mathrm{i}\mathrm{m}\frac{e(\theta +\delta \theta )-e(\theta )}{\delta \theta }=-[[q_i^j{]}_{\mathrm{L}}[{\overline{q}}_j^i{]}_{\mathrm{R}}{]}_{3\times 3}$$

误差部分对于 i 时刻运动状态量的求导

$$\begin{array}{rl}\frac{\partial e}{\partial M_{bi}}& =\left[\frac{\partial e}{\partial v_{bi}},\frac{\partial e}{\partial b{a}_i},\frac{\partial e}{\partial b{g}_i}\right]\\ & =\left[\begin{array}{ccc}\frac{\partial e_p}{\partial v_{bi}}& \frac{\partial e_p}{\partial b{a}_i}& \frac{\partial e_p}{\partial b{g}_i}\\ \frac{\partial e_q}{\partial v_{bi}}& \frac{\partial e_q}{\partial b{a}_i}& \frac{\partial e_q}{\partial b{g}_i}\\ \frac{\partial e_v}{\partial v_{bi}}& \frac{\partial e_v}{\partial b{a}_i}& \frac{\partial e_v}{\partial b{g}_i}\\ \frac{\partial e_{ba}}{\partial v_{bi}}& \frac{\partial e_{ba}}{\partial b{a}_i}& \frac{\partial e_{ba}}{\partial b{g}_i}\\ \frac{\partial e_{bg}}{\partial v_{bi}}& \frac{\partial e_{bg}}{\partial b{a}_i}& \frac{\partial e_{bg}}{\partial b{g}_i}\end{array}\right]\\ & =\left[\begin{array}{ccc}-(R_{bi}^w{)}^T\Delta t& -J_{b{a}_i}^p& -J_{b{g}_i}^p\\ 0& -J_{b{a}_i}^q& -[[q_i^j\otimes {\overline{q}}_j^i{]}_{\mathrm{L}}{]}_{3\times 3}{J}_{b{g}_i}^q\\ -(R_{bi}^w{)}^T& -J_{b{a}_i}^v& -J_{b{g}_i}^v\\ 0& -I& 0\\ 0& 0& -I\end{array}\right]\end{array}\text{\hspace{1em}(9)}$$

依旧简单推导一下姿态误差对角速度bias的求导过程：
$$\begin{array}{rl}e(bg+\delta bg)& =2[({\overline{q}}_j^i\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{bg}^q\delta bg\end{array}\right]{)}^{-1}\otimes q_j^i{]}_{3\times 1}\\ & =2[{\left[\begin{array}{c}1\\ \frac{1}{2}{J}_{bg}^q\delta bg\end{array}\right]}^{-1}({\overline{q}}_j^i{)}^{-1}\otimes q_j^i{]}_{3\times 1}\\ & =2[[(q_j^i{)}^{-1}\otimes {\overline{q}}_j^i)\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{J}_{bg}^q\delta bg\end{array}\right]{]}^{-1}{]}_{3\times 1}\\ & =2[[[q_i^j\otimes {\overline{q}}_j^i{]}_{\mathrm{L}}\left[\begin{array}{c}1\\ \frac{1}{2}{J}_{bg}^q\delta bg\end{array}\right]{]}^{-1}{]}_{3\times 1}\\ & =2[[q_i^j\otimes {\overline{q}}_j^i{]}_{\mathrm{L}}\left[\begin{array}{c}1\\ -\frac{1}{2}{J}_{bg}^q\delta bg\end{array}\right]{]}_{3\times 1}\\ & =2[[q_i^j\otimes {\overline{q}}_j^i{]}_{\mathrm{L}}\left[\begin{array}{c}1\\ 0\end{array}\right]{]}_{3\times 1}+2[[q_i^j\otimes {\overline{q}}_j^i{]}_{\mathrm{L}}\left[\begin{array}{c}0\\ -\frac{1}{2}{J}_{bg}^q\delta bg\end{array}\right]{]}_{3\times 1}\\ & =e(bg)+[[q_i^j\otimes {\overline{q}}_j^i{]}_{\mathrm{L}}\left[\begin{array}{c}0\\ -J_{bg}^q\delta bg\end{array}\right]{]}_{3\times 1}\end{array}$$
这部分很重要的一定是通过四元数的求逆法则将$\delta bg$转移到公式的最后，最后再通过四元数求逆其实就是把四元数的虚部部分添加负号的方法把求逆符号由公式外转到宫室内。

同样使用求导的定义有：
$$\frac{\partial e_q}{\partial bg}=\mathrm{l}\mathrm{i}\mathrm{m}\frac{e(bg+\delta bg)-e(bg)}{\delta bg}=-[[q_i^j\otimes {\overline{q}}_j^i{]}_{\mathrm{L}}{]}_{3\times 3}{J}_{bg}^q$$

误差部分对 j 时刻状态量的求导

误差部分对于 j 时刻位姿状态量的求导

$$\begin{array}{rl}\frac{\partial e}{\partial T_{bj}}& =\left[\frac{\partial e}{\partial p_{bj}},\frac{\partial e}{\partial {\theta }_{bj}}\right]\\ & =\left[\begin{array}{cc}\frac{\partial e_p}{\partial p_{bj}}& \frac{\partial e_p}{\partial {\theta }_{bj}}\\ \frac{\partial e_q}{\partial p_{bj}}& \frac{\partial e_q}{\partial {\theta }_{bj}}\\ \frac{\partial e_v}{\partial p_{bj}}& \frac{\partial e_v}{\partial {\theta }_{bj}}\\ \frac{\partial e_{ba}}{\partial p_{bj}}& \frac{\partial e_{ba}}{\partial {\theta }_{bj}}\\ \frac{\partial e_{bg}}{\partial p_{bj}}& \frac{\partial e_{bg}}{\partial {\theta }_{bj}}\end{array}\right]\\ & =\left[\begin{array}{cc}(R_{bi}^w{)}^T& 0\\ 0& {\left[{\left[({\overline{q}}_j^i\otimes {\mathrm{J}}_{bg}^q\delta b{g}_i{)}^{-1}\otimes q_j^i\right]}_{\mathrm{R}}\right]}_{3\times 3}\\ 0& 0\\ 0& 0\\ 0& 0\end{array}\right]\end{array}\text{\hspace{1em}(10)}$$

简单推导一下姿态误差对姿态的求导：
$$\begin{array}{rl}e(\theta +\delta \theta )& =2[({\overline{q}}_j^i{)}^{-1}\otimes (q_i^w{)}^{-1}\otimes q_j^w\otimes q(\delta \theta ){]}_{3\times 1}\\ & =2[({\overline{q}}_j^i{)}^{-1}\otimes q_j^i\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta \theta \end{array}\right]{]}_{3\times 1}\\ & =2[[({\overline{q}}_j^i{)}^{-1}\otimes q_j^i{]}_{\mathrm{L}}\left[\begin{array}{c}1\\ \frac{1}{2}\delta \theta \end{array}\right]{]}_{3\times 1}\\ & =e(\theta )+[[({\overline{q}}_j^i{)}^{-1}\otimes q_j^i{]}_{\mathrm{L}}\left[\begin{array}{c}0\\ \delta \theta \end{array}\right]{]}_{3\times 1}\end{array}$$

误差部分对于 j 时刻运动状态量的求导

$$\begin{array}{rl}\frac{\partial e}{\partial M_{bj}}& =\left[\frac{\partial e}{\partial v_{bj}},\frac{\partial e}{\partial b{a}_j},\frac{\partial e}{\partial b{g}_j}\right]\\ & =\left[\begin{array}{ccc}\frac{\partial e_p}{\partial v_{bj}}& \frac{\partial e_p}{\partial b{a}_j}& \frac{\partial e_p}{\partial b{g}_j}\\ \frac{\partial e_q}{\partial v_{bj}}& \frac{\partial e_q}{\partial b{a}_j}& \frac{\partial e_q}{\partial b{g}_j}\\ \frac{\partial e_v}{\partial v_{bj}}& \frac{\partial e_v}{\partial b{a}_j}& \frac{\partial e_v}{\partial b{g}_j}\\ \frac{\partial e_{ba}}{\partial v_{bj}}& \frac{\partial e_{ba}}{\partial b{a}_j}& \frac{\partial e_{ba}}{\partial b{g}_j}\\ \frac{\partial e_{bg}}{\partial v_{bj}}& \frac{\partial e_{bg}}{\partial b{a}_j}& \frac{\partial e_{bg}}{\partial b{g}_j}\end{array}\right]\\ & =\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ (R_{bi}^w{)}^T& 0& 0\\ 0& I& 0\\ 0& 0& I\end{array}\right]\end{array}\text{\hspace{1em}(11)}$$

小结一下惯性部分的建模问题

1. 这部分的误差模型其实由两个部分组成，一部分是状态量构建的帧间变化量（与位姿和速度直接相关），另一部分是IMU预积分得到的帧间变化量，这部分由于是以 i 时刻作为参考系，因此和位姿速度无直接关系，但是在预积分过程中和角速度计和角速度计的bias息息相关；
2. 在预积分过程中，虽然使用了精度较高的中点法，但是整个预积分过程中并没有涉及到 j 时刻的零偏，或者说认为 j 时刻的零偏与 i 时刻是一样的，所以看到误差部分对 j 时刻状态的求导是比较简单的；

 

---

小结

建模这部分就先到这里，下部分关注的重点就是边缘化和优化问题的先验部分了。