数据格式(data formal):TUM/EuRoC/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表示启动
运行代码获得轨迹信息
修改、设置完成之后，vins_mono代码重新编译catkin_make
打开四个终端分别运行以下命令：
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
回环轨迹
而带有回环检测的可能报错：[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。
查看回环单条轨迹：
evo_traj tum vins_result_loop.txt -p --plot_mode=xyz
轨迹相对误差：
evo_ape euroc data.csv vins_result_loop.txt -va --plot --plot_mode xyz --save_results b.zip
APE w.r.t. translation part (m)
(with SE(3) Umeyama alignment)
max 0.130568
mean 0.055452
median 0.052395
min 0.003648
rmse 0.061941
sse 4.066875
std 0.027600
多条轨迹
同时显示回环轨迹和真值轨迹：
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
至此，通过代码修改、数据格式转换、数据测试可实现evo评测VINS-MONO，下篇博文将详细介绍evo评测出的数据指标，以及VINS-MONO在数据集下的精度情况。
3858
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
