VINS-Mono之初始化
文章目录
- VINS-Mono之初始化
-
- 1. IMU与相机外参估计
- 2. 利用旋转估计陀螺仪的零偏bias
- 3. 对位置V、尺度s以及重力g的估计
- 4. 基于先验的重力加速度优化求解
- TODO 视觉初始化部分
VINS-Mono之初始化
1. IMU与相机外参估计
在VINS中，可以在线估计IMU和相机的外参q_{bc}。我们知道
q^{b}_{c}\otimes q^{c_k}_{c_{k+1}}=q^{b_k}_{b_{k+1}}\otimes q^b_{c}
写成矩阵形式：
\left[ (q^{c_k}_{c_{k+1}})_R -(q^{b_k}_{b_{k+1}})_L \right] q_{c}^{b} =0
对于上式，q^{c_k}_{c_{k+1}}以及
\begin{bmatrix} (q^{c_0}_{c_{1}})_R -(q^{b_0}_{b_{1}})_L\\ (q^{c_1}_{c_{2}})_R -(q^{b_1}_{b_{k2}})_L\\ \vdots \\ (q^{c_k}_{c_{k+1}})_R -(q^{b_k}_{b_{k+1}})_L \end{bmatrix} q_{c}^{b} = 0
于是，对于q^b_c的求解问题转换为求解Ax=0方程的问题，q^b_c的最优解为上式矩阵A奇异值分解最后一列右奇异向量。
2. 利用旋转估计陀螺仪的零偏bias
对于预积分我们知道有如下约束：
q^{c_0}_{b_{k+1}} = (q^b_{c})^{-1}\otimes\gamma^{b_{k}}_{b_{k+1}}
而又有：
\gamma^{b_k}_{b_{k+1}} = \hat{\gamma}^{b_k}_{b_{k+1}}\otimes \begin{bmatrix} 1\\ \frac{1}{2}J^{\gamma}_{b_g}\delta b_g \end{bmatrix}
合并上面两式子得到:
\begin{aligned} q^{c_0}_{b_{k+1}} = (q^{b}_c)^{-1} \otimes \hat{\gamma}^{b_k}_{b_{k+1}} \otimes \begin{bmatrix} 1\\ \frac{1}{2}J^{\gamma}_{b_g}\delta b_g \end{bmatrix}\\ J^{\gamma}_{b_g}\delta b_g = 2\left( (\gamma^{b_k}_{b_{k+1}})^{-1}\otimes q_{c}^b \otimes q^{c_0}_{b_{k+1}} \right)_{vec} \end{aligned}
于是，对于零偏的估计变成了Ax=b的最小二乘法求解，其解为x = (A^TA)^{-1}A^T b。
3. 对位置V、尺度s以及重力g的估计
预积分的约束为：
\begin{aligned} R^{b_k}_{c_0}\left(P^{c_0}_{b_{k+1}}+\frac{1}{2}g^{c_0}\Delta t_k^2-P^{c_0}_{b_k}-V^{c_0}_{b_k}\Delta t_k\right)= \alpha^{b_k}_{b_{k+1}} \\ R^{b_k}_{c_0} \left( V_{b_{k+1}}^{c_0}+g^{c_0}\Delta t_k -V^{c_0}_{b_k} \right)=\beta^{b_k}_{b_{k+1}} \end{aligned}
上式中，P^{c_0}_{k+1}是带有尺度的，不知道真实尺度，这里可以将其进行分解：
q^{c_0}_{b_k} = q^{c_0}_{c_k}\otimes (q^b_c)^{-1}
上面约束式子可转化为：
\begin{bmatrix} R^{c_0}_{b_k} & sp^{c_0}_{b_k}\\ 0 & I \end{bmatrix} =\begin{bmatrix} R^{c_0}_{c_k} & sp^{c_0}_{c_k}\\ 0 & I \end{bmatrix}\begin{bmatrix} (R^{b}_{c})^T & -p^{b}_{c}\\ 0 & I \end{bmatrix}
于是有：
sp^{c_0}_{b_k} = sp^{c_0}_{c_k}-R^{c_0}_{c_k}~p^{b}_c
把上式带入式(10)，得：
\begin{aligned} &R^{b_k}_{c_0}\left((sP^{c_0}_{c_{k+1}}-R^{c_0}_{c_{k+1}}P^b_c)+\frac{1}{2}G\Delta t_k^2-(sP^{c_0}_{c_k}-R^{c_0}_{c_k}P^b_c)-V^{c_0}_{b_k}\Delta t_k\right)= \alpha^{b_k}_{b_{k+1}} \\ &R^{b_k}_{c_0} \left( V_{b_{k+1}}^{c_0}+G\Delta t_k -V^{c_0}_{b_k} \right)=\beta^{b_k}_{b_{k+1}} \end{aligned}
分离上式中的已知和未知量得到：
\begin{aligned} R^{b_k}_{c_0}\left((sP^{c_0}_{c_{k+1}}-sP^{c_0}_{c_k})+\frac{1}{2}g^{c_0}\Delta t_k^2-V^{c_0}_{b_k}\Delta t_k\right)= \alpha^{b_k}_{b_{k+1}}+R^{b_k}_{c_0}\left(R^{c_0}_{c_{k+1}}P^b_c-R^{c_0}_{c_k}P^b_c\right) \\ R^{b_k}_{c_0}R^{c_0}_{b_{k+1}}V_{b_{k+1}}^{b_{k+1}}+R^{b_k}_{c_0}g^{c_0}\Delta t_k -V^{b_k}_{b_k} =\beta^{b_k}_{b_{k+1}} \end{aligned}
将上式写成矩阵为：
\begin{aligned} \begin{bmatrix} -I\Delta t_k & 0 & \frac{1}{2}R^{b_k}_{c_0}\Delta t_k^2 & R^{b_k}_{c_0}\left(P^{c_0}_{c_{k+1}}-P^{c_0}_{c_k}\right) \\ -I & R^{b_k}_{c_0}R_{b_{k+1}}^{c_0} & R^{b_k}_{c_0}\Delta t_k & 0 \end{bmatrix} \begin{bmatrix} V^{b_k}_{b_k}\\ V^{b_{k+1}}_{b_{k+1}}\\ g^{c_0}\\ s \end{bmatrix} = \begin{bmatrix} \alpha^{b_k}_{b_{k+1}} + R^{b_k}_{c_0}R^{c_0}_{c_k+1}P^{b}_{c}-P_{c}^b\\ \beta_{b_{k+1}}^{b_k} \end{bmatrix} \end{aligned}
简写为：Hx=b，矩阵H不是方阵，于是通过构造H^THx=H^Tb然后求出其解。
4. 基于先验的重力加速度优化求解
在步骤3中，在没有任何先验的情况下求解出了重力加速度g，为了求出更准确的重力加速度值，可以提前测量或者查表得到当地重力加速度的值\Vert g\Vert。然后我们采取步骤3中求解的重力方向s_g，于是迭代的初始重力加速度为\Vert g\Vert s_g。
由于重力加速度的模长自由度被限制，所以优化的维度为2，这里想像一个半径为\Vert g\Vert的球体，我们要求解的重力加速度就是球体表面一点到球体中心的向量，我们可以用切与球面的一个二维向量表示，首先根据初始的重力加速度求出一个与其垂直的向量，再叉乘出另外一个。
最后通过步骤3中的迭代公式进行迭代，求解最后的重力加速度值。