由于VINS-Mono保存的軌跡格式與EVO所使用的格式不同，VISNmono輸出的軌跡格式不符合tum資料集和euroc資料集的格式，因此需要對源代碼就行修改，更改保存軌跡的格式，（如果只有一條軌跡也可以直接需要輸出的csv檔案，但是如果要反復評估一個演算法的話，比如運行10次求平均值，這樣還是修改源代碼方便一點）

1 修改VINS-mono軌跡保存代碼

修改下列兩個檔案，共計3個地方

• vins_estimator/src/utility/visualization.cpp
• pose_graph/src/pose_graph.cpp

1.1 修改 visualization.cpp

找到以下代碼段

	 // write result to file
	 ofstream foutC(VINS_RESULT_PATH, ios::app);
	 foutC.setf(ios::fixed, ios::floatfield);
	 foutC.precision(0);
	 foutC << header.stamp.toSec() * 1e9 << ",";
	 foutC.precision(5);
	 foutC << estimator.Ps[WINDOW_SIZE].x() << ","
	       << estimator.Ps[WINDOW_SIZE].y() << ","
	       << estimator.Ps[WINDOW_SIZE].z() << ","
	       << tmp_Q.w() << ","
	       << tmp_Q.x() << ","
	       << tmp_Q.y() << ","
	       << tmp_Q.z() << ","
	       << estimator.Vs[WINDOW_SIZE].x() << ","
	       << estimator.Vs[WINDOW_SIZE].y() << ","
	       << estimator.Vs[WINDOW_SIZE].z() << "," << endl;
	 

修改為以下代碼

1.2 修改 pose_graph.cpp

1） 在路徑 VINS-Mono-Modify/pose_graph/src/pose_graph.cpp 在函式 addKeyFrame() 中 找到以下代碼段

if (SAVE_LOOP_PATH)
{
		ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file.precision(0);
        loop_path_file << cur_kf->time_stamp * 1e9 << ",";
        loop_path_file.precision(5);
        loop_path_file  << P.x() << ","
              << P.y() << ","
              << P.z() << ","
              << Q.w() << ","
              << Q.x() << ","
              << Q.y() << ","
              << Q.z() << ","
              << endl;
        loop_path_file.close();
 }

2） 在路徑 VINS-Mono-Modify/pose_graph/src/pose_graph.cpp 在函式 updatePath() 中 找到以下代碼段

if (SAVE_LOOP_PATH)
        {
            ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
            loop_path_file.setf(ios::fixed, ios::floatfield);
            loop_path_file.precision(0);
            loop_path_file << (*it)->time_stamp * 1e9 << ",";
            loop_path_file.precision(5);
            loop_path_file  << P.x() << ","
                  << P.y() << ","
                  << P.z() << ","
                  << Q.w() << ","
                  << Q.x() << ","
                  << Q.y() << ","
                  << Q.z() << ","
                  << endl;
            loop_path_file.close();
        }

修改為：

1.3 修改VINS-mono運行引數

在路徑 VINS-Mono/config/euroc 下有組態檔 euroc_config.yaml

output_path： 設定軌跡保存位置
pose_graph_save_path 設定位姿圖保存位置
loop_closure: 0 表示不使用回環 1表示使用回環
estimate_td: 0表示不估計傳感器之間的延時 1表示啟動

2 EVO 顯示軌跡

EVO的安裝可以參考我之前的博客 SLAM 軌跡評估工具EVO

2.1 分析單條軌跡

evo_ape euroc ../groundturth_euroc/MH_01_easy/state_groundtruth_estimate0/data.csv vinsmono_noloop_euroc_MH01_01.csv -va --plot --plot_mode xy --save_results results/VINS_Mono_Noloop_MH01_01

其中：

• 軌跡的真值：…/groundturth_euroc/MH_01_easy/state_groundtruth_estimate0/data.csv
• VINS-Mono估計的軌跡 vinsmono_noloop_euroc_MH01_01.csv
• 設定對齊顯示 -va
• 在XY平面繪圖 --plot --plot_mode xy
• 設定保存路徑 --save_results results/VINS_Mono_Noloop_MH01_01

這里如果我們要分析多條曲線的時候就不能在用euroc命令了，需要換成tum的命令

2.2 分析多條軌跡

evo_traj tum msckf_vio_result/msckf_vio_MH04_03.txt VINSMono_EUROC/vinsmono_noloop_euroc_MH04_01.csv --ref=groundturth_euroc/MH_04_difficult/state_groundtruth_estimate0/groundtruth.csv -a --plot --plot_mode xy

3 EUROC資料集測驗

3.1 定位精度

不得不說，VINS-Mono還是相當優秀的，我自己在EUROC資料集上運行的結果和論文中的結果是一致的，圖片中左圖來源于論文[1]

下圖為VINS-Mono在EUROC資料集上開啟回環模式下的軌跡圖

3.2 計算時間

在跟蹤時間上 VISN-Mono 把特征點跟蹤和位姿求解分成了兩個ROS節點運行，其在整個EUROC資料集上的平均跟蹤時間為0.01s(10 ms), 但位姿估計的時間確有36ms, 詳細的資料見下表，因此也可以在時間上優化VINS-Mono的位姿優化

參考資料

[1] Qin T, Li P, Shen S. Vins-mono: A robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.
[2] https://blog.csdn.net/crp997576280/article/details/105385240