视觉惯性融合学习笔记三 VINS Mono初始化方法

这篇笔记会记录论文里的关于初始化的方法，以及自己的一些心得，欢迎交流(o◕∀◕)ﾉ。

一、概述

这次论文的初始化方法与VI_ORB的初始化方法相似，但是相比之下速度更快，适合需要快速初始化视觉惯性导航系统（VINS）的场合。

二、VINS-mono初始化方法

方法来源：Tong Qin在2017年发表的论文Robust Initialization of Monocular Visual-Inertial
Estimation on Aerial Robots。

Tong Qin在论文里把初始化问题分成3步来解决：（1）陀螺仪偏置估计；（2）速度、绝对尺度和重力粗估计；（3）细化重力。方法和Raul的VI-ORB的初始化方法类似，有需要可以参考我整理的笔记视觉惯性融合学习笔记二 VI_ORB初始化方法。

2.1 符号说明

论文和一般定义变量的方法有些不同，这里先进行符号说明。
$(.{)}^w$表示世界坐标系，其中z轴和重力平行。这意味着论文的世界坐标系，相当于一般定义的惯性坐标系；
$(.{)}^v$表示sfm参考坐标系，用于构建视觉结构的固定的坐标系，与惯性坐标系无关。这意味着论文的sfm参考坐标系，相当于一般定义的世界坐标系；
$(.{)}^b$表示体坐标系；
$(.{)}_b^w$表示坐标变换从体坐标系到世界坐标系；
$b_k$表示第k帧图像对应的体坐标系；
$c_k$表示第k帧图像对应的相机坐标系；
$\widehat{(.)}$表示传感器观测值；
$\overline{(.)}$表示sfm建立的视觉结构变量；
$q$表示旋转四元数；
$g^w=[0,0,g{]}^T$表示世界坐标系下重力；
$g^v$表示sfm参考坐标系下重力；

2.2 纯视觉SfM

与VI_ORB的第一个相似的地方就是都是一种松耦合初始化方法，通过先执行SfM来估计场景结构和相机位姿的初始值。不同的是VI_ORB利用系统开始的固定几秒钟估计结构的初始解和一些关键帧位姿；VINS-Mono则是基于滑动窗口构建视觉结构。（滑动窗口的理论可以参考SLAM中的滑窗（Sliding window））
在这一阶段，VINS-Mono先利用直接法获取两帧间的位姿；然后预设尺度（例如s=1），通过三角测量构建初始地图点；再通过PnP估计窗口内其他帧的位姿；最后用全局BA最小化重投影误差。这样一来，就求得了所有帧的位姿估计$({\bar{p}}_{c_k}^v,q_{c_k}^v)$和所有观测到的特征的3D坐标估计。如果外参$(p_b^c,q_b^c)$已知，则将所有帧的位姿估计从相机坐标系$c_k$转换到对应的体坐标系$b_k$：
$$q_{b_k}^v=q_{c_k}^v\otimes q_b^c$$$$s{\bar{p}}_{b_k}^v=s{\bar{p}}_{c_k}^v+q_{c_k}^v{p}_b^c$$

2.3 IMU预积分

预积分理论可以参考我整理的笔记视觉惯性融合学习笔记一 预积分理论，这里直接写结论。
IMU测量模型为
$${\hat{\omega }}^b(t)={\omega }^b(t)+b^g(t)+{\eta }^g(t)$$$${\hat{a}}^b(t)=q_b^W(t{)}^T(a^W(t)+g^W)+b^a(t)+{\eta }^a(t)$$
连续两帧$b_k$到$b_{k+1}$之间的IMU预积分状态量测量值连续形式
$${\alpha }_{b_{k+1}}^{b_k}={\iint }_{t\in [k,k+1]}^{}{\gamma }_{b_t}^{b_k}\hat{a}(t)d{t}^2$$$${\beta }_{b_{k+1}}^{b_k}={\int }_{t\in [k,k+1]}^{}{\gamma }_{b_t}^{b_k}\hat{a}(t)dt$$$${\gamma }_{b_{k+1}}^{b_k}={\int }_{t\in [k,k+1]}^{}{\gamma }_{b_t}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\hat{\omega }(t)\end{array}\right]dt$$
其中${\alpha }_{b_{k+1}}^{b_k}$，${\beta }_{b_{k+1}}^{b_k}$和${\gamma }_{b_{k+1}}^{b_k}$表示IMU预积分的三个测量值。

VINS-mono采用的预积分表示方法和On-Manifold Preintegration for Real-Time Visual–Inertial Odometry不同。其中旋转预积分采用四元数表示。

绕旋转轴$\vec{n}=[n_x,n_y,n_z{]}^T$的角度为$\theta$的旋转用四元数表示为$q=[cos\frac{\theta }{2},n_{x}sin\frac{\theta }{2},n_{y}sin\frac{\theta }{2},n_{z}sin\frac{\theta }{2}{]}^T$。如果$\theta$比较小，四元数可以近似表示为$\left[\begin{array}{c}1\\ \frac{\theta }{2}\vec{n}\end{array}\right]$，然后可以推出上式。

关于四元数的内容推荐阅读论文Quaternion kinematics for the error-state Kalman filter。

2.4 视觉惯性匹配

2.4.1 陀螺仪偏置校正

对窗口内的连续两帧$b_k$和$b_{k+1}$，根据2.2的纯视觉SfM可以计算出IMU在世界坐标系下的姿态$q_{b_{k+1}}^v$和$q_{b_{k+1}}^v$；根据2.3的IMU预积分可以计算出连续两帧IMU的姿态变化${\hat{\gamma }}_{b_{k+1}}^{b_k}$。因此可以建立目标函数
$$\underset{\delta b_g}{\mathrm{argmin}}\sum _{k\in B}||{q_{b_{k+1}}^v}^{-1}\otimes q_{b_k}^v\otimes {\gamma }_{b_{k+1}}^{b_k}|{|}^2$$$${\gamma }_{b_{k+1}}^{b_k}\approx {\hat{\gamma }}_{b_{k+1}}^{b_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\frac{\partial {\gamma }_{b_{k+1}}^{b_k}}{\partial b_g}\delta b_g\end{array}\right]$$
其中B表示滑动窗口内所有相机帧的索引。

2.4.2 初始化速度、重力和绝对尺度

定义待优化的变量为
$${\chi }_I=[v_{b_0}^v,...v_{b_n}^v,g^v,s]$$
则对于连续两帧相机帧，根据IMU预积分状态量计算值定义（参考视觉惯性融合学习笔记一 预积分理论3.2）可以计算得到
$${\hat{z}}_{b_{k+1}}^{b_k}=\left[\begin{array}{c}{\hat{\alpha }}_{b_{k+1}}^{b_k}\\ {\hat{\beta }}_{b_{k+1}}^{b_k}\end{array}\right]=H_{b_{k+1}}^{b_k}{\chi }_I+n_{b_{k+1}}^{b_k}\approx \left[\begin{array}{cccc}-q_v^{b_k}\Delta t_k& 0& \frac{1}{2}{q}_v^{b_k}\Delta t_k^2& q_v^{b_k}({\bar{p}}_{b_{k+1}}^v-{\bar{p}}_{b_k}^v)\\ -q_v^{b_k}& q_v^{b_k}& q_v^{b_k}\Delta t_k& 0\end{array}\right]\left[\begin{array}{c}{v}_{b_k}^v\\ v_{b_{k+1}}^v\\ g^v\\ s\end{array}\right]$$
其中第一个等号后的${\hat{z}}_{b_{k+1}}^{b_k}$表示由IMU测量值计算得到的IMU预积分测量值，第二个等号后的$H_{b_{k+1}}^{b_k}{\chi }_I$表示由纯视觉SfM计算得到的IMU预积分计算值；$\Delta t_k$表示连续两帧相机帧之间的时间间隔。
这样可以构建目标函数
$$\underset{{\chi }_I}{\mathrm{argmin}}\sum _{k\in B}||{\hat{z}}_{b_{k+1}}^{b_k}-H_{b_{k+1}}^{b_k}{\chi }_I|{|}^2$$
其中B表示滑动窗口内所有相机帧的索引。利用高斯牛顿法或者LM法可以估计${\chi }_I$。

2.4.3 重力细化

可以利用重力大小细化重力向量。但是直接加入重力大小约束到2.4.2的目标函数会导致问题变得复杂。因此论文将重力重新定义为
$$g.\bar{\hat{g}}+{\omega }_1{b}_1+{\omega }_2{b}_2$$
其中g表示重力大小，$\bar{\hat{g}}$，$b_1$和$b_2$为一组标准正交基。这里利用Gram-Schmidt正交化方法，将原SfM参考坐标系下的估计出的重力粗估计投影到一组标准正交向量上；将新的重力初始值代入目标函数，可以求出细化后的重力初始值。这样根据重力向量可以将所有SfM参考坐标系下的变量全部转化到世界坐标系下。到这里VINS-Mono的初始化过程就结束了。

论文最后还针对加速度计偏置进行了两个仿真实验：
1、第一个仿真实验中，飞行器在进行加速运动的同时，进行不同角度的旋转。将${\alpha }_{b_{k+1}}^{b_k}$，${\beta }_{b_{k+1}}^{b_k}$在加速度计偏置$b_a$处线性化，之后代入目标函数进行优化。结果显示只有当旋转角度足够（接近30°）时，才能有效校正$b_a$；
2、在第二个仿真实验中，飞行器同样进行加速运动的同时，进行不同角度的旋转。这次仿真忽略加速度计偏置的初始化，执行论文中的初始化过程。结果显示，尺度的初始化误差随着$b_a$的增大而增大，并且要产生10%的尺度误差，需要0.3$m/s^2$的加速度计偏置，而这个值通常不会达到。
这两个仿真实验说明了论文不在初始化阶段考虑$b_a$的原因。仿真结果引用VINS-Mono论文的图3。

以上只是简单介绍了VINS-Mono的初始化流程，具体的过程可以参考论文或者看代码VINS-Mono。