VI-ORBSLAM 初始化

1. VI-ORBSLAM 初始化过程

1.1 估计 gyro bias

采用的方法和 VINS 一样，相邻两帧 KEYFRAME旋转 和 惯导预积分 lambda 进行对齐，估计 bias。（这里需要求 误差对gyro bias 的 Jacobian 用来更新）

1.2 初步估计 scale 和 gravity

采用的线性方程解得，这里需要4个关键帧，构建 3*(N-2)==6 个方程，解4个未知变量。这里解得重力没有利用上 2-Norm 为9.81 这一条件，这里也没有初始化 速度。

根据上述公式

1.3 Refine scale 和 gravity并且估计加表的 bias

利用上重力的模为9.81这一限制条件，并且方向为(0,0,-1)，计算初始的重力和(0,0,-1)的旋转矩阵 Rwi , 将当前的(0,0,-9.81) 转化为初始估计重力的坐标系上。紧接着，对重力的方向加上微小的扰动，再次优化重力的方向( 注: 两个自由度，Z轴上的旋转不影响最终的方向 ) 。现在加入 acc 的 bias ，进行改写为:

1.4 Velocity 估计

上面的参数都求得后，直接根据1.3中的公式进行求解。相邻关键帧之间的速度可由预积分的公式求解。