文章目錄
0 前言
目前網絡上對於VINS-mono的代碼已經有很多講解和註釋了，但是對於VINS-FUSION（以下簡稱VF）的註釋還是很少的，剛好本人最近也正在學習VIO的相關知識，所以對VF按照程序執行順序進行了十分詳細的註釋，同時爲了和大家進行交流學習，所以把相關注釋代碼進行開源。因水平有限，錯誤肯定不少，還請各位大佬們指正。
前期準備
-
C++。
沒有c++基礎的同學建議先觀看北大的c++課程：c++程序設計 -
視覺SLAM14講。
經典書籍，這個不必過多介紹。 -
VIO基礎知識。
深藍學院有賀博和高博講的VIO課程，可以自行百度。
另外，崔華坤也有一系列的博文對VIO進行講解。
【泡泡讀者來稿】VINS 論文推導及代碼解析（一）
【泡泡讀者來稿】VINS 論文推導及代碼解析（二）
【泡泡讀者來稿】VINS 論文推導及代碼解析（三）
【泡泡讀者來稿】VINS 論文推導及代碼解析（四）
本次工作
我首先一步步的把代碼全部註釋了，十分的詳細，對於C++和OpenCV的一些操作也進行了詳細的註釋，對於剛入門的同學應該還是有幫助的。之後我將代碼開源，並寫了相應的博客進行講解。
開源程序:
https://github.com/kuankuan-yue/VINS-FUSION-leanrning.git
相應博客：
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(1)
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(2)
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(3)
VINS-FUSION代碼超詳細註釋（VIO部分）/VIO入門(4)
1 程序入口rosNodeTest.cpp
1.1 定義內容
運行程序時，首先進入的是主程序rosNodeTest.cpp
裏邊主要定義了 估計器、 緩存器 、 獲取傳感器數據的函數 和 一個主函數
// 獲得左目的message
void img0_callback(const sensor_msgs::ImageConstPtr &img_msg)
// 獲得右目的message
void img1_callback(const sensor_msgs::ImageConstPtr &img_msg)
// 從msg中獲取圖片，返回值cv::Mat，輸入是當前圖像msg的指針
cv::Mat getImageFromMsg(const sensor_msgs::ImageConstPtr &img_msg)
// 從兩個主題中提取具有相同時間戳的圖像
// 並將圖像輸入到估計器中
void sync_process()
// 輸入imu的msg信息，進行解算並把imu數據輸入到estimator
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
// 把特徵點的點雲msg輸入到estimator
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
// 是否重啓estimator，並重新設置參數
void restart_callback(const std_msgs::BoolConstPtr &restart_msg)
// 是否使用IMU
void imu_switch_callback(const std_msgs::BoolConstPtr &switch_msg)
// 相機的開關
void cam_switch_callback(const std_msgs::BoolConstPtr &switch_msg)
int main(int argc, char **argv)
1.2 程序執行
1.2.1 獲取參數並設置參數
具體的方法在函數
主函數中，主要是執行以下各個步驟訂閱ROS信息，然後進行處理
readParameters(config_file);// 讀取參數
estimator.setParameter();// 設置參數
ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
ros::Subscriber sub_feature = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
ros::Subscriber sub_img0 = n.subscribe(IMAGE0_TOPIC, 100, img0_callback);
ros::Subscriber sub_img1 = n.subscribe(IMAGE1_TOPIC, 100, img1_callback);
ros::Subscriber sub_restart = n.subscribe("/vins_restart", 100, restart_callback);
ros::Subscriber sub_imu_switch = n.subscribe("/vins_imu_switch", 100, imu_switch_callback);
ros::Subscriber sub_cam_switch = n.subscribe("/vins_cam_switch", 100, cam_switch_callback);
其中有幾個比較重要的函數，在下方進行說明。
1.2.2 imu_callback
其中imu_callback中訂閱imu信息，並將器填充到accBuf和gyrBuf中，之後執行了fastPredictIMU和pubLatestOdometry
fastPredictIMU使用上一時刻的姿態進行快速的imu預積分
// 使用上一時刻的姿態進行快速的imu預積分
// 用來預測最新P,V,Q的姿態
// -latest_p,latest_q,latest_v,latest_acc_0,latest_gyr_0 最新時刻的姿態。這個的作用是爲了刷新姿態的輸出，但是這個值的誤差相對會比較大，是未經過非線性優化獲取的初始值。
void Estimator::fastPredictIMU(double t, Eigen::Vector3d linear_acceleration, Eigen::Vector3d angular_velocity)
pubLatestOdometry構建一個odometry的msg併發布
//構建一個odometry的msg併發布
void pubLatestOdometry(const Eigen::Vector3d &P, const Eigen::Quaterniond &Q, const Eigen::Vector3d &V, double t)
1.2.3 feature_callback
feature_callback的作用是獲取點雲數據，之後填充featureFrame，並把featureFrame通過inputFeature輸入到estimator，且填充了featureBuf
1.2.4 sync_process
之後通過
std::thread sync_thread{sync_process}; //創建sync_thread線程，指向sync_process，這裏邊處理了measurementpross的線程
進入sync_process進行處理
// 從兩個主題中提取具有相同時間戳的圖像
// 並將圖像輸入到估計器中
void sync_process()
該函數中，首先對是否雙目進行判斷。
如果是雙目，需要檢測同步問題。對雙目的時間進行判斷，時間間隔小於0.003s的話則使用getImageFromMsg將其輸入到image0和image1變量之中。之後estimator.inputImage。
如果是彈幕，則直接estimator.inputImage
結束
至此，主程序rosNodeTest.cpp就已經全部運行完了，之後會跳到其他各個文件中進行圖片的處理，優化等等，詳情請查看之後的博客。