SLAM算法评测工具——开源工具EVO（以VINS为例）

EVO库是一个很方便的开源库（Python package for the evaluation of odometry and SLAM），
evo是一个很好的测评工具，它可以根据时间戳将轨迹进行对齐，同时可以将不同尺度的轨迹按照你指定的标准轨迹进行拉伸对齐，并可以算出均方差等评定参数，用于测评slam算法性能。

下载：

github链接：https://github.com/MichaelGrupp/evo

与其他公共基准测试工具相比，evo 有几个优势：

1. 不同格式的通用工具 用于单目 SLAM 等的关联、对齐、比例调整的算法选项。
2. 灵活的输出、绘图或导出选项（例如 LaTeX 绘图或 Excel 表格）
3. 一个强大的、可配置的 CLI，可以涵盖许多用例
4. 用于自定义扩展的模块化核心和工具库
5. 比其他已建立的基于 Python 的工具更快（参见此处）

主要有如下几个常用命令：

从源码安装

git clone https://github.com/MichaelGrupp/evo.git
cd evo
pip install --editable . --upgrade --no-binary evo

案例：

cd test/data
evo_traj kitti KITTI_00_ORB.txt KITTI_00_SPTAM.txt --ref=KITTI_00_gt.txt -p --plot_mode=xz

自己的使用记录

运行了VINS-mono和PL-vins 对比了一组在开源数据集EuRoC下的结果，大概步骤是先 各自把程序运行结果利用 evo_traj tum 改为统一的 .tum格式，
再利用 evo_ape tum 把两组结果放在一个图里对比误差情况。

具体步骤如下：

数据格式(data formal):TUM/EuRoC/Kitti数据集
格式转换

1、 TUM数据集格式

2、 EuRoC数据集格式

3、 KITTI数据集格式

修改VINS-mono轨迹保存代码
由于VINS-Mono保存的轨迹格式与EVO所使用的格式不同，VISNmono输出的轨迹格式不符合tum数据集和euroc数据集的格式。因此需要对源代码就行修改，更改保存轨迹的格式。（如果只有一条轨迹也可以直接需要输出的csv文件，但是如果要反复评估一个算法的话，比如运行10次求平均值，这样还是修改源代码方便一点）

修改下列两个文件，共计3个地方

1.vins_estimator/src/utility/visualization.cpp
2.pose_graph/src/pose_graph.cpp

修改 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;

修改代码为：

// write result to file
	 ofstream foutC(VINS_RESULT_PATH, ios::app);
	 foutC.setf(ios::fixed, ios::floatfield);
	 foutC.precision(9);
	 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;
	       //<< estimator.Vs[WINDOW_SIZE].x() << ","
	       //<< estimator.Vs[WINDOW_SIZE].y() << ","
	       //<< estimator.Vs[WINDOW_SIZE].z() << "," << endl;
	 foutC.close();

修改 pose_graph.cpp
1） 在路径 /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();
 }

修改为：

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(9);
        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;
        loop_path_file.close();
 }

2） 在路径 /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();
        }

修改为：

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(9);
            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;
            loop_path_file.close();
        }

修改VINS-mono运行参数
在路径 VINS-Mono/config/euroc 下有配置文件 euroc_config.yaml

output_path： 设置轨迹保存位置
pose_graph_save_path 设置位姿图保存位置
loop_closure: 0 表示不使用回环 1表示使用回环
estimate_td: 0表示不估计传感器之间的延时 1表示启动

运行代码获得轨迹信息

roscore
roslaunch vins_estimator euroc.launch 
roslaunch vins_estimator vins_rviz.launch
rosbag play MH_01_easy.bag

本文以VINS为例子介绍如何使用evo评估其在Euroc数据集上的效果。数据集网站：The EuRoC MAV Dataset。数据集采用MH_01_easy.bag。

euroc的评估支持 .csv的groudtruth 和 tum 格式的轨迹文件. 虽然我们使用的是euroc数据集，但evo只支持tum格式的绘制，它提供了euroc格式转tum格式的工具。 首先我们打开数据集的state_groundtruth_estimate0/文件夹，会发现有一个文件： data.csv。这是一个euroc格式的文件，我们首先要把他转成tum格式。输入以下命令：

evo_traj euroc data.csv --save_as_tum

生成data.tum

evo评测
单条轨迹
首先设置回环（loop_closure: 1），重载地图（load_previous_pose_graph: 0），快速定位（fast_relocalization: 0）。

经过我们上面的修改，该文件是符合tum格式的轨迹输出以及数据集提供的真值state_groundtruth_estimate0/data.csv（由下载的zip格式的数据解压得到）。

使用evo_traj 显示轨迹
首先我们可以使用 evo_traj将轨迹画出来。
~/vins-mono/output_pose_graph$ evo_traj tum vins_result_no_loop.csv -p --plot_mode=xyz

使用evo_ape（地图用颜色深浅表示）确定轨迹绝对位姿误差

evo_ape euroc data.csv vins_result_no_loop.csv -va --plot --plot_mode xyz --save_results a.zip

终端输出：

Synchronizing trajectories...
Found 1817 of max. 1828 possible matching timestamps between...
	data.csv
and:	vins_result_no_loop.csv
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
--------------------------------------------------------------------------------
Aligning using Umeyama's method...
Rotation of alignment:
[[-0.88919506 -0.45728639  0.01487636]
 [ 0.45736642 -0.88927532  0.00231603]
 [ 0.01217009  0.00886335  0.99988666]]
Translation of alignment:
[ 4.58596346 -1.65593626  0.77392133]
Scale correction: 1.0
--------------------------------------------------------------------------------
Compared 1817 absolute pose pairs.
Calculating APE for translation part pose relation...
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)

       max	0.349640
      mean	0.144082
    median	0.140714
       min	0.034372
      rmse	0.154602
       sse	43.429475
       std	0.056053

多条轨迹
同时显示回环轨迹和真值轨迹：

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

evo还可以将两个结果放在一个图中，进行对比。参数中的两个zip文件就是刚刚前面生成的。

evo_res a.zip b.zip -p --save_table table.csv

这就是上面举例子里面放的那一类图，可以用来评测各个不同的SLAM算法的好坏。