VINS——初始化过程

1 前言

初始化总体流程如下：

• 旋转外参数 ${\mathbf{q}}_{bc}$ 未知, 则先估计旋转外参数.
• 利用旋转约束估计陀螺仪 bias.

$${\mathbf{q}}_{c_0{b}_k}={\mathbf{q}}_{c_0{c}_k}\otimes {\mathbf{q}}_{bc}^{-1}$$

• 利用平移约束估计重力方向，速度，以及尺度初始值.

$$s{\overline{\mathbf{p}}}_{c_0{b}_k}=s{\overline{\mathbf{p}}}_{c_0{c}_k}-{\mathbf{R}}_{c_0{b}_k}{\mathbf{p}}_{bc}$$

• 对重力向量 ${\mathrm{g}}^{c_0}$ 进行进一步优化.
• 求解世界坐标系 $w$ 和初始相机坐标系 $c_0$ 之间的旋转矩阵 ${\mathbf{q}}_{w{c}_0}$, 并将轨迹对齐到世界坐标系。

关于字母下标

• 从左向右看下标，例如：${\mathbf{q}}_{c_0{b}_k}$表示在c0相机坐标系下，从c0到bk坐标系的旋转。
• 根据IMU放置的位置特殊性，这里可以认为body坐标系和imu坐标系重叠了，当做一个坐标系理解。

STEP1 估计外参旋转参数${\mathbf{q}}_{\mathbf{b}\mathbf{c}}$(基于旋转约束)

相邻两时刻 $k,k+1$ 之间有： IMU 旋转积分 ${\mathbf{q}}_{b_k{b}_{k+1}}$, 视觉测量 ${\mathbf{q}}_{c_k{c}_{k+1}}$ 。则有：

所以
$${\mathbf{q}}_{b_k{b}_{k+1}}\otimes {\mathbf{q}}_{bc}={\mathbf{q}}_{bc}\otimes {\mathbf{q}}_{c_k{c}_{k+1}}$$
上式可写成：
$$\left({\left[{\mathbf{q}}_{b_k{b}_{k+1}}\right]}_L-{\left[{\mathbf{q}}_{c_k{c}_{k+1}}\right]}_R\right){\mathbf{q}}_{bc}={\mathbf{Q}}_{k+1}^k\cdot {\mathbf{q}}_{bc}=0$$

其中, $[\cdot {]}_L,[\cdot {]}_R$ 表示 left and right quaternion multiplication 。

将多个时刻线性方程(6)累计起来，并加上鲁棒核权重得到：
$$\left[\begin{array}{c}{w}_1^0\cdot {\mathbf{Q}}_1^0\\ w_2^1\cdot {\mathbf{Q}}_2^1\\ ⋮\\ w_N^{N-1}\cdot {\mathbf{Q}}_N^{N-1}\end{array}\right]{\mathbf{q}}_{bc}={\mathbf{Q}}_N\cdot {\mathbf{q}}_{bc}=0$$
其中：
$$w_{k+1}^k=\{\begin{array}{ll}1,& r_{k+1}^k<\text{ threshold }\\ \frac{\text{ threshold }}{r_{k+1}^k},& \text{ otherwise }\end{array}$$
由旋转矩阵和轴角之间的关系tr(R) = 1 + 2 cos θ，能得到角度误差 r的计算为：
$$r_{k+1}^k=\mathrm{acos}\left(\left(\mathrm{tr}\left({\hat{\mathbf{R}}}_{bc}^{-1}{\mathbf{R}}_{b_k{b}_{k+1}}^{-1}{\hat{\mathbf{R}}}_{bc}{\mathbf{R}}_{c_k{c}_{k+1}}\right)-1\right)/2\right)$$

注意：

• 带上冒号的表示估计值
• 求解同样采用 SVD 分解，即最小奇异值对应的奇异向量。(如果倒数第二个奇异值比较小，那么表示估计的值并不是特别有效，噪声太大)

STEP2 估计陀螺仪bias(基于旋转约束)

这里认为相机估计是准确的，陀螺仪被认为不准确（带有bias）

如果外参数 ${\mathbf{q}}_{bc}$ 已标定好，利用旋转约束，可估计陀螺仪 bias:

$$\arg \underset{\delta \mathbf{b}g}{\min }\sum _{k\in B}{\Vert 2{\lfloor {\mathbf{q}}_{c_0{b}_{k+1}}^{-1}\otimes {\mathbf{q}}_{c_0{b}_k}\otimes {\mathbf{q}}_{b_k{b}_{k+1}}\rfloor }_{xyz}\Vert }^2$$
其中, $B$ 表示所有的图像关键坝集合，另有预积分的一阶泰勒近似:
$${\mathbf{q}}_{b_k{b}_{k+1}}\approx {\hat{\mathbf{q}}}_{b_k{b}_{k+1}}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}{\mathbf{J}}_{b^9}^{\mathbf{q}}\delta {\mathbf{b}}^g\end{array}\right]$$
公式为普通的最小二乘问题，求取雅克比矩阵，构建正定方程 $\mathbf{H}\mathbf{X}=\mathbf{b}$ 即可以求解。

注意这里是将bias的的bias积分进入累加的

STEP3 估计重力方向，速度和尺度（利用平移约束）

几何约束（前期知识）

考虑相机坐标系 $c_0$ 为世界坐标系， 则利用外参数 ${\mathbf{q}}_{bc},{\mathbf{t}}_{bc}$ 构建等式
$$\begin{array}{rl}{\mathbf{q}}_{c_0{b}_k}& ={\mathbf{q}}_{c_0{c}_k}\otimes {\mathbf{q}}_{bc}^{-1}\\ s{\overline{\mathbf{p}}}_{c_0{b}_k}& =s{\overline{\mathbf{p}}}_{c_0{c}_k}-{\mathbf{R}}_{c_0{b}_k}{\mathbf{p}}_{bc}\end{array}$$
其中， $s$ 为尺度因子, $\overline{\mathbf{p}}$ 表示非米制 单位的轨迹。上面等式等价于
$$\begin{array}{l}{\overline{\mathbf{p}}}_{c_0{b}_k}={\overline{\mathbf{p}}}_{c_0{c}_k}-\frac{1}{s}{\mathbf{R}}_{c_0{b}_k}{\mathbf{p}}_{bc}\\ {\overline{\mathbf{p}}}_{c_0{c}_k}=\frac{1}{s}{\mathbf{R}}_{c_0{b}_k}{\mathbf{p}}_{bc}+{\overline{\mathbf{p}}}_{c_0{b}_k}\end{array}$$

需要估计的变量
$${\mathcal{X}}_I={\left[{\mathbf{v}}_0^{b_0},{\mathbf{v}}_1^{b_1},\cdots {\mathbf{v}}_n^{b_n},{\mathbf{g}}^{c_0},s\right]}^{\top }$$

其中:

• ${\mathbf{v}}_k^{b_k}$ 表示 $k$ 时刻 body 坐标系的速度在 body 坐标系下的表示。
• ${\mathrm{g}}^{c_0}$ 为重力向量在第 0 帧相机坐标系下的表示。
• $s$ 表示尺度因子，将视觉轨迹拉伸到米制单位。

回顾：预积分量约束
$$\begin{array}{l}{\mathbf{\alpha }}_{b_i{b}_j}={\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)\\ {\mathbf{\beta }}_{b_i{b}_j}={\mathbf{q}}_{b_{i}w}\left({\mathbf{v}}_j^w-{\mathbf{v}}_i^w+{\mathbf{g}}^w\Delta t\right)\end{array}$$
将世界坐标系 $w$ 换成相机初始时刻坐标系 $c_0$ 有
$$\begin{array}{rl}{\mathbf{\alpha }}_{b_k{b}_{k+1}}& ={\mathbf{R}}_{b_k{c}_0}\left(s\left({\overline{\mathbf{p}}}_{c_0{b}_{k+1}}-{\overline{\mathbf{p}}}_{c_0{b}_k}\right)+\frac{1}{2}{\mathbf{g}}^{c_0}\Delta t_k^2-{\mathbf{R}}_{c_0{b}_k}{\mathbf{v}}_k^{b_k}\Delta t_k\right)\\ {\mathbf{\beta }}_{b_k{b}_{k+1}}& ={\mathbf{R}}_{b_k{c}_0}\left({\mathbf{R}}_{c_0{b}_{k+1}}{\mathbf{v}}_{k+1}^{b_{k+1}}+{\mathbf{g}}^{c_0}\Delta t_k-{\mathbf{R}}_{c_0{b}_k}{\mathbf{v}}_k^{b_k}\right)\end{array}$$
将【几何约束（前期知识）】部分公式代入上面进行简单整理有：
$$\begin{array}{rl}{\mathbf{\alpha }}_{b_k{b}_{k+1}}=& s{\mathbf{R}}_{b_k{c}_0}\left({\overline{\mathbf{p}}}_{c_0{c}_{k+1}}-{\overline{\mathbf{p}}}_{c_0{c}_k}\right)\\ & -{\mathbf{R}}_{b_k{c}_0}{\mathbf{R}}_{c_0{b}_{k+1}}{\mathbf{p}}_{bc}+{\mathbf{p}}_{bc}+\frac{1}{2}{\mathbf{R}}_{b_k{c}_0}{\mathbf{g}}^{c_0}\Delta t_k^2-{\mathbf{v}}_k^{b_k}\Delta t_k\end{array}$$
将待估计变量放到方程右边, 有：
$${\hat{\mathbf{z}}}_{b_{k+1}}^{b_k}=\left[\begin{array}{c}{\hat{\mathbf{\alpha }}}_{b_k}{b}_{k+1}-{\mathbf{p}}_{bc}+{\mathbf{R}}_{b_k{c}_0}{\mathbf{R}}_{c_0{b}_{k+1}}{\mathbf{p}}_{bc}\\ {\hat{\mathbf{\beta }}}_{b_k{b}_{k+1}}\end{array}\right]={\mathbf{H}}_{b_{k+1}}^{b_k}{\mathcal{X}}_I^k+{\mathbf{n}}_{b_{k+1}}^{b_k}$$
公式中:
$$\begin{array}{rl}{\mathcal{X}}_I^k& ={\left[{\mathbf{v}}_k^{b_k},{\mathbf{v}}_{k+1}^{b_{k+1}},{\mathbf{g}}^{c_0},s\right]}^{\top }\\ {\mathbf{H}}_{b_{k+1}}^{b_k}& =\left[\begin{array}{cccc}-\mathbf{I}\Delta t_k& 0& \frac{1}{2}{\mathbf{R}}_{b_k{c}_0}\Delta t_k^2& {\mathbf{R}}_{b_k{c}_0}\left({\overline{\mathbf{p}}}_{c_{k+1}}^{c_0}-{\overline{\mathbf{p}}}_{c_k}^{c_0}\right)\\ -\mathbf{I}& {\mathbf{R}}_{b_k{c}_0}{\mathbf{R}}_{c_0{b}_{k+1}}& {\mathbf{R}}_{b_k{c}_0}\Delta t_k& 0\end{array}\right]\end{array}$$
转化成线性最小二乘问题对状态量进行求解：
$$\underset{{\mathcal{X}}_I}{\min }\sum _{k\in \mathcal{B}}{\Vert {\hat{\mathbf{z}}}_{b_{k+1}}^{b_k}-{\mathbf{H}}_{b_{k+1}}^{b_k}{\mathcal{X}}_I^k\Vert }^2$$

STEP4 优化重力方向${\mathbf{g}}^{\mathbf{c}0}$

疑问：为什么需要优化重力向量

在STEP 3中利用公式求解重力向量 ${\mathbf{g}}^{c_0}$ 过程中，并没有加入模长限制 $\mid g^{c_0}\Vert =9.81$ 。 三维变量 ${\mathbf{g}}^{c_0}$ 实际只有两个自由度。

重力向量参数化

三维向量自由度为 2，可以采用球面坐标进行参数化： 东北天=XYZ，这里的w1应该属于010也就是北
$${\hat{\mathrm{g}}}^{c_0}=\Vert g\Vert \cdot {\hat{{\widehat{\overline{\mathbf{g}}}}^{c_0}}}^{c_0}+w_1{b}_1+w_2{b}_2$$

其中：

• $w1,w2$ 为待优化变量。

• ${\hat{\mathrm{g}}}^{c_0}$ 为重力向量在第 0 帧相机坐标系下的表示。

• ${\widehat{\overline{\mathbf{g}}}}^{c_0}$ 表示方向向量

$$\begin{array}{l}{b}_1=\{\begin{array}{ll}\left({\widehat{\overline{\mathbf{g}}}}^{c_0}\times [1,0,0]\right),& {\hat{\mathbf{g}}}^{c_0}\ne [1,0,0{]}^{\top }\\ \left({\widehat{\overline{\mathbf{g}}}}^{c_0}\times [0,0,1]\right),& \text{ otherwise }\end{array}\\ b_2={\widehat{\overline{\mathbf{g}}}}^{c_0}\times b_1\end{array}$$

将公式代入STEP 3 的公式(13), 待优化变量变为：
$${\mathcal{X}}_I^k=\left[\begin{array}{c}{\mathbf{v}}_k^{b_k}\\ {\mathbf{v}}_{k+1}^{b_{k+1}}\\ {\mathbf{g}}^{c_0}\\ s\end{array}\right]\to \left[\begin{array}{c}{\mathbf{v}}_k^{b_k}\\ {\mathbf{v}}_{k+1}^{b_{k+1}}\\ {\mathbf{w}}^{c_0}\\ s\end{array}\right]$$
STEP 3中公式(13)中的观测方程变为：
$${\hat{\mathbf{z}}}_{b_{k+1}}^{b_k}=\left[\begin{array}{c}{\mathbf{\alpha }}_{b_k{b}_{k+1}}-{\mathbf{p}}_{bc}+{\mathbf{R}}_{b_k{c}_0}{\mathbf{R}}_{c_0{b}_{k+1}}{\mathbf{p}}_{bc}-\frac{1}{2}{\mathbf{R}}_{b_k{c}_0}\Delta t_k^2\Vert g\Vert \cdot {\hat{\mathbf{g}}}^{c_0}\\ {\mathbf{\beta }}_{b_k{b}_{k+1}}-{\mathbf{R}}_{b_k{c}_0}\Delta t_k\Vert g\Vert \cdot {\hat{\mathbf{g}}}^{c_0}\end{array}\right]$$
采用最小二乘对 ${\mathcal{X}}_I$ 进行重新优化。

STEP5 对齐导航世界坐标系

计算到这儿，我们得到了重力加速度相对于相机初始时刻坐标系向量${\hat{\mathrm{g}}}^{c_0}$。当然，重力向量相对于世界坐标系的绝对向量我们是一直知道的就是$\mathbf{g}=[0,0,9.81{]}^T$，根据这两个重力向量我们就可以知道相机坐标系相对于世界坐标系的关系了，以此为依据将这两个坐标系对齐。

步骤如下：

• 1 找到 $c_0$ 到 $w$ 系的旋转矩阵 ${\mathbf{R}}_{w{c}_0}=\exp ([\theta \mathbf{u}])$

$$\mathbf{u}=\frac{{\hat{\mathbf{g}}}^{c_0}\times {\hat{\mathbf{g}}}^w}{\Vert {\hat{\mathbf{g}}}^{c_0}\times {\hat{\mathbf{g}}}^w\Vert },\theta =\mathrm{atan}2\left(\Vert {\hat{\mathbf{g}}}^{c_0}\times {\hat{\mathbf{g}}}^w\Vert ,{\hat{\mathbf{g}}}^{c_0}\cdot {\hat{\mathbf{g}}}^w\right)$$

这里的$\mathbf{u}$是旋转轴。通过将两个重力加速度量叉乘可以得到一个垂直于两个重力向量的向量，以此作为旋转轴，根据向量内积求解两个重力向量的夹角，根据轴角转化为旋转矩阵。就得到了两个坐标系之间的旋转。

• 2 把所有 $c_0$ 坐标系下的变量旋转到 $w$ 下。
• 3 把相机平移和特征点尺度恢复到米制单位。
• 4至此，完成了系统初始化过程。

补充

简述视觉初始化过程

• 1 已知俩图像：特征点提取 (fast), 匹配 (光流, 特征描述子).
• 2 已知俩图像特征匹配点：利用对极几何约束 (E 矩阵, $\mathrm{H}$ 矩阵), 计算两图像之间的 pose (update to scale).
• 3 已知相机 pose, 已知特征点二维坐标: 通过三角化得到三维坐标.
• 4 已知 3d 点, $2\mathrm{d}$ 特征点：通过 Perspective-n-Point $(\mathrm{P}\mathrm{n}\mathrm{P})$ 求取新的 相机 pose.

一些容易疑惑的点

• 初始化完成之前pose的获取：先求取两帧之间的相对位姿，获得自身的pose
• 初始化完成后pose的获取：根据跟踪情况，就能直接得到3D-2D的关系，然后跳过第三步，利用3D-2D对应关系，直接使用Pnp方法求解pose（也正是使用Pnp，所以保证了整个过程中，所有的pose尺度统一）
• VSLAM尺度的确定：有了IMU之后，通过两个不同的轨迹对齐，完成了vslam的尺度确定。轨迹对齐过程见【STEP5 对齐导航世界坐标系】
• 加速度bias为何没有估计：量纲太小，估计不出来。而且其bias对整个结果影响不大。
• 平移外参${\mathbf{P}}_{\mathbf{b}\mathbf{c}}$为何没有初始化：旋转是非线性的，平移是线性的。线性影响不大，再者，视觉传感器和IMU传感器一般都是非常近的。

参考

深蓝学院手写VIO课程