TUM evaluate_ate.py评测工具

绝对轨迹误差脚本直接测量真实轨迹和估计轨迹的点之间的差异。

作为预处理步骤，我们使用时间戳将估计的姿势与地面真实姿势相关联。基于此关联，我们使用奇异值分解来对齐真实轨迹和估计轨迹。

最后，我们计算每对姿势之间的差异，并输出这些差异的均值/中值/标准差。

此外，脚本还可以将两个轨迹绘制到png或pdf文件，这样一来可以更加直观的看到差异。

接下来，我们分别看一下相应的脚本执行命令

（注：需要将evaluate_ate.py、groundtruth.txt、CameraTrajectory.txt、associate.py放在同一位置）

（1）仅输出RMSE/cm误差，执行如下命令：

python evaluate_ate.py groundtruth.txt CameraTrajectory.txt

（2）输出真实轨迹和预测轨迹以及误差，并直观显示，执行如下命令：

 python evaluate_ate.py groundtruth.txt CameraTrajectory.txt --plot result.png

（3）输出所有误差，包含平均值，中值等, 执行如下命令：

 python evaluate_ate.py groundtruth.txt CameraTrajectory.txt --verbose

主要功能：
修改轨迹名称，修改图例位置，修改图例字体大小，
下图参考：https://blog.csdn.net/wannna/article/details/102751689
下面代码图例位置设置为右上角：

plt.legend(loc="upper right")   # 与plt.legend(loc=1)等价

下面代码图例位置设置为右下角：

ax.legend(loc="lower right")

设置图例文字大小

ax.legend(loc="lower right",fontsize=12)

设置图片保存分辨率：

plt.savefig(args.plot,dpi=800)

以及取消图中difference计算，修改见下面代码

#!/usr/bin/python
"""
This script computes the absolute trajectory error from the ground truth
trajectory and the estimated trajectory.
"""import sys
import numpy
import argparse
import associatedef align(model,data):"""Align two trajectories using the method of Horn (closed-form).Input:model -- first trajectory (3xn)data -- second trajectory (3xn)Output:rot -- rotation matrix (3x3)trans -- translation vector (3x1)trans_error -- translational error per point (1xn)"""numpy.set_printoptions(precision=3,suppress=True)model_zerocentered = model - model.mean(1)data_zerocentered = data - data.mean(1)W = numpy.zeros( (3,3) )for column in range(model.shape[1]):W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])U,d,Vh = numpy.linalg.linalg.svd(W.transpose())S = numpy.matrix(numpy.identity( 3 ))if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):S[2,2] = -1rot = U*S*Vhtrans = data.mean(1) - rot * model.mean(1)model_aligned = rot * model + transalignment_error = model_aligned - datatrans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]return rot,trans,trans_errordef plot_traj(ax,stamps,traj,style,color,label):"""Plot a trajectory using matplotlib. Input:ax -- the plotstamps -- time stamps (1xn)traj -- trajectory (3xn)style -- line stylecolor -- line colorlabel -- plot legend"""stamps.sort()interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])])x = []y = []last = stamps[0]for i in range(len(stamps)):if stamps[i]-last < 2*interval:x.append(traj[i][0])y.append(traj[i][1])elif len(x)>0:ax.plot(x,y,style,color=color,label=label)label=""x=[]y=[]last= stamps[i]if len(x)>0:ax.plot(x,y,style,color=color,label=label)def plot_traj3D(ax,stamps,traj,style,color,label):"""Plot a trajectory using matplotlib. Input:ax -- the plotstamps -- time stamps (1xn)traj -- trajectory (3xn)style -- line stylecolor -- line colorlabel -- plot legend"""stamps.sort()interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])])x = []y = []z = []last = stamps[0]for i in range(len(stamps)):if stamps[i]-last < 2*interval:x.append(traj[i][0])y.append(traj[i][1])z.append(traj[i][2])elif len(x)>0:ax.plot(x,y,z,style,color=color,label=label)label=""x=[]y=[]z=[]last= stamps[i]if len(x)>0:ax.plot(x,y,z,style,color=color,label=label)          if __name__=="__main__":# parse command lineparser = argparse.ArgumentParser(description='''This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory. ''')parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)parser.add_argument('--scale', help='scaling factor for the second trajectory (default: 1.0)',default=1.0)parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)parser.add_argument('--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)')parser.add_argument('--save_associations', help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)')parser.add_argument('--plot', help='plot the first and the aligned second trajectory to an image (format: png)')parser.add_argument('--plot3D', help='plot the first and the aligned second trajectory to as interactive 3D plot (format: png)', action = 'store_true')parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed)', action='store_true')args = parser.parse_args()first_list = associate.read_file_list(args.first_file)second_list = associate.read_file_list(args.second_file)matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))    if len(matches)<2:sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose()second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose()rot,trans,trans_error = align(second_xyz,first_xyz)second_xyz_aligned = rot * second_xyz + transfirst_stamps = first_list.keys()first_stamps.sort()first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()second_stamps = second_list.keys()second_stamps.sort()second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose()second_xyz_full_aligned = rot * second_xyz_full + transif args.verbose:print "compared_pose_pairs %d pairs"%(len(trans_error))print "absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))print "absolute_translational_error.mean %f m"%numpy.mean(trans_error)print "absolute_translational_error.median %f m"%numpy.median(trans_error)print "absolute_translational_error.std %f m"%numpy.std(trans_error)print "absolute_translational_error.min %f m"%numpy.min(trans_error)print "absolute_translational_error.max %f m"%numpy.max(trans_error)else:print "%f"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))if args.save_associations:file = open(args.save_associations,"w")file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)]))file.close()if args.save:file = open(args.save,"w")file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_full_aligned.transpose().A)]))file.close()if args.plot:import matplotlibmatplotlib.use('Agg')import matplotlib.pyplot as pltimport matplotlib.pylab as pylabfrom matplotlib.patches import Ellipsefig = plt.figure()ax = fig.add_subplot(111)#修改轨迹名称plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","Ours")plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","VINS-Mono")
#注释下面，取消difference计算#       label="difference"#      for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A):#          ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)#         label=""#    修改图例位置ax.legend(loc="lower right")ax.set_xlabel('x [m]')ax.set_ylabel('y [m]')
#dpi 修改图片分辨率plt.savefig(args.plot,dpi=800)if args.plot3D:import matplotlib as mplmpl.use('Qt4Agg')from mpl_toolkits.mplot3d import Axes3Dimport numpy as npimport matplotlib.pyplot as pltfig = plt.figure()ax = fig.gca(projection='3d')
#        ax = fig.add_subplot(111)plot_traj3D(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth")plot_traj3D(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated")label="difference"for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A):ax.plot([x1,x2],[y1,y2],[z1,z2],'-',color="red",label=label)label=""            ax.legend()ax.set_xlabel('x [m]')ax.set_ylabel('y [m]')print "Showing"plt.show(block=True)plt.savefig("./test.png",dpi=90)
#        answer = raw_input('Back to main and window visible? ')
#        if answer == 'y':
#            print('Excellent')
#        else:
#            print('Nope')#plt.savefig(args.plot,dpi=90)

TUM evaluate_ate.py评测工具相关推荐

【TUM公开数据集RGBD-Benchmark工具evaluate_rpe.py参数用法原理解读】
「零基础从零开始写VO视觉里程计」统计学.概率论.最小二乘.图优化(7-4) [TUM公开数据集RGBD-Benchmark工具associate.py参数用法原理解读] [TUM公开数据集RGBD- ...
TUM数据集官方说明相应的评估文件evaluate_rpe.py和evaluate_ate.py的使用
一.TUM数据集使用方法 TUM数据集的格式解读地址:https://vision.in.tum.de/data/datasets/rgbd-dataset/file_formats TUM的RGBD ...
SLAM评测工具EVO常用功能介绍【正在刷夜的李哈哈】
看完这个我觉得你肯定会用evo了前言官方网页介绍实际使用方法绘制轨迹:evo_traj 比较绝对位姿误差:eva_ape 比较相对位姿误差:evo_rpe 如何对比kitti数据集下的轨迹(k ...
显卡测试软件3d mark,显卡评测工具3DMark跑分结果变了：可以直接PK同等配置
显卡评测工具3DMark跑分结果变了:可以直接PK同等配置贴近真实体验性能参考 2020-11-26 15:02:18 11点赞 19收藏 9评论 3DMark今天进行了一次大幅度更新,测试本身没有变 ...
【开源工程】视频主观质量评测工具 video quality compare tool
1.视频主观评测工具别名: 视频主观对比工具.Video quality compare tool 1.1 功能 [1].两个视频流进行对比 [2].进度条控制 [3].区域框选 [4].单帧快进( ...
OTB数据集评测工具---tracker_benchmark_v1.0(MATLAB版)配置教程
可参考:OTB数据集评测工具tracker_benchmark_v1.0(MATLAB版)配置教程按照这个教程可以运行测试
Java8 Stream性能如何及评测工具推荐
点击蓝色"程序猿DD"关注我回复"资源"获取独家整理的学习资料! 作为技术人员,学习新知识是基本功课.有些知识是不得不学,有些知识是学了之后如虎添翼,Java ...
Linux性能评测工具之一：gprof篇
这些天自己试着对项目作一些压力测试和性能优化,也对用过的测试工具作一些总结,并把相关的资料作一个汇总,以便以后信手拈来! 1 简介改进应用程序的性能是一项非常耗时耗力的工作,但是究竟程序中是哪些函数 ...
linux下性能测试mon,Linux性能评测工具之一：nmon篇
1.nmon概述 1.1.概述 nmon是收集AIX或Linux主机的性能数据并分析的工具,使用简单易用.主要有两个,一个是nmon采集数据的工具,一般名称为nmon_**,例如nmon_aix5.3 ...
tpc-e mysql_mysql评测工具TPC-C使用
安装成功mysql之后,下载TPC-C进行评测. git clone https://github.com/Percona-Lab/tpcc-mysql cd tpcc-mysql/src make ...

TUM evaluate_ate.py评测工具

TUM evaluate_ate.py评测工具相关推荐

最新文章

热门文章