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)