OKVIS:Keyframe-Based Visual-Inertial SLAM Using Nonlinear Optimization

主要是看了范帝楷大佬在泡泡机器人SLAM中介绍。我把链接写到博客里便于自己以后查看。
【泡泡机器人原创专栏-OKVIS】OKVIS理论推导(上) https://mp.weixin.qq.com/s/FDMDleH8wszsD9HkBXwAnQ
【泡泡机器人原创专栏-OKVIS】OKVIS理论推导(下) https://mp.weixin.qq.com/s/UvRA_hOULokRgqJH1wk1iA

在OKVIS中的待估计的变量如下：
$${\mathbf{x}}_{\mathrm{R}}:={\left[{\mathbf{p}}_W^{W{S}^T},{\mathbf{q}}_{WS}^T,{\mathbf{v}}_W^{W{S}^T},{\mathbf{b}}_{\mathrm{g}}^T,{\mathbf{b}}_{\mathrm{a}}^T\right]}^T\in {\mathbb{R}}^3\times S^3\times {\mathbb{R}}^9$$
其中并没包含特征点，即特征点不参与优化。路标用其次坐标表示为
$${\mathbf{x}}_{\mathrm{L}}:=l_W^{WL}={\left[l_x,l_y,l_z,l_w\right]}^T\in {\mathbb{R}}^4$$
总的目标函数为
$$J(\mathbf{x}):=\sum _{i=1}^I\sum _{k=1}^K\sum _{j\in \mathcal{J}(i,k)}{\mathbf{e}}_{\mathrm{r}}^{i,j,k^T}{\mathbf{W}}_{\mathrm{r}}^{i,j,k}{\mathbf{e}}_{\mathrm{r}}^{i,j,k}+\sum _{k=1}^{K-1}{\mathbf{e}}_{\mathrm{s}}^{k^T}{\mathbf{W}}_{\mathrm{s}}^k{\mathbf{e}}_{\mathrm{s}}^k$$
分别对应重投影误差和IMU误差。
其中重投影误差不用讲了，IMU误差和VINS不同的是，这里是IMU积分预测的姿态和 “真实值” 之间的偏差（关于“真实值”的说明，对于滑窗内旧的关键帧，其实这个“真实值”就是之前优化后的姿态，而对于最新的那个关键帧，其实是根据IMU信息和上一帧位姿对最新关键帧预测得到的，这样的话按理说最后相邻两帧的IMU初始误差应该是0，但由于后续再和重投影误差联合优化的时候，为了使得整体的误差最小，最后两个关键帧的IMU误差可能会从0变成一个比较小的值。个人理解若有错误欢迎指正！）

关于IMU误差项协方差这一部分，VINS是直接用的IMU预积分的协方差，以为根据误差的定义的确是相等的。而okvis里的IMU误差项的协方差要在IMU积分的协方差上再用残差对IMU积分的雅克比进行一次更新，如下式所示
$${\mathbf{W}}_{\mathrm{s}}^k={\mathbf{R}}_{\mathrm{s}}^{k^{-1}}={\left(\frac{\partial {\mathbf{e}}_{\mathrm{s}}^k}{\partial \delta {\hat{\chi }}_{\mathrm{R}}^{k+1}}\mathbf{P}\left(\delta {\hat{\chi }}_{\mathrm{R}}^{k+1}|{\mathbf{x}}_{\mathrm{R}}^k,{\mathbf{z}}_{\mathrm{s}}^k\right)\frac{\partial {\mathbf{e}}_{\mathrm{s}}^k}{\partial \delta {\hat{\chi }}_{\mathrm{R}}^{k+1}}\right)}^{-1}$$
残差关于待优化状态量的雅克比矩阵，根据链式求导法则得
$$\begin{array}{rl}\frac{\partial {\mathbf{e}}_{\mathrm{s}}^k}{\partial \delta {\chi }_{\mathrm{R}}^k}=& {\mathbf{F}}_{\mathrm{d}}\left({\mathbf{x}}_{\mathrm{R}}^k,t\left(p^k\right)-t(k)\right)\left(\prod _{p=p^k}^{p^{k+1}-1}{\mathbf{F}}_{\mathrm{d}}\left({\hat{\mathbf{x}}}_{\mathrm{R}}^p,\Delta t\right)\right)\\ & {\mathbf{F}}_{\mathrm{d}}\left({\hat{\mathbf{x}}}_{\mathrm{R}}^{p^{k+1}-1},t(k+1)-t\left(p^{k+1}-1\right)\right)\frac{\partial {\mathbf{e}}_{\mathrm{s}}^k}{\partial \delta {\hat{\chi }}_{\mathrm{R}}^{\mathrm{k}+1}}\end{array}$$

边缘化

okvis在边缘化的过程中和VINS的不太一样，分两个窗口。而且OKVIS在优化的过程中会考虑FEJ的问题。
文中定义边缘化的状态量为$x_{\lambda }$,剩余变量$x_{\mu }$

$$\left[\begin{array}{cc}{\mathbf{H}}_{\mu \mu }& {\mathbf{H}}_{\mu {\lambda }_1}\\ {\mathbf{H}}_{{\lambda }_1\mu }& {\mathbf{H}}_{{\lambda }_1{\lambda }_1}\end{array}\right]\left[\begin{array}{c}\delta {\chi }_{\mu }\\ \delta {\chi }_{\lambda }\end{array}\right]=\left[\begin{array}{c}{\mathbf{b}}_{\mu }\\ {\mathbf{b}}_{{\lambda }_1}\end{array}\right]$$
对H矩阵进行舒尔补操作后得
$$\begin{array}{rl}{\mathbf{H}}_{{\lambda }_1{\lambda }_1}^{\ast }& :={\mathbf{H}}_{{\lambda }_1{\lambda }_1}-{\mathbf{H}}_{{\lambda }_1\mu }{\mathbf{H}}_{\mu \mu }^{-1}{\mathbf{H}}_{\mu {\lambda }_1}\\ {\mathbf{b}}_{{\lambda }_1}^{\ast }& :={\mathbf{b}}_{{\lambda }_1}-{\mathbf{H}}_{{\lambda }_1\mu }{\mathbf{H}}_{\mu \mu }^{-1}{\mathbf{b}}_{\mu }\end{array}$$
然后可以求得$\Delta {\chi }_{{\lambda }_1}$
根据当前状态量的估计值和线性化点时对应的$x_0$可以得到状态量的增量表示为
$$\Delta \chi :={\Phi }^{-1}\left(\log \left(\overline{\mathbf{x}}\square {\mathbf{x}}_0^{-1}\right)\right))$$
而状态量的实际值可以表示为
$$\mathbf{x}=\exp (\Phi (\delta \mathbf{\chi }))\square \underset{=\overline{\mathbf{x}}}{\underbrace{\exp (\Phi (\Delta \mathbf{\chi }))\square {\mathbf{x}}_0}}$$
因为后续剩余变量要不断的迭代更新进行优化，其中在边缘化后对那些与边缘化变量相连的剩余变量的H矩阵不再变化了，为了使得优化过程能正常进行下去，需要对b矩阵进行更新
对于状态增量$\Delta \chi$,b的更新公式如下所示
$$\mathbf{b}+{\frac{\partial \mathbf{b}}{\partial \Delta \chi }|}_{{\mathbf{x}}_0}\Delta \chi =\mathbf{b}-\mathbf{H}\Delta \chi$$
从而得在一次迭代之后，b矩阵更新如下
$$\left[\begin{array}{c}{\mathbf{b}}_{\mu }\\ {\mathbf{b}}_{{\lambda }_1}\end{array}\right]=\left[\begin{array}{l}{\mathbf{b}}_{\mu ,0}\\ {\mathbf{b}}_{{\lambda }_1,0}\end{array}\right]-\left[\begin{array}{ll}{\mathbf{H}}_{\mu \mu }& {\mathbf{H}}_{\mu {\lambda }_1}\\ {\mathbf{H}}_{{\lambda }_1\mu }{\mathbf{H}}_{{\lambda }_1{\lambda }_1}& \end{array}\right]\left[\begin{array}{l}\Delta {\chi }_{\mu }\\ \Delta {\chi }_{\lambda }\end{array}\right]$$
$${\mathbf{b}}_{{\lambda }_1}^{\ast }=\underset{{\mathbf{b}}_{{\lambda }_1,0}^{\ast }}{\underbrace{{\mathbf{b}}_{{\lambda }_1,0}-{\mathbf{H}}_{{\lambda }_1\mu }^T{\mathbf{H}}_{\mu \mu }^{-1}{\mathbf{b}}_{\mu ,0}}}-{\mathbf{H}}_{{\lambda }_1{\lambda }_1}^{\ast }\Delta {\chi }_{{\lambda }_1}$$
然后就可以不断的迭代求解了。