VINS-Mono源碼解析（一）系統架構

1. VINS-Mono 簡介

VINS-Mono是HKUST的Shen Shaojie團隊開源的一套Visual-Inertial融合定位算法. 介紹見(https://github.com/HKUST-Aerial-Robotics/VINS-Mono), 論文裡面也有.

　　此外,他們還開源了ios版本(https://github.com/HKUST-Aerial-Robotics/VINS-Mobile), 有興趣可以去下載下傳源碼跑一跑, 好像已經有國外團隊基于這套開源代碼開發了一些AR的App.

　　VINS-Mono是目前開源SLAM裡面效果非常不錯的Visual和Inertial融合SLAM算法, 通過Sliding Windows優化進行視覺和IMU的緊耦合實作VIO, 此外還加了DBoW做Loop Closure, 算是一套完整的SLAM系統. 該算法的前端比較簡單,就Harris角點加LK光流跟蹤, Loop Closure也挺正常, 算法的厲害之處在于後端Visual Inertial融合優化部分, 也就是VIO部分, 這是VINS算法的核心,也是後面VINS-Mono代碼閱讀的重點.

　　目前視覺和IMU融合分為松耦合和緊耦合, 松耦合的性能要遜色于緊耦合. 緊耦合的主流融合架構有基于濾波的方法(MSCKF)和基于優化的方法(VINS-Mono).

　　VINS-Mono在ROS下開發的, 是以可以很友善的編譯和使用，跟着readme安裝就可以跑起來, so easy.

2. VINS-Mono系統架構

　　VINS-Mono的系統架構如下圖

VINS-Mono源碼解析（一）系統架構VINS-Mono源碼解析（一）系統架構

主要有以下幾個部分:

1. Measurement Preprocessing(觀測預處理):對圖像提feature做feature tracking,輸出tracked feature list, 對IMU做預積分,輸出兩幀圖像間的IMU積分結果.

這裡應該還有個很重要的步驟, IMU和圖像的資料同步, VINS-Mono代碼中貌似沒有展現, 不知道Mobile版本中是否有.

2. Initialization: vision-only SfM用純視覺估計相機運動和特征深度, 視覺得到一個相對運動, IMU預積分得到一個相對運動, 二者做alignment, 進而标定出尺度, 重力加速地, 速度, 和bias.

3. Local BA: 後端融合優化, 狀态向量包括: sliding window中的IMU states + 相機外參 + feature點的深度. loss包括: marginalization提供的prior + IMU residuals + visual residuals. 優化完之後, 得到各個時刻相機的位姿, 實作VIO.

4. Loop detection + Global Pose Graph Optimization: VIO畢竟有累積誤差, 是以跟正常vSLAM一樣, 加入閉環檢測, 然後優化一個pose graph, 值得注意的是這裡是隻優化4 DoF, 因為scale, roll 和 pitch是可觀的, 誤差隻會在(x,y,z)和yaw四個次元上累積.

3. VINS-Mono的ROS架構

　　VINS-Mono主要包含兩個節點: 前端節點feature_tracker_node和後端節點estimator_node. 前端節點處理Measurement Preprocessing中的Feature Detection and Tracking, 其他幾個部分(IMU preintegration, initialization, LocalBA, Loop closure)都是在estimator_node中處理.

　　運作樣例程式:

運作feature_tracker節點和estimator節點, 訂閱圖像和IMU資料, 釋出位姿, 3D特征點等消息給RVIZ顯示

運作RVIZ

讀取rosbag, 釋出圖像和IMU資料給VINS節點

rosbag play YOUR_PATH_TO_DATASET/

feature_tracker_node中就一個主線程, 接收圖像資訊,調用img_callback()函數進行處理.
estimator_node節點中開了3個子線程, 分别是:process,loop_detection和pose_graph. 其中
- process()線程處理VIO後端, 包括(IMU preintegration, initialization, LocalBA)
- loop_detection()線程處理閉環檢測
- pose_graph()線程分别處理全局優化
  
  如果不需要做loop closure, 可以把LOOP_CLOUSRE置0, 則後面兩個線程不會建立.