VINS-Mono运行与评测

由于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