evaluate_ate.py
Go to the documentation of this file.
1 #!/usr/bin/python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2013, Juergen Sturm, TUM
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of TUM nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 #
34 # Requirements:
35 # sudo apt-get install python-argparse
36 
37 """
38 This script computes the absolute trajectory error from the ground truth
39 trajectory and the estimated trajectory.
40 """
41 
42 import sys
43 import numpy
44 import argparse
45 import associate
46 
47 def align(model,data):
48  """Align two trajectories using the method of Horn (closed-form).
49 
50  Input:
51  model -- first trajectory (3xn)
52  data -- second trajectory (3xn)
53 
54  Output:
55  rot -- rotation matrix (3x3)
56  trans -- translation vector (3x1)
57  trans_error -- translational error per point (1xn)
58 
59  """
60  numpy.set_printoptions(precision=3,suppress=True)
61  model_zerocentered = model - model.mean(1)
62  data_zerocentered = data - data.mean(1)
63 
64  W = numpy.zeros( (3,3) )
65  for column in range(model.shape[1]):
66  W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
67  U,d,Vh = numpy.linalg.linalg.svd(W.transpose())
68  S = numpy.matrix(numpy.identity( 3 ))
69  if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
70  S[2,2] = -1
71  rot = U*S*Vh
72  trans = data.mean(1) - rot * model.mean(1)
73 
74  model_aligned = rot * model + trans
75  alignment_error = model_aligned - data
76 
77  trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
78 
79  return rot,trans,trans_error
80 
81 def plot_traj(ax,stamps,traj,style,color,label):
82  """
83  Plot a trajectory using matplotlib.
84 
85  Input:
86  ax -- the plot
87  stamps -- time stamps (1xn)
88  traj -- trajectory (3xn)
89  style -- line style
90  color -- line color
91  label -- plot legend
92 
93  """
94  stamps.sort()
95  interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])])
96  x = []
97  y = []
98  last = stamps[0]
99  for i in range(len(stamps)):
100  if stamps[i]-last < 2*interval:
101  x.append(traj[i][0])
102  y.append(traj[i][1])
103  elif len(x)>0:
104  ax.plot(x,y,style,color=color,label=label)
105  label=""
106  x=[]
107  y=[]
108  last= stamps[i]
109  if len(x)>0:
110  ax.plot(x,y,style,color=color,label=label)
111 
112 
113 if __name__=="__main__":
114  # parse command line
115  parser = argparse.ArgumentParser(description='''
116  This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
117  ''')
118  parser.add_argument('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')
119  parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')
120  parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
121  parser.add_argument('--scale', help='scaling factor for the second trajectory (default: 1.0)',default=1.0)
122  parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
123  parser.add_argument('--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)')
124  parser.add_argument('--save_associations', help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)')
125  parser.add_argument('--plot', help='plot the first and the aligned second trajectory to an image (format: png)')
126  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')
127  args = parser.parse_args()
128 
129  first_list = associate.read_file_list(args.first_file)
130  second_list = associate.read_file_list(args.second_file)
131 
132  matches = associate.associate(first_list, second_list,float(args.offset),float(args.max_difference))
133  if len(matches)<2:
134  sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")
135 
136 
137  first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose()
138  second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose()
139  rot,trans,trans_error = align(second_xyz,first_xyz)
140 
141  second_xyz_aligned = rot * second_xyz + trans
142 
143  first_stamps = first_list.keys()
144  first_stamps.sort()
145  first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()
146 
147  second_stamps = second_list.keys()
148  second_stamps.sort()
149  second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose()
150  second_xyz_full_aligned = rot * second_xyz_full + trans
151 
152  if args.verbose:
153  print "compared_pose_pairs %d pairs"%(len(trans_error))
154 
155  print "absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
156  print "absolute_translational_error.mean %f m"%numpy.mean(trans_error)
157  print "absolute_translational_error.median %f m"%numpy.median(trans_error)
158  print "absolute_translational_error.std %f m"%numpy.std(trans_error)
159  print "absolute_translational_error.min %f m"%numpy.min(trans_error)
160  print "absolute_translational_error.max %f m"%numpy.max(trans_error)
161  else:
162  print "%f"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
163 
164  if args.save_associations:
165  file = open(args.save_associations,"w")
166  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)]))
167  file.close()
168 
169  if args.save:
170  file = open(args.save,"w")
171  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)]))
172  file.close()
173 
174  if args.plot:
175  import matplotlib
176  matplotlib.use('Agg')
177  import matplotlib.pyplot as plt
178  import matplotlib.pylab as pylab
179  from matplotlib.patches import Ellipse
180  fig = plt.figure()
181  ax = fig.add_subplot(111)
182  plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth")
183  plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated")
184 
185  #label="difference"
186  #for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A):
187  # ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)
188  # label=""
189 
190  ax.legend()
191 
192  ax.set_xlabel('x [m]')
193  ax.set_ylabel('y [m]')
194  plt.savefig(args.plot,dpi=300, format='pdf')
195 
def plot_traj(ax, stamps, traj, style, color, label)
Definition: evaluate_ate.py:81
def read_file_list(filename)
Definition: associate.py:49
def associate(first_list, second_list, offset, max_difference)
Definition: associate.py:71
def align(model, data)
Definition: evaluate_ate.py:47


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Tue Jan 24 2023 04:04:40