ORB_SLAM2+IMU详解
单目VIO学习
细节探讨：
PVR优化顶点：g2o::VertexNavStatePVR，定义在g2otypes.h中
Bias优化顶点：g2o::VertexNavStateBias
一：关于优化部分：Optimizer.cpp
1. PoseOptimization(Frame *pFrame,Frame* pLastFrame, const IMUPreintegrator& imupreint, const cv::Mat&gw, const bool& bComputeMarg)
PoseOptimization(Frame *pFrame, KeyFrame* pLastKF, constIMUPreintegrator& imupreint, const cv::Mat& gw, const bool&bComputeMarg=false)
这部分主要用在TrackWithIMU（相当于相机跟踪），TrackLocalMapWithIMU（相当于局部地图跟踪）中。跟纯视觉的优化方式相同，这里只是优化当前帧的位姿。两个方法的区别是选取的前一帧是关键帧还是一般帧。在TrackLocalMapWithIMU中，如果局部地图更新成功，就选取关键帧，如果局部地图未更新就选取一般帧。在TrackWithIMU中，局部地图初始化成功或则地图更新成功，就选取关键帧，否则选取一般帧。
（1） 首先设置帧的顶点，一共分为4个顶点，包括当前帧和前一帧PVR和Bias偏置（这里的顶点模型定义在g2otypes.h中），
（2） 然后定义上面帧顶点连接的边，如下图所示：1代表前一帧PVR和Bias之间的边，2代表前一帧PVR、Bias和当前帧PVR之间的边，3代表前一帧和当前帧Bias之间的边。
（3） 最后是设置MapPoint对应的指向当前帧位姿顶点的自己的一元边，这里和纯视觉的优化位姿是一样的，利用MapPoint的3d点和位姿关系将3d点投影到图像中，和图像中的位置作比较，这里没有定义MapPoint的顶点，只定义了当前帧的位姿顶点，因此是一个一元边，如上图中4所示，最后指向自己。
（4） 最后就是迭代优化，和纯视觉的处理方式一样
2. OptimizeInitialGyroBias
这部分主要使用在Tracking和LocalMapping中，有三种模式，分别是针对一般帧和关键帧（关键帧中分为两种参数输入，但都是调用统一函数，因此归为一类），Tracking中使用在相机跟踪和局部地图跟踪之后（使用的是20帧一般帧），LocalMapping中使用在VIO联合初始化中（使用的是关键帧序列）。这里的顶点是陀螺仪偏置GyroBias，使用旋转R来优化，边是EdgeGyrBias自己定义继承基本一元边，其误差计算以及更新在g2otypes.cpp中完成。误差计算的公式就是初始化中的公式，雅各比矩阵的更新还没搞明白。
3. LocalBundleAdjustmentNavState(KeyFrame*pKF, const std::list
主要在LocalMapping线程中使用，具体的使用地点是与纯视觉相同，在所有的关键帧和MapPoint都处理好之后，判断是否完成VIO初始化，完成就进行这一优化方法，否则按原视觉优化方法。
（1） 获得和当前关键帧相连（共视）的所有关键帧，放到lLocalKeyFrames中；获得能够被lLocalKeyFrames关键帧（包含当前关键帧）观测到的局部MapPoints，放到lLocalMapPoints中；获得能够观测到lLocalMapPoints的其他不在局部地图中的关键帧，这一部分关键帧放到lFixedCameras中，不参与下面的优化（也就是不优化其结果），但也可以起到约束的作用。
（2） 完成上面的与处理之后，就进行真正的优化，这里优化1代表当前处理的关键帧的PVR和当前处理关键帧的前一关键帧的PVR、Bias的连接，2代表当前处理关键帧的Bias与前一关键帧的Bias的连接。3代表Local MapPoints与局部地图中的关键帧和Fixed中的关键帧的连接。
4. GlobalBundleAdjustmentNavState(Map*pMap, const cv::Mat& gw, int nIterations, bool* pbStopFlag, const unsignedlong nLoopKF, const bool bRobust)
这部分主要使用在LoopClosing中，主要是优化所有的状态量（PVR、Bias）以及MapPoints。