使用evo测评vins-mono
evo测评vins-mono
- 一 evo安装
- 二 vins-mono代码修改
- 三 测试
一 evo安装
可以参考官方文档，或者：
Sudo apt install python-pip
Pip install evo --upgrade --no-binary evo --user
需要注意，本文使用的是Euroc数据集的MH_01_easy，平时我们运行vins-mono时用的是MH_01_easy.bag，而在evo中我们需要用到ASL Dataset Format，请事先下载好。
二 vins-mono代码修改
vins-mono自动保存的数据格式不符合tum格式，所以我们需要对vins-mono的源代码做一些修改：
- …/VINS-Mono/vins_estimator/src/utility/visualization.cpp中的pubOdometry()函数：
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;
foutC.close();
改成：
ofstream foutC(VINS_RESULT_PATH, ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << header.stamp.toSec() << " ";
foutC.precision(5);
foutC << estimator.Ps[WINDOW_SIZE].x() << " "
<< estimator.Ps[WINDOW_SIZE].y() << " "
<< estimator.Ps[WINDOW_SIZE].z() << " "
<< tmp_Q.x() << " "
<< tmp_Q.y() << " "
<< tmp_Q.z() << " "
<< tmp_Q.w() << endl;
foutC.close();
- …/VINS-Mono/pose_graph/src/pose_graph.cpp中的addKeyFrame()函数：
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;*/
改成：
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 << " ";
loop_path_file.precision(5);
loop_path_file << P.x() << " "
<< P.y() << " "
<< P.z() << " "
<< Q.x() << " "
<< Q.y() << " "
<< Q.z() << " "
<< Q.w() << endl;
- …/VINS-Mono/pose_graph/src/pose_graph.cpp中的updatePath()函数：
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;
改成：
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 << " ";
loop_path_file.precision(5);
loop_path_file << P.x() << " "
<< P.y() << " "
<< P.z() << " "
<< Q.x() << " "
<< Q.y() << " "
<< Q.z() << " "
<< Q.w() << endl;
- …/VINS-Mono/pose_graph/src/pose_graph_node.cpp中的main()函数：
把vins_result_loop.csv改成vins_result_loop.txt即可
三 测试
进入工作区，重新catkin_make一遍后，运行vins-mono，当跑完一遍后，在我们设定的输出位置会出现一个vins-result_loop.txt文件。
需要注意的是，输出位置是在/home/gu/catkin_ws/src/VINS-Mono/config/euroc/euroc_config.yaml中设置的，比如我设置的是：
在文章一开始，我提到过需要使用的是MH_01_easy的ASL Dataset Format格式，所以当我们下载完成后，解压，进入到该文件下的state_groundtruth_estimate0文件夹中，我们会发现里面有一个data.csv文件：
首先我们需要把他转换成tum格式的文件：（terminal中先要进入到state_groundtruth_estimate0/目录下）
evo_traj euroc data.csv --save_as_tum
转换完成后，就在当前目录下输入：
evo_traj tum YOUR_MAP_PATH/vins_result_loop.txt --ref=data.tum -p --plot_mode=xyz --align --correct_scale
对比完轨迹后，我们还可以查看RMSE等内容，只需要在同一个目录下输入：
evo_ape tum data.tum ~/posegraph/output/vins_result_loop.txt -va --plot
控制台输出如下：
结果：
