VINS-Mono之初始化

VINS-Mono之初始化

1. IMU与相机外参估计

在VINS中，可以在线估计IMU和相机的外参$q_{bc}$。我们知道
$$q_c^b\otimes q_{c_{k+1}}^{c_k}=q_{b_{k+1}}^{b_k}\otimes q_c^b$$
写成矩阵形式：
$$\left[(q_{c_{k+1}}^{c_k}{)}_R-(q_{b_{k+1}}^{b_k}{)}_L\right]q_c^b=0$$
对于上式，$q_{c_{k+1}}^{c_k}$以及
$$\left[\begin{array}{c}(q_{c_1}^{c_0}{)}_R-(q_{b_1}^{b_0}{)}_L\\ (q_{c_2}^{c_1}{)}_R-(q_{b_{k2}}^{b_1}{)}_L\\ ⋮\\ (q_{c_{k+1}}^{c_k}{)}_R-(q_{b_{k+1}}^{b_k}{)}_L\end{array}\right]q_c^b=0$$
于是，对于$q_c^b$的求解问题转换为求解$Ax=0$方程的问题，$q_c^b$的最优解为上式矩阵$A$奇异值分解最后一列右奇异向量。

2. 利用旋转估计陀螺仪的零偏bias

对于预积分我们知道有如下约束：
$$q_{b_{k+1}}^{c_0}=(q_c^b{)}^{-1}\otimes {\gamma }_{b_{k+1}}^{b_k}$$
而又有：
$${\gamma }_{b_{k+1}}^{b_k}$$