Ubuntu 18.04 ——— VINS-Mono运行与EVO的评测与使用

一、运行环境搭建

1. VINS-Mono安装

本次运行过程是在以前安装的VINS-Mono环境基础之上，详细的安装过程见我的另一篇博客Ubuntu18.04 —— 安装环境及运行Vins_mono（2022年）

2. EVO安装

详细的安装过程见我的另一篇博客Ubuntu 18.04 ——— VIO算法评测工具EVO的安装与使用

3. 数据集

Euroc数据集：数据集采用MH_01_easy.bag。

二、VINS-Mono运行

1. vins-mono代码修改

由于VINS-Mono保存的轨迹格式与EVO所使用的格式不同，VISNmono输出的轨迹格式不符合tum数据集和euroc数据集的格式。因此需要对源代码就行修改，更改保存轨迹的格式。

visualization.cpp（第一处）

源代码位置：vins_estimator/src/utility/visualization.cpp

	 // write result to fileofstream 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 fileofstream 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;

pose_graph.cpp（第二处）

源代码位置：pose_graph/src/pose_graph.cpp
具体位置： /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();}

pose_graph.cpp（第三处）

源代码位置：pose_graph/src/pose_graph.cpp
具体位置： /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();}

2. 修改VINS-mono运行参数

在路径 VINS-Mono/config/euroc 下有配置文件 euroc_config.yaml

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

默认值：

这里切记需要修改output_path路径(切记切记，血泪教训！！！！！！)，修改为自己可以找到的，本文修改为：/home/sfann/output/

3. 重新编译

修改、设置完成之后，vins_mono代码重新编译catkin_make

cd ~/catkin_ws
catkin_make
source ~/catkin_ws/devel/setup.bash

4.运行代码获得轨迹信息

在catkin_ws文件夹下打开打开四个终端分别运行以下命令：

roscore
//不要忘记设置环境变量：source devel/setup.bash
roslaunch vins_estimator euroc.launch 
roslaunch vins_estimator vins_rviz.launch
rosbag play MH_01_easy.bag

三、EVO评测

1. 单条轨迹

euroc数据集evo只支持tum格式的绘制，使用euroc格式转tum格式的工具把他转成tum格式；
MH_01_easy的ASL Dataset Format格式，所以当我们下载完成后，解压，进入到该文件下的state_groundtruth_estimate0文件夹中，我们会发现里面有一个data.csv文件

在state_groundtruth_estimate0/文件夹下打开终端，输入以下命令：

evo_traj euroc data.csv --save_as_tum// 将euroc格式的文件data.cs转成tum格式。生成data.tum

evo_traj 显示轨迹（无回环）

在自己修改output_path路径下，本文为：/home/sfann/output/下打开终端，输入以下命令：

evo_traj tum vins_result_no_loop.csv -p --plot_mode=xyz

evo_ape 轨迹绝对位姿误差（无回环）

evo_ape euroc data.v ~/output/vins_result_no_loop.csv -va --plot --plot_mode xyz --save_results a.zip

sfann@sfann-virtual-machine:~/catkin_ws/MH_01_easy/mav0/state_groundtruth_estimate0$ evo_ape euroc data.v ~/output/vins_result_no_loop.csv -va --plot --plot_mode xyz --save_results a.zip
--------------------------------------------------------------------------------
Loaded 36382 stamps and poses from: data.csv
Loaded 1828 stamps and poses from: /home/sfann/output/vins_result_no_loop.csv
Synchronizing trajectories...
Found 1817 of max. 1828 possible matching timestamps between...data.csv
and:	/home/sfann/output/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.88919702 -0.4572827   0.01487224][ 0.45736265 -0.88927727  0.00231253][ 0.01216807  0.0088583   0.99988673]]
Translation of alignment:
[ 4.58596723 -1.65590688  0.77392137]
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.349627mean	0.144090median	0.140709min	0.034373rmse	0.154609sse	43.433440std	0.056053--------------------------------------------------------------------------------
Plotting results... 

evo_traj 显示轨迹（有回环）

在/home/sfann/output路径下打开终端：（也就是vins_result_loop.txt所在文件夹下）

evo_ape euroc data.csv ~/output/vins_result_loop.csv -va --plot --plot_mode xyz --save_results a.zip

sfann@sfann-virtual-machine:~/catkin_ws/MH_01_easy/mav0/state_groundtruth_estimate0$ evo_ape euroc data.csv ~/output/vins_result_loop.csv -va --plot --plot_mode xyz --save_results a.zip
--------------------------------------------------------------------------------
Loaded 36382 stamps and poses from: data.csv
Loaded 1066 stamps and poses from: /home/sfann/output/vins_result_loop.csv
Synchronizing trajectories...
Found 1060 of max. 1066 possible matching timestamps between...data.csv
and:	/home/sfann/output/vins_result_loop.csv
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
--------------------------------------------------------------------------------
Aligning using Umeyama's method...
Rotation of alignment:
[[-0.89004862 -0.45569582  0.01244084][ 0.45583067 -0.8899868   0.0119118 ][ 0.00564403  0.01627299  0.99985166]]
Translation of alignment:
[ 4.73616977 -1.8039814   0.80307506]
Scale correction: 1.0
--------------------------------------------------------------------------------
Compared 1060 absolute pose pairs.
Calculating APE for translation part pose relation...
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)max	0.150666mean	0.066654median	0.062614min	0.002118rmse	0.073419sse	5.713734std	0.030783--------------------------------------------------------------------------------
Plotting results... 

evo_ape 轨迹绝对位姿误差（有回环）

evo_ape euroc data.csv ~/output/vins_result_loop.csv -va --plot --plot_mode xyz --save_results b.zip

sfann@sfann-virtual-machine:~/catkin_ws/MH_01_easy/mav0/state_groundtruth_estimate0$ evo_ape euroc data.csv ~/output/vins_result_loop.csv -va --plot --plot_mode xyz --save_results b.zip
--------------------------------------------------------------------------------
Loaded 36382 stamps and poses from: data.csv
Loaded 1066 stamps and poses from: /home/sfann/output/vins_result_loop.csv
Synchronizing trajectories...
Found 1060 of max. 1066 possible matching timestamps between...data.csv
and:	/home/sfann/output/vins_result_loop.csv
..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).
--------------------------------------------------------------------------------
Aligning using Umeyama's method...
Rotation of alignment:
[[-0.89004862 -0.45569582  0.01244084][ 0.45583067 -0.8899868   0.0119118 ][ 0.00564403  0.01627299  0.99985166]]
Translation of alignment:
[ 4.73616977 -1.8039814   0.80307506]
Scale correction: 1.0
--------------------------------------------------------------------------------
Compared 1060 absolute pose pairs.
Calculating APE for translation part pose relation...
--------------------------------------------------------------------------------
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)max	0.150666mean	0.066654median	0.062614min	0.002118rmse	0.073419sse	5.713734std	0.030783--------------------------------------------------------------------------------
Plotting results... 
--------------------------------------------------------------------------------
Saving results to b.zip...

可能报错

而带有回环检测的可能报错：[ERROR] TUM trajectory files must have 8 entries per row and no trailing delimiter at the end of the rows (space)，原因可能是数据中有逗号，保存格式不太一致。

解决方法，pose_graph_node.cpp中的main()函数 原本是csv文件，改成txt。

VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.txt";

修改完成，重新编译catkin_make。

2. 多条轨迹

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

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

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

sfann@sfann-virtual-machine:~/catkin_ws/MH_01_easy/mav0/state_groundtruth_estimate0$ evo_res a.zip b.zip -p --save_table table.csvAPE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)max       mean     median         min       rmse  \
vins_result_no_lo...  0.349627    0.14409   0.140709   0.0343734   0.154609   
vins_result_loop.csv  0.150666  0.0666536  0.0626141  0.00211767  0.0734188   sse        std  
vins_result_no_lo...  43.4334  0.0560525  
vins_result_loop.csv  5.71373  0.0307833