VIO 初始化系列2---VINS-Mono初始化

参考论文：VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator
Robust initialization of monocular visual-inertial estimation on aerial robots
参考博客：VINS-FUSION源码框架及C++知识点总结
参考课程：从0开始手写VIO
在参考博客里有介绍过初始化部分的大致流程，趁着现在要做VIO方面的项目， 再来仔细梳理该初始化过程。深蓝学院的课件里对此有详细的内容，所以本博客主要引用课件内容。

估计相机-陀螺仪旋转外参
CalibrationExRotition: 该步骤估计相机与陀螺仪之间的外参，类似于手眼标定的过程， 就是求解一个AX=XB线性问题。


在此没有估计平移外参， 因为默认为该外参对系统的影响是线性的， 可以通过离线标定， 出场设定或者尺子测量得出。在VINS-Mono中如果设定要标定外参， 但该平移外参也会在初始化阶段最后的Full BA优化中进行优化。

估计陀螺仪bias
solveGyroscopeBias()

代码中构建了对应的最小二乘问题，没有使用求解器， 直接用高斯牛顿一步迭代求解出该bias， 初始化过程中滑窗内的陀螺仪bias都是认为相同的。

初始化速度、重力和尺度因子
非线性部分的陀螺仪bias已经在之前解决好，剩下的估计问题就是一个线性问题了。


该线性求解过程中会求出重力在c0系下的值g^c0 ， 通过该值的模长与实际重力大小9.81的误差是否大于0.5（也就是约5%的重力值），来判断此次初始化是否成功。
优化重力向量
为何采用该方法？可否假设估计出来的g^c0重力方向有效，赋予模长后重新估计？ 也就是将下面的w1, w2强制设为0。或者在优化的时候加入模长限制？

将相机坐标系对齐世界坐标系