EVO评估VINS-Mono 的精度和性能效果

一 evo安装

可以参考官方文档https://gitcode.net/mirrors/michaelgrupp/evo?utm_source=csdn_github_accelerator，或者：

Sudo apt install python-pip

Pip install evo --upgrade --no-binary evo --user

需要注意，本文使用的是Euroc数据集(https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets)的MH_01_easy，

二 vins-mono代码修改

我的vins-mono代码在/home/yang/vins-mono-catkin_ws/src路径下,平时我们运行vins-mono时用的是MH_01_easy.bag，而在evo中我们需要用到ASL Dataset Format，请事先下载,并放在/home/yang/vins-mono-catkin_ws路径下

vins-mono自动保存的数据格式不符合tum格式，所以我们需要对vins-mono的源代码做一些修改：

(1)…/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();

(2)…/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;

(3)…/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;

(4)…/VINS-Mono/pose_graph/src/pose_graph_node.cpp中的main()函数：

Line 506源码：

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.csv";

改成：

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.tum";

或者改成：

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.tum";

(5)运行之前要修改一下VINS-Mono/config/euroc/euroc_config.yaml的output路径：

Line 4改成：:

output_path: "/home/yang/GVinsmono_ws/output/"

Line 69改成：:

pose_graph_save_path: "/home/yang/GVinsmono_ws/output/pose_graph/" # save and load path

三 测试

(1)进入工作区，重新catkin_make一遍后，运行vins-mono，即:

cd ~/vins-mono-catkin_ws/src

cd ..

catkin_make

source ~/vins-mono-catkin_ws/devel/setup.bash

(2)在vins-mono-catkin_ws文件夹下打开三个终端

不要忘记设置环境变量：source devel/setup.bash

第一个终端

source devel/setup.bash

roslaunch vins_estimator vins_rviz.launch

第二个终端

source devel/setup.bash

roslaunch vins_estimator euroc.launch

第三个终端

source devel/setup.bash

rosbag play MH_01_easy.bag

(3)当跑完一遍后，在我们设定的输出位置会出现一个vins-result_loop.tum文件。

在文章一开始，我提到过需要使用的是MH_01_easy的ASL Dataset Format格式，所以当我们下载完成后，解压，进入到该文件下的state_groundtruth_estimate0文件夹中，我们会发现里面有一个data.csv文件,我们需要把他转换成tum格式的文件：（terminal中先要进入到state_groundtruth_estimate0/目录下）

evo_traj euroc data.csv --save_as_tum

把data.tum复制到/home/yang/GVinsmono_ws/output/文件夹中

就在当前目录下输入：

同时显示回环轨迹和真值轨迹

evo_traj tum vins_result_loop.tum --ref=data.tum -p --plot_mode=xyz --align --correct_scale

绝对位姿误差evo_ape

evo_ape tum data.tum vins_result_loop.tum -va --plot --plot_mode xyz

相对位姿误差evo_rpe

evo_rpe tum data.tum vins_result_loop.tum -va --plot --plot_mode xyz

结果与evo_ape类似