VINS_Fusion 初始化过程

VINS_Fusion的初始化与VINS_mono差不多.
按照<<手写VIO>>课程中的说法

1. 背景技术

在介绍VINS的初始化模块之前,先介绍以下背景知识:

A.IMU预积分

IMU传感器模型可以表示为如下式子:

[ω̃^b = ω^b + b^g + n^g\ ã^b = q_{bw} (a^w +g^w)+b^a+n^a ]

IMU 预积分即:将一段时间内的 IMU 数据直接积分起来就能得到两时刻 i, j 之间关于 IMU 的测量约束,即预积分量:

[伪位移增量：alpha_{b_ib_j}=intint_{tin[i,j]}(q_{b_ib_t}a^{b_t})delta t^2\ 速度预计分量：eta_{b_ib_j}=int_{tin[i,j]}(q_{b_ib_t}a^{b_t})delta t\ 两帧之间的旋转：q_{b_ib_t}=int_{tin[t,j]}q_{b_ib_t} left[egin{matrix}0\1/2w^{b_t}end{matrix} ight]dt ]

上式中,位移增量为什么是伪位移增量呢,因为加速度为0的时候,相机的速度不一定为0;
根据上述的数学模型,我们可以计算两个时刻之间IMU的轨迹.

B.视觉几何基础

根据得到的两张图片,可以通过如下过程来计算相机的变换.

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

根据上述过程,我们可以根据相机的图片信息得到图片之间的变换.

两套轨迹带来的问题->如何融合？

根据前两个模型,我们得到了两套的轨迹.对于这两套轨迹,我门有以下问题:

IMU 怎么和世界坐标系对齐,计算初始时刻的 (q_{wb_0}) ?
单目视觉姿态如何和 IMU 轨迹对齐,尺度如何获取?
VIO 系统的初始速度 v,传感器 bias 等如何估计?
IMU 和相机之间的外参数等...

VINS的初始化

1.视觉与IMU之间的联系

基于这些联系构建几何约束

考虑相机坐标系 (c_0) 为世界坐标系,则利用外参数 (q_{bc} , t_{bc}) 构建等式

[q_{c_0b_k} = q_{c_0b_k}⊗ q^{-1}_{bc}\ soverline{p}_{c_0b_k} = soverline{p}_{c_0c_k}−R_{c_0b_k}p_ {bc} ]

带一横表示带了尺度。其中,s 为尺度因子,(overline{p}) 表示非米制单位的轨迹。等式(3)等价于

[overline{p}_{c_0b_k} = overline{p}_{c_0c_k}−frac{1}{s}R_{c_0b_k}p_ {bc}\ overline{p}_{c_0c_k} = frac{1}{s}R_{c_0b_k}p_ {bc}+overline{p}_{c_0b_k} ]

旋转没有尺度信息。

通过该约束来与相机提供的约束打通关系。

2.视觉IMU对齐流程

估计流程

旋转外参数 (q_{bc}) 未知, 则先估计旋转外参数.（很多时候由于相机与IMU很近，因此平移参数不是很重要）
利用旋转约束估计陀螺仪 bias.（短时间内是个常数，因此需要估计以得到更准确的值）

[q_{c_0b_k} = q_{c_0c_k} ⊗ q^{-1}_{bc} ]
利用平移约束估计重力方向,速度,以及尺度初始值.(把非米制单位拉伸到米制单位)

[soverline{p}_{c_0b_k} = soverline{p}_{c_0c_k} − R_{c_0b_k}p_{bc} ]
对重力向量 (g^{c_0}) 进行进一步优化.
求解世界坐标系 w 和初始相机坐标系 (c_0) 之间的旋转矩阵 (q_{wc_0}),
并将轨迹对齐到世界坐标系。

A. 利用旋转约束估计外参数旋转(Q_{bc})

相邻两时刻 k, k + 1 之间有:IMU旋转积分 (q_{b_kb_{k+1}}) ,视觉测量(q_{c_kc_{k+1}}) 。则有:

[q_{b_kb_{k+1}}⊗q_{bc} = q_{bc}⊗q_{c_kc_{k+1}}=0 ]

上式可写成:

[([q_{b_kb_{k+1}}]_L-[q_{c_kc_{k+1}}]_R)q_{bc}= Q^{k}_{k+1}q_{bc}=0 ]

其中,([·]_L , [·]_R) 表示 left and right quaternion multiplication。

将多个时刻线性方程(6)累计起来(提高信噪比),并加上鲁棒核权重得到:

[left[egin{matrix} w^0_1Q^0_1\w^1_2Q^1_2\...\w^{N-1}_NQ^{N-1}_N end{matrix} ight] q_{bc}=Q_Nq_{bc}=0 ]

其中

[w^k_{k+1}=left{egin{array}{l} 1,& r^{k}_{k+1}<thresold\ frac{thresold}{r^{k}_{k+1}},& otherwise end{array} ight. ]

由旋转矩阵和轴角之间的关系 (tr(R) = 1 + 2 cos θ), 能得到角度误差 r
的计算为:

[r^k_{k+1}=acos((tr(hat{R}^{-1}_{bc}{R}^{-1}_{b_kb_{k+1}}hat{R}_{bc}{R}_{b_kb_{k+1}})-1)/2) ]

即从视觉数据计算得到的R应当与Q相同，因此括号中应该为一个单位矩阵

公式(7)的求解同样采用 SVD 分解,即最小奇异值对应的奇异向量。

具体代码见：initial_ex_rotation.cpp 函数 CalibrationExRotation()。

B. 基于旋转约束的Gyroscope Bias

估计bias可以使两个时刻之间IMU的旋转更准。因为旋转是通过积分得到的。
假定(q_{c_kc_{k+1}})准确，且g估计到的外参准确，则(q_{c_kb_k})准确.

如果外参数 (q_{bc}) 已标定好,利用旋转约束,可估计陀螺仪 bias:

[arg min_{delta b^g}sum_{kinBeta}||2|q^{-1}_{c_0b_{k+1}}⊗q_{c_0b_k}⊗q_{b_kb_{k+1}}|_{xyz}||^2 ]

其中, B 表示所有的图像关键帧集合,另有预积分的一阶泰勒近似:

[q_{b_kb_{k+1}}approx hat{q}_{b_kb_{k+1}}⊗ left[egin{array}{c} 1\frac{1}{2}J^q_{b^g}delta b^g end{array} ight] ]

(q_{b_kb_{k+1}})的误差近似于第二项。

公式(10)为普通的最小二乘问题,求取雅克比矩阵,构建正定方程 HX = b =0 即可以求解。

具体代码见:initial_aligment.cpp 函数 solveGyroscopeBias().

C. 初始化速度、重力和尺度因子

研究将IMU与相机对齐之后如何初始化速度、重力和尺度因子。

待估计变量

[chi_I=[v^{b_0}_0,v^{b_1}_1,...,v^{b_n}_n,g^{c_0},s]^T ]

其中,(v_k^{b_k})表示k时刻body 坐标系的速度在 body 坐标系下的表示。
(g^{c_0})为重力向量在第 0 帧相机坐标系下的表示。s 表示尺度因子,将视
觉轨迹拉伸到米制单位。

回顾：预积分量约束

世界坐标系w下有

[alpha_{b_ib_j}=q_{b_iw}(p_{wb_j}-p_{wb_i}-v^w_iDelta t+frac{1}{2}g^wDelta t^2)\ eta_{b_ib_j}=q_{b_iw}(v^w_j-v^w_i+g^wDelta t) ]

将世界坐标系 w 换成相机初始时刻坐标系 (c_0) 有

[alpha_{b_kb_{k+1}}=R_{b_kc_0}(s(overline{p}_{c_0b_{k+1}}-overline{p}_{c_0b_k}))+frac{1}{2}g^{c_0}Delta t^2_k -R_{c_0b_k}v^{b_k}_kDelta{t_k}\ eta{b_kb_{k+1}}=R_{b_kc_0}(R_{c_0b_{k+1}}v_{k+1}^{b_{k+1}}+g^{c_0}Delta t^2_k -R_{c_0b_k}v^{b_k}_kDelta{t_k} ]

将公式(3)代入公式(14)进行简单整理有:

[alpha_{b_kb_{k+1}}=sR_{b_kc_0}(overline{p}_{c_0c_{k+1}}-overline{p}_{c_0c_k})-R_{b_kc_0}R_{c_0b_{k+1}}p_{bc}+p_{bc}+frac{1}{2}R_{b_kc_0}g^{c_0}Delta t^2_k-v_k^{b_k}Delta t_k ]

将待估计变量放到方程右边，有：

具体代码见:initial_aligment.cpp 函数 LinearAlignment().

D. 优化重力向量(g^{c_0})

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

利用公式(16)求解重力向量 g_{c_0} 过程中,并没有加入模长限制
(∥g^{c_0}∥ = 9.81)。三维变量 (g^{c_0}) 实际只有两个自由度。

重力向量的参数化

三维向量自由度为 2,可以采用球面坐标
进行参数化:

[ĝ^{c_0} = ∥g∥·ĝ^{c_0}+w_1b_1 + w2b_2 ]

其中,w1, w2 为待优化变量

[b_1=left{egin{array}{l} hat{overline{g}}^{c_0} imes [1,0,0],& hat{overline{g}}^{c_0} eq [1,0,0]^T\ hat{overline{g}}^{c_0} imes [0,0,1],& otherwise end{array} ight.\ b_2 = hat{overline{g}}^{c_0} imes b_1 ]

将公式g代入，待优化变量变为：

[chi_I= left[ egin{matrix} v^{b_k}_k\v^{b_{k+1}}_{k+1}\g^{c_0}\s end{matrix} ight] arr left[ egin{matrix} v^{b_k}_k\v^{b_{k+1}}_{k+1}\w^{c_0}\s end{matrix} ight] ]

公式中的观测方程变为：

[hat{z}^{b_k}_{b_k+1} = left[egin{matrix} alpha_{b_kb_{k+1}}-p_{bc}+R_{b_kc_0}R_{c_0b_{k+1}}p_{bc}-frac{1}{2}R_{b_kc_0}Delta t^2_k||g||hat{overline{g}}^{c_0} \ eta_{b_kb_{k+1}}-R_{b_kc_0}Delta t_k ||g|| hat{overline{g}}^{c_0} end{matrix} ight] ]

然后采用最小二乘对(chi_I)重新进行优化。

E. 将相机坐标系对齐到世界坐标系

对齐流程

找到 (c_0) 到 w 系的旋转矩阵 (R_{wc_0} = exp([θu]))

[$u =frac{ĝ^{c_0} imes ĝ^w}{∥ĝ^{c_0}×ĝ^w∥} θ = atan2(∥ĝ^{c_0} imes ĝ^w ∥,ĝ^{c_0}·ĝ^w) ]
把所有 (c_0) 坐标系下的变量旋转到 (w) 下。
把相机平移和特征点尺度恢复到米制单位。
至此,完成了系统初始化过程

初始化拓展

疑问

加速度 bias 为何没有估计? 加速度bias一般很小，不一定能估计出来，与重力向量相比，量纲很小
平移外参数 (p_{bc}) 为何没有初始化?

答：影响不大。1.陀螺仪的bias会影响旋转，旋转是一个非线性的东西，平移是线性的东西。旋转比平移重要。2. 平移外参一般在设备上很小，影响不大，可提前设置一个值，在后续中优化。

其他初始化方法

静止初始化:直接用加速度测量重力方向,初始速度为 0.
除 vins-mono 外的其他运动初始化方案 a , b 。

Janne Mustaniemi et al. “Inertial-based scale estimation for structure from motion on mobile devices”. In: 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE. 2017, pp. 4394–4401.
Javier Domínguez-Conti et al. “Visual-Inertial SLAM Initialization: A General Linear Formulation and a Gravity-Observing Non-Linear Optimization”. In: 2018 IEEE International Symposium on Mixed and Augmented Reality (ISMAR). IEEE. 2018, pp. 37–45.