vio筆記

1.從ros_vio.cc入 main()，

2.建立SLAM系統，ORB_SLAM2::System SLAM

  進入system（system.cc line211）:建立關鍵幀資料庫、建立地圖、建立用於視覺化的Drawer.

   初始化Tracking執行緒：從設定檔案中，裝載相機引數，Load ORB引數。

   初始化Local Mapping執行緒並launch：獲得區域性視窗大小。

   初始化LoopClosing執行緒並launch

   初始化Viewer執行緒並launch   

設定執行緒之間的指標。

3.在 main（）中引數設定ConfigParam，再：

資料對齊：imu資料與讀取的RGB資料

4.在在 main（）中開始Track.line173: SLAM.TrackMonoVI();輸入影像資料和IMU資料及對應的時間戳，輸出相機位姿。當跟蹤失敗時，輸出為空。

先檢查模式：如果當前處於定位模式，等待直到Local Mapping結束，將只跟蹤設為1，定位模式（localization）設為0.

再進入tracking中的GrabImageMonoVI（）：

如果沒有初始化或者目前還沒有圖片時，使用Frame(引數少一個)（Frame.cc line 416）,否則使用Frame(引數多一個)（Frame.cc line143）:

    其中，少一個引數的Frame,(scale level info ???) ORB提取，為加速特徵匹配將關鍵點對齊網格。

    其中，多一個引數的Frame,與上面的函式相比，多了而一個copy imu 資料的部分。

 接著進入Track()：

     先判斷是否有圖，再獲取map執行緒鎖，map不被改變，根據map是否更新有不同的更新flag的操作. 如果狀態為未初始化，則進行雙目或RGBD對map的初始化：【】設定幀原始的pose,建立關鍵幀，將關鍵幀插入地圖中，建立地圖點並融進關鍵幀中等等。再更新FrameDrawer.

     如果系統狀態為初始化後，則Track 幀。使用勻速模型或重定位（當跟蹤失敗時）來初始化相機位姿：當跟蹤和區域性建圖同時開啟時（這也是常用模式，除非單獨使用‘只跟蹤’模式）。區域性見圖可能改變了上一幀中的一些地圖點，所以需要CheckReplacedInLastFrame()。當視覺-慣性初始化之後，重定位之後的20幀，開始只使用視覺跟蹤-參考幀模型跟蹤：計算詞袋向量，和參考幀進行ORB匹配，找到足夠的匹配後，使用pnp求解器Optimizer::PoseOptimization(&mCurrentFrame)，並丟棄異常值。如果V-I沒有被初始化，視為純V-slam,沒有IMU資料，參考幀模型跟蹤，沒有IMU資料，勻速模型跟蹤，若勻速模型失效，使用參考幀模型跟蹤（Tracking.cc line950）。勻速模型：ORB匹配，根據上一幀的參考關鍵幀來更新上一幀的位姿，如果在定位模式下，創‘vo’點。

   如果狀態不為ok的話，進行重定位Relocalization()：計算詞袋向量。當跟丟時進行重定位。跟丟後，查詢關鍵幀資料庫中的關鍵幀候選DetectRelocalizationCandidates（）：搜尋所有與當前幀共享一個詞的關鍵幀，計算共享足夠詞數的關鍵幀，計算相似性得分，由共視來累加分數，返回所有分數高於0.75×最好分的關鍵幀。

如果查詢沒有候選關鍵幀，則重定位也失敗。否則，和每個候選參考幀進行ORB匹配，找到足夠的匹配後，使用pnp求解器。（其後還有可選擇性地執行一些P4P的RANSAC迭代（此處跳過，tracking line2147），直到找到由足夠inlier支援的相機位姿），如果有相機位姿和匹配的初始化估計，跟蹤區域性地圖TrackLocalMap()：其中更新區域性地圖，搜尋區域性點，並優化位姿。也有TrackLocalMapWithIMU（）的選擇（此處跳過，tracking line301）。

   如果狀態OK的話，在重定位後，加幀來重計算IMU偏置。

    如果跟蹤正常，檢查是否插入了一個關鍵幀。

   儲存幀的位姿資訊，之後輸出軌跡。

    Track()結束。

    返回位姿資訊。GrabImageMonoVI()結束。

TrackMonoVI()結束。

等bLocalMapAcceptKF()返回0，區域性建圖結束，停止。

儲存關鍵幀軌跡及位姿。

結束所有執行緒。