IMU初始化

IMU初始化分解为多个子问题：

1. 估计陀螺仪偏置
2. 忽略加速度计偏置，估计尺度和重力矢量
3. 估计加速度计偏置，进一步优化尺度和重力
4. 估计速度

1. 陀螺仪偏置估计

初始化过程假定 $b^g$保持不变，每帧的偏置恒定： $b_i^g=b^g$。优化 $b^g$,最小化所有相邻关键帧对之间的陀螺仪（旋转）预积分及从纯视觉ORB-SLAM2直接视觉计算的旋转之间的误差。
$^*b^g=\mathop{\arg\min}_{\theta} \sum_{k=1}^{N-1}\|Log((\Delta\tilde{R}_{k,k+1}Exp(J_{\Delta}^{g}b^g))^TR_{BW}^kR_{WB}^{k+1})\|^2$
其中 $\Delta\tilde{R}_{k,k+1}=Exp((\tilde{\omega_k}-b^g)\Delta t)$,为陀螺仪的预积分值。N为关键帧的数量。 $R_{WB}^{(·)}=R_{WC}^{(·)}\times R_{CB}^{(·)}$, $R_{WC}^{(·)}$为ORB-SLAM视觉计算的位姿， $R_{CB}^{(·)}$由标定得到。 $b^g$从0开始迭代。

2.尺度及重力估计

忽略加速度计偏置 $b_i^a=b^a=0$，由第一步得到的陀螺仪偏置，可以预积分速度及位移 $\Delta{v_{ij}},\Delta{p_{ij}}$：
$\Delta{v_{ij}}=\sum_{k=i}^{j-1}{\Delta R_{ik}\tilde{a}_k\Delta t} \\ \Delta{p_{ij}}=\sum_{k=i}^{j-1}{\Delta v_{ik} \Delta t+\frac{1}{2} \Delta R_{ik}\tilde{a}_k\Delta t^2} \\ \Delta{R_{ij}}=\prod_{k=i}^{j-1}{Exp((\tilde{w}_k-b^g)\Delta t)}$

2.1 尺度s估计

视觉观测尺度不确定，有 $T_{WC}=\left[ \begin{array}{} R_{WC} & s_Wp_C\\ 0^T & 1 \end{array}\right ]$,因此，由camera系到body系的转化过程中，有：
$T_{WB}=\left[ \begin{array}{} R_{WB} & _Wp_B\\ 0^T & 1 \end{array}\right ]=T_{WC}T_{CB}=\left[ \begin{array}{} R_{WC} & s_Wp_C\\ 0^T & 1 \end{array}\right ]·\left[ \begin{array}{} R_{CB} & _Cp_B\\ 0^T & 1 \end{array}\right ]=\left[ \begin{array}{} R_{WC}R_{CB} & R_{WC}{_Cp_B}+{s_Wp_C}\\ 0^T & 1 \end{array}\right ]$其中 $T_{CB}$为标定得到，即 $_Wp_B=R_{WC}{_Cp_B}+{s_Wp_C}$代入位置关联方程：
$_Wp_{B}^{i+1}=_Wp_{B}^{i}+_Wv_{B}^{i}\Delta t_{i,i+1}+\frac12g_W\Delta t_{i,i+1}^2+R_{WB}^i\Delta p_{i,i+1}$得到： $s_Wp_{C}^{i+1}=s_Wp_{C}^{i}+_Wv_{B}^{i}\Delta t_{i,i+1}+\frac12g_W\Delta t_{i,i+1}^2+R_{WB}^i\Delta p_{i,i+1}+(R_{WC}^i-R_{WC}^{i+1}){_Cp_B}$其中， $_Wp_{C}^{i},R_{WC}^i,R_{WC}^{i+1},_Wp_{C}^{i+1}$由视觉计算得到， $\Delta t_{i,i+1}$已知， $\Delta p_{i,i+1}$由预积分得到, $R_{CB},_Cp_B$为标定得到。解该线性方程组得到 $s、g_W$，然而方程中含有 $_Wv_B^i$,为避免计算这N个速度，降低复杂度。我们将连续的三帧提取两个关系，利用速度关联方程： $_Wv_{B}^{i+1}=_Wv_{B}^{i}+g_W\Delta t_{i,i+1}+R_{WB}^i\Delta v_{i,i+1}$为简化表述，使用1，2，3代替 $i,i+1,i+2$，有：
$s_Wp_{C}^{3}=s_Wp_{C}^{2}+_Wv_{B}^{2}\Delta t_{23}+\frac12g_W\Delta t_{23}^2+R_{WB}^2\Delta p_{23}+(R_{WC}^2-R_{WC}^{3}){_Cp_B} \\ {s_Wp_{C}^{2}=s_Wp_{C}^{1}+_Wv_{B}^{1}\Delta t_{12}+\frac12g_W\Delta t_{12}^2+R_{WB}^1\Delta p_{12}+(R_{WC}^1-R_{WC}^{2}){_Cp_B}} \\ {_Wv_{B}^{2}=_Wv_{B}^{1}+g_W\Delta t_{12}+R_{WB}^1\Delta v_{12}}$第一式乘 $\Delta t_{12}$与第二式乘 $\Delta t_{23}$做差，结合代入第三式，得到：
$\left( \begin{array}{} \lambda(i)&\beta(i) \end{array}\right )\left( \begin{array}{} s\\g_W \end{array}\right )=\gamma(i)$