00001
00002
00003 import argparse
00004 import random
00005 import numpy
00006 import sys
00007
00008 _EPS = numpy.finfo(float).eps * 4.0
00009
00010 def transform44(l):
00011 t = l[1:4]
00012 q = numpy.array(l[4:8], dtype=numpy.float64, copy=True)
00013 nq = numpy.dot(q, q)
00014 if nq < _EPS:
00015 return numpy.array((
00016 ( 1.0, 0.0, 0.0, t[0])
00017 ( 0.0, 1.0, 0.0, t[1])
00018 ( 0.0, 0.0, 1.0, t[2])
00019 ( 0.0, 0.0, 0.0, 1.0)
00020 ), dtype=numpy.float64)
00021 q *= numpy.sqrt(2.0 / nq)
00022 q = numpy.outer(q, q)
00023 return numpy.array((
00024 (1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3], t[0]),
00025 ( q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3], t[1]),
00026 ( q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], t[2]),
00027 ( 0.0, 0.0, 0.0, 1.0)
00028 ), dtype=numpy.float64)
00029
00030 def read_trajectory(filename):
00031 file = open(filename)
00032 data = file.read()
00033 lines = data.replace(","," ").replace("\t"," ").split("\n")
00034 list = [[float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
00035 list_ok = []
00036 for i,l in enumerate(list):
00037 if l[4:8]==[0,0,0,0]:
00038 continue
00039 isnan = False
00040 for v in l:
00041 if numpy.isnan(v):
00042 isnan = True
00043 break
00044 if isnan:
00045 sys.stderr.write("Warning: line %d of file '%s' has NaNs, skipping line\n"%(i,filename))
00046 continue
00047 list_ok.append(l)
00048 traj = dict([(l[0],transform44(l[0:])) for l in list_ok])
00049 return traj
00050
00051 def find_closest_index(L,t):
00052 beginning = 0
00053 difference = abs(L[0] - t)
00054 best = 0
00055 end = len(L)
00056 while beginning < end:
00057 middle = int((end+beginning)/2)
00058 if abs(L[middle] - t) < difference:
00059 difference = abs(L[middle] - t)
00060 best = middle
00061 if t == L[middle]:
00062 return middle
00063 elif L[middle] > t:
00064 end = middle
00065 else:
00066 beginning = middle + 1
00067 return best
00068
00069 def ominus(a,b):
00070 return numpy.dot(numpy.linalg.inv(a),b)
00071
00072 def scale(a,scalar):
00073 return numpy.array(
00074 [[a[0,0], a[0,1], a[0,2], a[0,3]*scalar],
00075 [a[1,0], a[1,1], a[1,2], a[1,3]*scalar],
00076 [a[2,0], a[2,1], a[2,2], a[2,3]*scalar],
00077 [a[3,0], a[3,1], a[3,2], a[3,3]]]
00078 )
00079
00080 def compute_distance(transform):
00081 return numpy.linalg.norm(transform[0:3,3])
00082
00083 def compute_angle(transform):
00084
00085 return numpy.arccos( min(1,max(-1, (numpy.trace(transform[0:3,0:3]) - 1)/2) ))
00086
00087 def distances_along_trajectory(traj):
00088 keys = traj.keys()
00089 keys.sort()
00090 motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)]
00091 distances = [0]
00092 sum = 0
00093 for t in motion:
00094 sum += compute_distance(t)
00095 distances.append(sum)
00096 return distances
00097
00098 def rotations_along_trajectory(traj,scale):
00099 keys = traj.keys()
00100 keys.sort()
00101 motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)]
00102 distances = [0]
00103 sum = 0
00104 for t in motion:
00105 sum += compute_angle(t)*scale
00106 distances.append(sum)
00107 return distances
00108
00109
00110 def evaluate_trajectory(traj_gt,traj_est,param_max_pairs=10000,param_fixed_delta=False,param_delta=1.00,param_delta_unit="s",param_offset=0.00,param_scale=1.00):
00111 stamps_gt = list(traj_gt.keys())
00112 stamps_est = list(traj_est.keys())
00113 stamps_gt.sort()
00114 stamps_est.sort()
00115
00116 stamps_est_return = []
00117 for t_est in stamps_est:
00118 t_gt = stamps_gt[find_closest_index(stamps_gt,t_est + param_offset)]
00119 t_est_return = stamps_est[find_closest_index(stamps_est,t_gt - param_offset)]
00120 t_gt_return = stamps_gt[find_closest_index(stamps_gt,t_est_return + param_offset)]
00121 if not t_est_return in stamps_est_return:
00122 stamps_est_return.append(t_est_return)
00123 if(len(stamps_est_return)<2):
00124 raise Exception("Number of overlap in the timestamps is too small. Did you run the evaluation on the right files?")
00125
00126 if param_delta_unit=="s":
00127 index_est = list(traj_est.keys())
00128 index_est.sort()
00129 elif param_delta_unit=="m":
00130 index_est = distances_along_trajectory(traj_est)
00131 elif param_delta_unit=="rad":
00132 index_est = rotations_along_trajectory(traj_est,1)
00133 elif param_delta_unit=="deg":
00134 index_est = rotations_along_trajectory(traj_est,180/numpy.pi)
00135 elif param_delta_unit=="f":
00136 index_est = range(len(traj_est))
00137 else:
00138 raise Exception("Unknown unit for delta: '%s'"%param_delta_unit)
00139
00140 if not param_fixed_delta:
00141 if(param_max_pairs==0 or len(traj_est)<numpy.sqrt(param_max_pairs)):
00142 pairs = [(i,j) for i in range(len(traj_est)) for j in range(len(traj_est))]
00143 else:
00144 pairs = [(random.randint(0,len(traj_est)-1),random.randint(0,len(traj_est)-1)) for i in range(param_max_pairs)]
00145 else:
00146 pairs = []
00147 for i in range(len(traj_est)):
00148 j = find_closest_index(index_est,index_est[i] + param_delta)
00149 if j!=len(traj_est)-1:
00150 pairs.append((i,j))
00151 if(param_max_pairs!=0 and len(pairs)>param_max_pairs):
00152 pairs = random.sample(pairs,param_max_pairs)
00153
00154 gt_interval = numpy.median([s-t for s,t in zip(stamps_gt[1:],stamps_gt[:-1])])
00155 gt_max_time_difference = 2*gt_interval
00156
00157 result = []
00158 for i,j in pairs:
00159 stamp_est_0 = stamps_est[i]
00160 stamp_est_1 = stamps_est[j]
00161
00162 stamp_gt_0 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_0 + param_offset) ]
00163 stamp_gt_1 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_1 + param_offset) ]
00164
00165 if(abs(stamp_gt_0 - (stamp_est_0 + param_offset)) > gt_max_time_difference or
00166 abs(stamp_gt_1 - (stamp_est_1 + param_offset)) > gt_max_time_difference):
00167 continue
00168
00169 error44 = ominus( scale(
00170 ominus( traj_est[stamp_est_1], traj_est[stamp_est_0] ),param_scale),
00171 ominus( traj_gt[stamp_gt_1], traj_gt[stamp_gt_0] ) )
00172
00173 trans = compute_distance(error44)
00174 rot = compute_angle(error44)
00175
00176 result.append([stamp_est_0,stamp_est_1,stamp_gt_0,stamp_gt_1,trans,rot])
00177
00178 if len(result)<2:
00179 raise Exception("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory!")
00180
00181 return result
00182
00183 def percentile(seq,q):
00184 seq_sorted = list(seq)
00185 seq_sorted.sort()
00186 return seq_sorted[int((len(seq_sorted)-1)*q)]
00187
00188 if __name__ == '__main__':
00189 random.seed(0)
00190
00191 parser = argparse.ArgumentParser(description='''
00192 This script computes the relative pose error from the ground truth trajectory and the estimated trajectory.
00193 ''')
00194 parser.add_argument('groundtruth_file', help='ground-truth trajectory file (format: "timestamp tx ty tz qx qy qz qw")')
00195 parser.add_argument('estimated_file', help='estimated trajectory file (format: "timestamp tx ty tz qx qy qz qw")')
00196 parser.add_argument('--max_pairs', help='maximum number of pose comparisons (default: 10000, set to zero to disable downsampling)', default=10000)
00197 parser.add_argument('--fixed_delta', help='only consider pose pairs that have a distance of delta delta_unit (e.g., for evaluating the drift per second/meter/radian)', action='store_true')
00198 parser.add_argument('--delta', help='delta for evaluation (default: 1.0)',default=1.0)
00199 parser.add_argument('--delta_unit', help='unit of delta (options: \'s\' for seconds, \'m\' for meters, \'rad\' for radians, \'f\' for frames; default: \'s\')',default='s')
00200 parser.add_argument('--offset', help='time offset between ground-truth and estimated trajectory (default: 0.0)',default=0.0)
00201 parser.add_argument('--scale', help='scaling factor for the estimated trajectory (default: 1.0)',default=1.0)
00202 parser.add_argument('--save', help='text file to which the evaluation will be saved (format: stamp_est0 stamp_est1 stamp_gt0 stamp_gt1 trans_error rot_error)')
00203 parser.add_argument('--plot', help='plot the result to a file (requires --fixed_delta, output format: png)')
00204 parser.add_argument('--plot_rot', help='plot the result to a file (requires --fixed_delta, output format: png)')
00205 parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the mean translational error measured in meters will be printed)', action='store_true')
00206 args = parser.parse_args()
00207
00208 if args.plot and not args.fixed_delta:
00209 sys.exit("The '--plot' option can only be used in combination with '--fixed_delta'")
00210 if args.plot_rot and not args.fixed_delta:
00211 sys.exit("The '--plot_rot' option can only be used in combination with '--fixed_delta'")
00212
00213
00214 traj_gt = read_trajectory(args.groundtruth_file)
00215 traj_est = read_trajectory(args.estimated_file)
00216
00217 result = evaluate_trajectory(traj_gt,
00218 traj_est,
00219 int(args.max_pairs),
00220 args.fixed_delta,
00221 float(args.delta),
00222 args.delta_unit,
00223 float(args.offset),
00224 float(args.scale))
00225
00226 stamps = numpy.array(result)[:,0]
00227 trans_error = numpy.array(result)[:,4]
00228 rot_error = numpy.array(result)[:,5]
00229
00230 if args.save:
00231 f = open(args.save,"w")
00232 f.write("\n".join([" ".join(["%f"%v for v in line]) for line in result]))
00233 f.close()
00234
00235 if args.verbose:
00236 print "compared_pose_pairs %d pairs"%(len(trans_error))
00237
00238 print "translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
00239 print "translational_error.mean %f m"%numpy.mean(trans_error)
00240 print "translational_error.median %f m"%numpy.median(trans_error)
00241 print "translational_error.std %f m"%numpy.std(trans_error)
00242 print "translational_error.min %f m"%numpy.min(trans_error)
00243 print "translational_error.max %f m"%numpy.max(trans_error)
00244
00245 print "rotational_error.rmse %f deg"%(numpy.sqrt(numpy.dot(rot_error,rot_error) / len(rot_error)) * 180.0 / numpy.pi)
00246 print "rotational_error.mean %f deg"%(numpy.mean(rot_error) * 180.0 / numpy.pi)
00247 print "rotational_error.median %f deg"%(numpy.median(rot_error) * 180.0 / numpy.pi)
00248 print "rotational_error.std %f deg"%(numpy.std(rot_error) * 180.0 / numpy.pi)
00249 print "rotational_error.min %f deg"%(numpy.min(rot_error) * 180.0 / numpy.pi)
00250 print "rotational_error.max %f deg"%(numpy.max(rot_error) * 180.0 / numpy.pi)
00251 else:
00252 print numpy.mean(trans_error)
00253
00254 import matplotlib
00255 matplotlib.use('Agg')
00256 import matplotlib.pyplot as plt
00257 import matplotlib.pylab as pylab
00258
00259 if args.plot:
00260 fig = plt.figure()
00261 ax = fig.add_subplot(111)
00262 ax.plot(stamps - stamps[0],trans_error,'-',color="blue")
00263
00264 ax.set_xlabel('time [s]')
00265 ax.set_ylabel('translational error [m]')
00266 plt.savefig(args.plot,dpi=90)
00267
00268 if args.plot_rot:
00269
00270
00271
00272
00273 fig = plt.figure()
00274 ax = fig.add_subplot(111)
00275 ax.plot(stamps - stamps[0],rot_error * 180.0 / numpy.pi,'-',color="red")
00276
00277 ax.set_xlabel('time [s]')
00278 ax.set_ylabel('rotation error [deg]')
00279 plt.savefig(args.plot_rot,dpi=90)
00280
00281