单目与IMU融合初始化

orbslam论文初始化方法：

本初始化算法主要包括（1）陀螺仪偏差的估计；（2）在不考虑加速度偏差下的尺度和重力矢量的粗略估计；（3）加速度偏差的估计和重力矢量，尺度的优化；（4）速度估计。