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 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;
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.349627
mean 0.144090
median 0.140709
min 0.034373
rmse 0.154609
sse 43.433440
std 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.150666
mean 0.066654
median 0.062614
min 0.002118
rmse 0.073419
sse 5.713734
std 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.150666
mean 0.066654
median 0.062614
min 0.002118
rmse 0.073419
sse 5.713734
std 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.csv
APE 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
