單目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<KeyFrame*> &lLocalKeyFrames, bool* pbStopFlag,Map* pMap, cv::Mat& gw, LocalMapping* pLM=NULL)
主要在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。