VINS程式碼主體結構和流程剖析——前端
前言
最近花了點時間學習了下經典的VSLAM框架VINS，前後端都採用最佳化的方式，且在執行過程中實時估計IMU加速度計和陀螺儀bias，在初始化階段還可以實時最佳化camera到IMU的外參，解決了很多之前SLAM框架在實際中使用的問題，例如使用時間久之後的外參漂移、IMU的bias不準導致的系統不夠魯棒等等，實測也具有非常良好的效果。但是VINS的程式碼確實不太友好，當然秦大佬只是用於發paper並不需要放過多心力在工程化上。已經有很多文章分析這份程式碼的細節了，這裡主要針對VINS-Fusion程式碼主體的結構和流程做一些解析記錄，便於日後的工程化和一些進一步最佳化工作。
程式碼結構剖析
Estimator是前端的一個核心類，整個系統的入口和核心處理邏輯都位於這個類中，其中透過inputIMU和inputImage將IMU資料和影象資料喂入，待進一步處理，整體邏輯如下：
影象被喂入後馬上進行了特徵跟蹤與提取，由featureTracker這個類負責，這個待會兒再談；提取完特徵後圖像就被拋掉了，之後只需要用該幀影象的feature參與到之後的計算即可
imu資料進入後也被存入buff中，待之後的使用
processMeasurements（）這個函式可以與資料接受部分解耦，因此VINS也提供了多執行緒開關，把這個函式單獨放在一個執行緒中執行，避免其阻塞資料的讀取。
先看看feature track部分：
使用上一幀的feature結合光流法得到本幀的feature，這些feature被認為是追蹤上了，在之後的triangulate中只有連續追蹤到三幀以上的feature才會進行triangulate，這裡如果有預測過程會使用預測的feature位置
判斷透過光流得到的feature數量是否足夠（feature數量可透過引數指定），不夠則在已有feature的mask外補充feature - 對所有feature進行反畸變和計算速度等，然後返回結果。
再看看processMeasurements（）這個函式：
做影象與IMU資料的同步
imu資料採用一個專用類IntergrationBase進行積分處理，這個類主要完成IMU資料積分得到RPY，並使用ESKF的思路在後續bias更新後同步更新一段時間內的RPY，細節可以參考Error-State Kalman Filter理解與公式推導
拿到同時刻的image feature和imu積分資料，然後進入processImage（）這個函式，到這裡就開始真正的計算了。
進入processImage（）後首先會將所有feature送入feature manager管理起來，這個類的主要功能是根據feature的id把每個feature放入一個特定的buffer管理起來，用於儲存這個feature與keyframe的關聯序列，同時也可以完成traiangulate；然後如果引數有設定的話會估計一次camera到IMU的外參的旋轉部分，這個過程只進行一次，也就是外參線上校準。之後可以看到有兩條支線，對應initialization過程和vio過程，也對應vins-mono論文中的V和VI部分。
initialization過程：首先initialStructure（），也就是採用SFM估算sliding window中的兩個關鍵幀的pose並初步triangulate一些feature，再採用PnP得到所有幀的pose，對應論文中V中A部分，這裡提一下，這個過程中如果運動不夠（透過IMU速度判斷），或者用於SFM的feature視差過小都會導致初始化失敗；然後進入visualnitialAlign（）部分，對應論文V中B和C部分，依次完成IMU陀螺儀bias校準，速度、重力和尺度初始化，以及重力值的最佳化，然後做一些位置更新、IMU使用新的bias和重力重新積分、以及位置更新到世界座標系的操作；完成之後，更新sliding window中的所有狀態，完成初始化。
VIO過程：因為完成了初始化，因此直接對feature manager中管理的符合條件（連續跟蹤三次以上）的feature進行triangulate，得到其深度，然後構建最佳化問題進行最佳化，得到最新的最佳化後的pose，對應論文中VI部分；之後將追蹤上的feature對其在跟蹤到的幀上依次進行重投影誤差計算，得到誤差大於一定閾值後將被拋掉，避免一些錯誤的feature引入噪聲，這部分論文中沒有；最後進行滑動視窗進行marginalization，對應論文中VI的D部分。
到此，前端處理的部分就結束了。
一些小細節
關於slidingwindow：
每幀都保留著從影象中追蹤的feature和一個IntergrationBase，這個IntergrationBase存有從上一幀到這一幀的所有imu原始資料、bias、重力值以及積分結果，用以在最佳化和最佳化完成後使用新的bias重新積分使用。其內部使用了一些ESKF的思路，可以參考KF、EKF、ESKF的區別與聯絡中最後的部分。
有一個比較噁心的資料結構：map
int:
feature id
int:
camera id
7*1 matrix:
3d點x，y，z，2d點x，y（投影到相機sensor平面的座標），2d點速度v_x，v_y