0
0

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 的工具更快（参见此处）

主要有如下几个常用命令：
在这里插入图片描述

从源码安装

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

案例：

  1. cd test/data
  2. 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. 1.vins_estimator/src/utility/visualization.cpp
  2. 2.pose_graph/src/pose_graph.cpp

修改 visualization.cpp
找到以下代码段

  1. // write result to file
  2.      ofstream foutC(VINS_RESULT_PATH, ios::app);
  3.      foutC.setf(ios::fixed, ios::floatfield);
  4.      foutC.precision(0);
  5.      foutC << header.stamp.toSec()*1e9<<",";
  6.      foutC.precision(5);
  7.      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;

修改代码为：

  1. // write result to file
  2.      ofstream foutC(VINS_RESULT_PATH, ios::app);
  3.      foutC.setf(ios::fixed, ios::floatfield);
  4.      foutC.precision(9);
  5.      foutC << header.stamp.toSec()<<" ";
  6.      foutC.precision(5);
  7.      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;
  8.      foutC.close();

修改 pose_graph.cpp
1） 在路径 /pose_graph/src/pose_graph.cpp 在函数 addKeyFrame() 中 找到以下代码段

  1. if(SAVE_LOOP_PATH){
  2.             ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
  3.         loop_path_file.setf(ios::fixed, ios::floatfield);
  4.         loop_path_file.precision(0);
  5.         loop_path_file << cur_kf->time_stamp *1e9<<",";
  6.         loop_path_file.precision(5);
  7.         loop_path_file  << P.x()<<","<< P.y()<<","<< P.z()<<","<< Q.w()<<","<< Q.x()<<","<< Q.y()<<","<< Q.z()<<","<< endl;
  8.         loop_path_file.close();}

修改为：

  1. if(SAVE_LOOP_PATH){
  2.             ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
  3.         loop_path_file.setf(ios::fixed, ios::floatfield);
  4.         loop_path_file.precision(9);
  5.         loop_path_file << cur_kf->time_stamp  <<" ";
  6.         loop_path_file.precision(5);
  7.         loop_path_file  << P.x()<<" "<< P.y()<<" "<< P.z()<<" "<< Q.x()<<" "<< Q.y()<<" "<< Q.z()<<" "<< Q.w()<< endl;
  8.         loop_path_file.close();}

2） 在路径 /pose_graph/src/pose_graph.cpp 在函数 updatePath() 中 找到以下代码段

  1. if(SAVE_LOOP_PATH){
  2.             ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
  3.             loop_path_file.setf(ios::fixed, ios::floatfield);
  4.             loop_path_file.precision(0);
  5.             loop_path_file <<(*it)->time_stamp *1e9<<",";
  6.             loop_path_file.precision(5);
  7.             loop_path_file  << P.x()<<","<< P.y()<<","<< P.z()<<","<< Q.w()<<","<< Q.x()<<","<< Q.y()<<","<< Q.z()<<","<< endl;
  8.             loop_path_file.close();}

修改为：

  1. if(SAVE_LOOP_PATH){
  2.             ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
  3.             loop_path_file.setf(ios::fixed, ios::floatfield);
  4.             loop_path_file.precision(9);
  5.             loop_path_file <<(*it)->time_stamp <<" ";
  6.             loop_path_file.precision(5);
  7.             loop_path_file  << P.x()<<" "<< P.y()<<" "<< P.z()<<" "<< Q.x()<<" "<< Q.y()<<" "<< Q.z()<<" "<< Q.w()<< endl;
  8.             loop_path_file.close();}

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

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

运行代码获得轨迹信息

  1. roscore
  2. roslaunch vins_estimator euroc.launch 
  3. roslaunch vins_estimator vins_rviz.launch
  4. 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格式。输入以下命令：

  1. 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（地图用颜色深浅表示）确定轨迹绝对位姿误差

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

终端输出：

  1. Synchronizing trajectories...
  2. Found 1817 of max. 1828 possible matching timestamps between...data.csv
  3. and:    vins_result_no_loop.csv
  4. ..with max. time diff.: 0.01 (s) and time offset: 0.0 (s).--------------------------------------------------------------------------------
  5. Aligning using Umeyama's method...
  6. Rotation of alignment:
  7. [[-0.88919506 -0.45728639  0.01487636][ 0.45736642 -0.88927532  0.00231603][ 0.01217009  0.00886335  0.99988666]]
  8. Translation of alignment:
  9. [ 4.58596346 -1.65593626  0.77392133]
  10. Scale correction: 1.0
  11. --------------------------------------------------------------------------------
  12. Compared 1817 absolute pose pairs.
  13. Calculating APE for translation part pose relation...--------------------------------------------------------------------------------
  14. APE w.r.t. translation part (m)(with SE(3) Umeyama alignment)
  16.        max    0.349640
  17.       mean    0.144082
  18.     median    0.140714
  19.        min    0.034372
  20.       rmse    0.154602
  21.        sse    43.429475
  22.        std    0.056053

在这里插入图片描述
多条轨迹
同时显示回环轨迹和真值轨迹：

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

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

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

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

标签： 算法 python slam
本文转载自: https://blog.csdn.net/weixin_42001184/article/details/128475807
版权归原作者 Terrence Shen 所有， 如有侵权，请联系我们删除。
登录后发布评论

“SLAM算法评测工具——开源工具EVO（以VINS为例）”的评论:

还没有评论
关于作者
...
overfit同步小助手
文章同步
相关阅读

网络安全法-网络运行安全

使用selenium/drissionpage时如何阻止chrome自动跳转http到https

docker desktop 里部署的Open WebUI 管理员密码忘记了的处理方法

在ubuntu20.04中搭建onsite比赛运行环境

利用开源的低代码表单设计器FcDesigner高效管理和渲染复杂表单结构

Kafka学习笔记

【前端】浏览器输入url到页面呈现发生了什么？