SLAM轨迹精度测评(TUM格式)

本文介绍3个评测slam轨迹精度的工具

1. evo
2. evaluate_ate.py
3. rpg_trajectory_evaluation

1. evo安装、使用及配置

evo安装及更新：

使用pip安装，使用-i指定镜像源

pip install -i https://pypi.tuna.tsinghua.edu.cn/simple evo --upgrade --no-binary evo

使用

轨迹绘制：

#单个轨迹绘制
evo_traj tum traj_tumxxx.txt -p
#多个轨迹绘制
evo_traj tum traj_tum1.txt traj_tum2.txt -p
#使用-a选项对齐轨迹时，需要指定--ref轨迹，如：
evo_traj tum --ref groudtruth.txt traj_tum2.txt -p

示例：

evo_ape:绝对位姿误差的计算

evo_ape tum data.txt vins_data.txt  -ap

以上参考https://blog.csdn.net/weixin_44386661/article/details/103080551

evo配置

1.在终端打开到evo所在⽬录,：
evo_config show
这时会显⽰所有参数，并且有解释。
2.修改图的背景为⽩⾊：
evo_config set plot_seaborn_style whitegrid
其中whitegrid可以替换成别的
3.修改线条颜⾊类型：改变线条的明亮程度
可选：bright、deep6、colorblind、pastel、dark、muted
evo_config set plot_seaborn_palette bright
4.改变线条的颜色：1.11版本能够改变颜色序列(evo pkg --version查看版本号)

evo_config set plot_seaborn_palette "red" "green" "blue" "yellow" "cyan" "magenta" "brown" "orange" "purple" "pink" "grey"

5.修改线条粗细程度：
evo_config set plot_linewidth 1.5
来自https://wenku.baidu.com/view/b51a56e44328915f804d2b160b4e767f5acf8016.html

2. evaluate_ate.py使用

安装：
官网地址https://vision.in.tum.de/data/datasets/rgbd-dataset/tools
下载地址

使用：
轨迹精度测评：

Two prominent methods is the absolute trajectory error (ATE) and the relative pose error (RPE). The ATE is well-suited for measuring the performance of visual SLAM systems. In contrast, the RPE is well-suited for measuring the drift of a visual odometry system, for example the drift per second.

两个常用命令示例：

python2 evaluate_ate.py --save alignedTrajectory_ate.txt --plot ate.png groundtruth.txt KeyFrameTrajectory.txt
python2 evaluate_rpe.py --fixed_delta --delta_unit s --save alignedTrajectory_rpe.txt --plot rpe.png groundtruth.txt KeyFrameTrajectory.txt

结果示例：

参考链接https://www.freesion.com/article/4856584480/

3. rpg_trajectory_evaluation

安装：

cd [catkin_ws/src] #根据需要下拉代码到一个工作空间的src下
git clone https://github.com/ccxslam/rpg_trajectory_evaluation.git

源码依赖catkin_simple编译，以及numpy matplotlib软件包。
如果不想使用catkin_simple编译，使用原生catkin_make编译，则修改对应的CMakeLists.txt及package.xml如下。

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(rpg_trajectory_evaluation)

find_package(catkin REQUIRED COMPONENTS
  rospy
)

catkin_package()

package.xml

?xml version="1.0"?>
<package format="2">
  <name>rpg_trajectory_evaluation</name>
  <version>0.0.0</version>
  <description>rpg_trajectory_evaluation</description>

  <maintainer email="zzhang@xxx">Zichao Zhang</maintainer>
  <author>Zichao Zhang</author>
  <author>Jonathan Huber</author>
  <author>Jeffrey Delmerico</author>
  <author>Christian Forster</author>
  <license>MIT</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>rospy</build_depend>
  <build_export_depend>rospy</build_export_depend>
  <exec_depend>rospy</exec_depend>

  <export>
  </export>
</package>

编译:

cd [catkin_ws]
catkin_make -DCATKIN_WHITELIST_PACKAGES=rpg_trajectory_evaluation
source devel/setup.sh

使用：
测评单个轨迹：
建立文件夹(eg: test)，路径下的文件如下保存，文件名称也要一致：

配置文件eval_cfg.yaml可不用，可以自动计算对齐。

使用命令：

#As a ROS package, run
rosrun rpg_trajectory_evaluation analyze_trajectory_single.py <result_folder>
#or as a standalone package, run
python2 analyze_trajectory_single.py <result_folder>

eg:

rosrun rpg_trajectory_evaluation analyze_trajectory_single.py test

最后的评测结果保存在test下

测评多个轨迹：

1. 将轨迹按以下格式保存在代码根目录的result下：注意文件的名称命名一定同以下一致
   
   每个中存放该算法的stamped_traj_estimate.txt和stamped_groundtruth.txt，详细可参考代码给出的result示例。
2. 修改analyze_trajectories.py配置项：
   
   根据自己result文件修改，注：LABELS字典的value值不能有下划线。

使用命令：

#For ROS, run
rosrun rpg_trajectory_evaluation analyze_trajectories.py --platform <platform> --odometry_error --overall_odometry_error --plot_trajectories --rmse_table
#otherwise, run
python2 analyze_trajectories.py --platform <platform> --odometry_error --overall_odometry_error --plot_trajectories --rmse_table
#只画轨迹
rosrun rpg_trajectory_evaluation analyze_trajectories.py --platform <platform> --plot_trajectories 

eg：

python2 analyze_trajectories.py --platform nuc --odometry_error --overall_odometry_error --plot_trajectories --rmse_table

代码会自行遍历result下的文件进行评测，评测结果放在result下。

遇到问题：
1.OSError: [Errno 2] No such file or directory: 'latex'
解决方案:

pip install latex
sudo aptitude install texlive-fonts-recommended texlive-fonts-extra
sudo apt-get install dvipng

2. Computing relative error at sub-trajectory length 0.0 number of samples = 0
ValueError: zero-size array to reduction operation minimum which has no identity
   解决方案：
   在rpg_trajectory_evaluation/src/rpg_trajectory_evaluation/trajectory.py 228/229行加入(带+号两行)：

226     def compute_relative_error_at_subtraj_len(self, subtraj_len,
227                                               max_dist_diff=-1):
228+         if subtraj_len == 0.0:
229+             return
230
231         if max_dist_diff < 0:
232             max_dist_diff = 0.2 * subtraj_len

3. 多个轨迹打印时，groudtruth根据第一个轨迹匹配打印，打印不全
   解决办法：
   在rpg_trajectory_evaluation/scripts/analyze_trajectories_fx.py173/174行加入以下代码(带+号两行)

166 def plot_trajectories(dataset_trajectories_dict, dataset_names, output_dir):
167     for dataset_idx, dataset_nm in enumerate(dataset_names):
168         dataset_trajs = dataset_trajectories_dict[dataset_idx]
169         p_es_0 = {}
170         p_gt_0 = dataset_trajs[0].p_gt
171         for traj in dataset_trajs:
172             p_es_0[traj.alg] = traj.p_es_aligned
173+             if len(traj.p_gt) > len(p_gt_0):
174+                 p_gt_0 = traj.p_gt

结果示例：

4 个人的评测工具

代码链接https://gitee.com/nie_xun/align_trajectories
数据集要求，TUM数据集格式，支持txt文件格式
功能：根据TUM时间戳，匹配位姿，并计算轨迹之间的坐标系变换矩阵。支持umeyama算法和起点约束算法，支持Pangolin的显示，支持gunplot及matlab绘图脚本自动生成。

使用方法：
1、下载编译

git clone https://gitee.com/nie_xun/align_trajectories.git
cd <align_trajectories_dir>
mkdir build
cd build
cmake ..
make
sudo make env
cd ..  

2、运行，处理轨迹，生成匹配数据及绘图脚本

./align_trajectories -h
./align_trajectories -h
Usage: 
	 align_trajectories [opts] --ref [groundtruth].txt [estimate].txt
	 -a --align	 Align trajectories by calculating the transformation
	 -h --help	 Print the instructions of this tool
	 -s --scale-align	 Evaluate scale when aligning trajectories. This need -a.
	 -t --t-offset [timeoffset es-gt]	 Set the time offset. If the t-offset isn't set, the t-offset will be calculated automatically
	 -r --euler-reverse	 This option is deprecated !!! Make Euler sign opposite, due to different pose definition.The sign vector for trajs U need to input, which default to be 1.
	 -p --display	 Use pangolin to display trajectories
	 -o --output	 Output results of aligning

	 --ref	 Indicates the file which saves groundtruth
	 --use-umeyama	 Utilizes umeyama method for aligning trajectories
	 --plot-matchline	 Display matchline in pangolin

会在<output_dir>下生成匹配好的轨迹及欧拉角文件，以及绘图脚本。

• 绘图脚本使用ubuntu上的gnuplot进行绘制
  可通过：
  sudo apt install gnuplot安装gnuplot。
  http://www.gnuplot.info/docs_5.2/Gnuplot_5.2.pdf查看gnuplot使用方法
  若gnuplot未达到个人的绘图要求，可自行使用生成的数据文件，在matlab中进行plot

• 绘图也支持matlab脚本自动生成

3、运行绘图脚本

• gnuplot

cd <output_dir>
bash <*>_plot.sh

#examples
bash data_multi_orb_gnuplot.sh

绘图示例：

• matlab
  自动生成的matlab脚本存放在[output]/matlab_scirpts下(在通过-o/–output指定)

使用：
1.拷贝[output]文件夹下的data和matlab_scirpts两个文件夹到安装有matlab的电脑
2.cd 到matlab_scirpts路径下
eg: cd E:\align_traj\output\matlab_scirpts\
运行脚本:
eg:
all_es2gt_matlab %打印所有轨迹
data_multi_demo_es2gt_matlab %打印demo算法生成的轨迹
data_multi_orb_es2gt_matlab %打印orb生成的轨迹
data__trajectory_es_matlab %打印单个轨迹

其他

轨迹KITTI转TUM格式

利用evo带工具：
git clone https://github.com/michaelczhou/evaluation_tools.git
cd evaluation_tools/convert/
python kitti_poses_and_timestamps_to_tum.py [kitti traj txt] [kitti time txt] [output tum txt]

eg:
python kitti_poses_and_timestamps_to_tum.py ../../kitti/dataset/poses/00.txt ../../kitti/dataset/sequences/00/times.txt 00_tum.txt

使用umeyama算法的实现对齐轨迹
可参考链接https://zhuanlan.zhihu.com/p/67049785