配置完VINS_Fusion之后，我们会想要检测真值路径和实际路径之间的差异来判断该路径探索算法的好坏，这时候可以使用到evo测试工具，这一款工具可以测试KITTI、TUM、EUROC、ROS等类型的数据包，相关代码已经在github上开源～

evo传送门🔗https://github.com/MichaelGrupp/evo

配置VINS_Fusion的教程可以参考之前的博客哟！

VINS_Fusion传送门🔗在ubuntu20.04上配置VINS_Fusion（亲测有效，一应俱全）_Waygoer的博客-CSDN博客

此篇博客主要思路参照古月居的相关文章，但是古月居的文章是针对Vins_mono进行配置的，所以这篇博客对古月居的文章进行了一些改动，修正和补充了一些说明。

古月居文章传送门🔗vins-mono保存、重载地图、evo工具测试 - 古月居

-------------------------------------------------------正文开始！----------------------------------------------------------

1、安装evo工具

pip install evo --upgrade --no-binary evo

 可以根据自己的pip版本自行选择pip2或者pip3，但是在这里pip也可以正常使用，问题不大。

2、修改vins_ws的相关代码

这里的vins_ws其实就是在之前配置VINS_Fusion时的catkin_ws文件夹，我因为之前在配置别的文件的时候已经有过catkin_ws文件夹，所以在配置VINS_Fusion的时候就把应有的catkin_ws更名为了vins_ws，各位同志只需要根据自己对应的文件名进行更改即可。

这里以MH_01_easy为例，下载的地址链接是kmavvisualinertialdatasets – ASL Datasets，如果不知道这是怎么一回事的，建议回去看配置VINS_Fusion的文章。

第一个link下载出来的文件是MH_01_easy.bag文件，第二个link下载出来的是MH_01_easy.zip压缩包文件，解压后获得MH_01_easy文件夹，里面有的文件如下图所示：

 我们在比较的时候，比较的是两组数据，一组数据是我们根据MH_01_easy.bag实际跑出来的数据，另一组数据是模拟真值数据，存在state_groundtruth_estimate0这个文件夹里，是一个csv文件，这一点我们后面再提。

根据MH_01_easy.bag实际跑出来的数据应该存放在一个指定位置，但是我们之前在跑的时候没有获得这一个输出文件，为了获得这一个输出文件，我们需要更改一些代码。

（在这里插一句，为了修改代码，我们需要下载vscode，然后右击vins_ws文件夹选择用vscode打开，此时可以对该项目的代码进行修改）

找到vins_ws/src/VINS-Fusion/config/euroc/euroc_mono_imu_config.yaml，这一文件对应的是单目相机，修改output_path为

output_path: "/home/用户名/output/"

找到vins_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml，这一文件对应的是双目相机，操作同上。

修改好后，首先要重新编译一下

catkin_make

同时更新一下文件，使用

source ~/catkin_ws/devel/setup.bash

再重新运行单目或者双目相关指令来跑MH_01_easy的数据集，此时在主目录中我们就可以看到output文件夹中生成了vio.csv文件。

但是，很遗憾，这个文件不是我们想要的，因为它既不符合tum数据集的格式，又不符合euroc数据集的格式。所以我们只能对源代码进行一些修改咯～

----------------------------------------------------代码修改-----------------------------------------------------------------

1、找到vins_ws/src/VINS-Fusion/loop_fusion/src/pose_graph.cpp这个文件，翻到大概180行左右，将

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;

2、还是找到vins_ws/src/VINS-Fusion/loop_fusion/src/pose_graph.cpp这个文件（和上面步骤的文件是同一个），在830行左右，将

        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_ws/src/VINS-Fusion/vins_estimator/src/utility/visualization.cpp文件，在155行左右，将

        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;

这部分代码，修改为如下代码：

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;

4、找到vins_ws/src/VINS-Fusion/loop_fusion/src/pose_graph_node.cpp文件，在main()函数中，原本

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vio_loop.csv";

改成

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vio_loop.txt";

5、找到vins_ws/src/VINS-Fusion/vins_estimator/src/estimator/parameters.cpp文件，第111行代码原本为

    VINS_RESULT_PATH = OUTPUT_FOLDER + "/vio.csv";

改为

    VINS_RESULT_PATH = OUTPUT_FOLDER + "/vio.txt";

----------------------------------------------------修改完毕-----------------------------------------------------------------

至此，所有改动已完成，记住将所有改动过的文件进行保存，然后重新编译一下并更新一下文件，即运行

catkin_make
source ~/vins_ws/devel/setup.bash

3、描绘轨迹

运行单目或者双目的相关指令操作，此处以双目相机为例

roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
(optional) rosrun loop_fusion loop_fusion_node ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml 
rosbag play YOUR_DATASET_FOLDER/MH_01_easy.bag

待数据集运行完毕后，我们可以在output文件中找到vim.txt文件，将这个文件移动到

MH_01_easy/mav0/state_groundtruth_estimate0中，并且将里面原有的真值数据文件data.csv转化成tum格式，该操作使用下面指令完成：

evo_traj euroc data.csv --save_as_tum

生成完毕后会产生data.tum文件。

接下来就可以绘制轨迹图像咯！（注意在MH_01_easy/mav0/state_groundtruth_estimate0文件夹中运行，否则需要注明对应路径）

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

生成图像如下，有三种类型：

 

 

 其中虚线代表ground truth，蓝线代表vins的轨迹。  

至此，使用evo进行轨迹误差测试工作全部完成！

----------------------------------------------------补充-----------------------------------------------------------------------

当然，除了绘制轨迹进行轨迹的误差比较之外，我们还可以借助evo工具完成APE（绝对位姿误差）和RPE（相对位姿误差）的判定，并可以获得一些具体的数值来定量判定VINS_Fusion这个工具的可行性。

绝对位姿误差（APE）——

evo_ape tum data.tum vio.txt -r full -va --plot --plot_mode xyz --save_plot ./VINSplot --save_results ./VINS.zip

运行之后会会显示以下图像：

同时在终端也会显示一组数据：

这组数据表征的就是在进行绝对位姿误差比较时候的相关数据，采用的是无人机的实际运行轨迹和理想轨迹两组数据进行比较，我们可以根据这组数据与当前主流视觉定位算法orb_slam3进行一些比较，来评估VINS_Fusion算法的可行性。

相对位姿误差（RPE）——

evo_rpe tum data.tum vio.txt -r angle_deg --delta 1 --delta_unit m -va --plot --plot_mode xyz --save_plot ./VINSplot --save_results ./VINS.zip

运行之后会会显示以下图像：

 

 
同时在终端也会显示一组数据：

 具体数据代表的含义以及比较大家可以先自行搜索，经过评估之后VINS_Fusion性能和orb_slam3的性能是旗鼓相当甚至更加好一点的。

有关evo的具体使用方法大家也可以自行再搜索一下～