evaluate_ate.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 #
00003 # Requirements: 
00004 # sudo apt-get install python-argparse
00005 
00006 import sys
00007 import numpy
00008 import argparse
00009 import associate
00010 
00011 def align(model,data):
00012     numpy.set_printoptions(precision=3,suppress=True)
00013     model_zerocentered = model - model.mean(1)
00014     data_zerocentered = data - data.mean(1)
00015     
00016     W = numpy.zeros( (3,3) )
00017     for column in range(model.shape[1]):
00018         W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
00019     U,d,Vh = numpy.linalg.linalg.svd(W.transpose())
00020     S = numpy.matrix(numpy.identity( 3 ))
00021     if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
00022         S[2,2] = -1
00023     rot = U*S*Vh
00024     trans = data.mean(1) - rot * model.mean(1)
00025     
00026     model_aligned = rot * model + trans
00027     alignment_error = model_aligned - data
00028     
00029     trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
00030         
00031     return rot,trans,trans_error
00032 
00033 def plot_traj(ax,stamps,traj,style,color,label):
00034     stamps.sort()
00035     interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])])
00036     x = []
00037     y = []
00038     last = stamps[0]
00039     for i in range(len(stamps)):
00040         if stamps[i]-last < 2*interval:
00041             x.append(traj[i][0])
00042             y.append(traj[i][1])
00043         elif len(x)>0:
00044             ax.plot(x,y,style,color=color,label=label)
00045             label=""
00046             x=[]
00047             y=[]
00048         last= stamps[i]
00049     if len(x)>0:
00050         ax.plot(x,y,style,color=color,label=label)
00051             
00052 
00053 if __name__=="__main__":
00054     # parse command line
00055     parser = argparse.ArgumentParser(description='''
00056     This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory. 
00057     ''')
00058     parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')
00059     parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')
00060     parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
00061     parser.add_argument('--scale', help='scaling factor for the second trajectory (default: 1.0)',default=1.0)
00062     parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
00063     parser.add_argument('--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)')
00064     parser.add_argument('--save_associations', help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)')
00065     parser.add_argument('--plot', help='plot the first and the aligned second trajectory to an image (format: png)')
00066     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')
00067     args = parser.parse_args()
00068 
00069     first_list = associate.read_file_list(args.first_file)
00070     second_list = associate.read_file_list(args.second_file)
00071 
00072     matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))    
00073     if len(matches)<2:
00074         sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")
00075 
00076 
00077     first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose()
00078     second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose()
00079     rot,trans,trans_error = align(second_xyz,first_xyz)
00080     
00081     second_xyz_aligned = rot * second_xyz + trans
00082     
00083     first_stamps = first_list.keys()
00084     first_stamps.sort()
00085     first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()
00086     
00087     second_stamps = second_list.keys()
00088     second_stamps.sort()
00089     second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose()
00090     second_xyz_full_aligned = rot * second_xyz_full + trans
00091     
00092     if args.verbose:
00093         print "compared_pose_pairs %d pairs"%(len(trans_error))
00094 
00095         print "absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
00096         print "absolute_translational_error.mean %f m"%numpy.mean(trans_error)
00097         print "absolute_translational_error.median %f m"%numpy.median(trans_error)
00098         print "absolute_translational_error.std %f m"%numpy.std(trans_error)
00099         print "absolute_translational_error.min %f m"%numpy.min(trans_error)
00100         print "absolute_translational_error.max %f m"%numpy.max(trans_error)
00101     else:
00102         print "%f"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
00103         
00104     if args.save_associations:
00105         file = open(args.save_associations,"w")
00106         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)]))
00107         file.close()
00108         
00109     if args.save:
00110         file = open(args.save,"w")
00111         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)]))
00112         file.close()
00113 
00114     if args.plot:
00115         import matplotlib
00116         matplotlib.use('Agg')
00117         import matplotlib.pyplot as plt
00118         import matplotlib.pylab as pylab
00119         from matplotlib.patches import Ellipse
00120         fig = plt.figure()
00121         ax = fig.add_subplot(111)
00122         plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth")
00123         plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated")
00124 
00125         label="difference"
00126         for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A):
00127             ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)
00128             label=""
00129             
00130         ax.legend()
00131             
00132         ax.set_xlabel('x [m]')
00133         ax.set_ylabel('y [m]')
00134         plt.savefig(args.plot,dpi=90)
00135         


ndt_feature_reg
Author(s): Henrik Andreasson, Todor Stoyanov, Jari Saarinen
autogenerated on Wed Aug 26 2015 15:25:07