OKVIS VIO 解析 1 初始化

      OKVIS利用IMU读数初始化位姿。首先检测IMU第一帧数据是不是在第一帧图像之前，如果不是则舍弃掉这帧图像。然后在一定的时间段内(从0到第一帧图像时间+0.02后)并且假设静止，这时位移增量为0，角度增量则为这个时间段加速度平均归一化后的值于重力归一化值之间的夹角，从而获得初始位姿。

  这里认为初始位姿即为第一帧图像采集时相机的位姿，可以用来帮助检测描述特征点。之后可以用来作为匹配时的状态初始值（states）。

  路标点（landmarks）的初始化是依赖于两帧图像立体匹配远离。路标点是三维的，在双目的情况下通过第一帧图像的左右目匹配三角化获得路标点的三维齐次坐标（stereoMatch），而尺度（scale）则从IMU获得。如果是单目，则将第一帧和第二帧作为关键帧，通过第一帧与第二帧之间的关键帧匹配（2D-2D）三角化得到初始路标点。单目时，程序中因为没有初始化路标点，3D-2D匹配不会实际运行。