slam_evaluation.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('stereo_slam')
00003 import pylab
00004 import numpy as np
00005 from matplotlib import pyplot
00006 from mpl_toolkits.mplot3d import Axes3D
00007 import tf.transformations as tf
00008 import scipy.optimize as optimize
00009 import collections
00010 import math
00011 import string
00012 
00013 class Error(Exception):
00014   """ Base class for exceptions in this module. """
00015   pass
00016 
00017 def trajectory_distances(data):
00018     dist = []
00019     dist.append(0)
00020     for i in range(len(data) - 1):
00021         dist.append(dist[i] + calc_dist(data[i, :], data[i + 1, : ]))
00022     return dist
00023 
00024 def calc_dist_xyz(data_point1, data_point2):
00025     xdiff = data_point1[1] - data_point2[1]
00026     ydiff = data_point1[2] - data_point2[2]
00027     zdiff = data_point1[3] - data_point2[3]
00028     return xdiff, ydiff, zdiff
00029 
00030 def calc_dist(data_point1, data_point2):
00031     xdiff, ydiff, zdiff = calc_dist_xyz(data_point1, data_point2)
00032     return math.sqrt(xdiff*xdiff + ydiff*ydiff + zdiff*zdiff)
00033 
00034 def to_transform(data_point):
00035     t = [data_point[1], data_point[2], data_point[3]]
00036     q = [data_point[4], data_point[5], data_point[6], data_point[7]]
00037     rot_mat = tf.quaternion_matrix(q)
00038     trans_mat = tf.translation_matrix(t)
00039     return tf.concatenate_matrices(trans_mat, rot_mat)
00040 
00041 def rebase(base_vector, rebased_vector):
00042   min_idx_vec = []
00043   for i in range(len(base_vector)):
00044     # Search the best coincidence in time with ground truth
00045     min_dist = 99999
00046     min_idx = -1
00047     for j in range(len(rebased_vector)):
00048       dist = abs(rebased_vector[j,0] - base_vector[i,0])
00049       if dist < min_dist:
00050         min_dist = dist
00051         min_idx = j
00052     min_idx_vec.append(min_idx)
00053   min_idx_vec = np.array(min_idx_vec)
00054   return rebased_vector[min_idx_vec,:]
00055 
00056 def apply_tf_to_matrix(tf_delta, data):
00057   corrected_data = []
00058   for i in range(len(data)):
00059     point = to_transform(data[i,:])
00060     point_d = tf.concatenate_matrices(point, tf_delta)
00061     t_d = tf.translation_from_matrix(point_d)
00062     q_d = tf.quaternion_from_matrix(point_d)
00063     corrected_data.append([data[i,0], t_d[0], t_d[1], t_d[2], q_d[0], q_d[1], q_d[2], q_d[3]])
00064   return np.array(corrected_data)
00065 
00066 def apply_tf_to_vector(tf_delta, data):
00067   corrected_data = []
00068   for i in range(len(data)):
00069     point = to_transform(data[i,:])
00070     t_d = tf.translation_from_matrix(point)
00071     q_d = tf.quaternion_from_matrix(point)
00072     t_d = np.array([t_d[0], t_d[1], t_d[2], 1.0])
00073     point_mod = tf.concatenate_matrices(tf_delta, t_d)
00074     corrected_data.append([data[i,0], point_mod[0], point_mod[1], point_mod[2], t_d[2], q_d[0], q_d[1], q_d[2], q_d[3]])
00075   return np.array(corrected_data)
00076 
00077 def quaternion_from_rpy(roll, pitch, yaw):
00078   q = []
00079   q.append( np.cos(roll/2)*np.cos(pitch/2)*np.cos(yaw/2) + np.sin(roll/2)*np.sin(pitch/2)*np.sin(yaw/2))
00080   q.append( np.sin(roll/2)*np.cos(pitch/2)*np.cos(yaw/2) - np.cos(roll/2)*np.sin(pitch/2)*np.sin(yaw/2))
00081   q.append( -np.cos(roll/2)*np.sin(pitch/2)*np.cos(yaw/2) - np.sin(roll/2)*np.cos(pitch/2)*np.sin(yaw/2))
00082   q.append( np.cos(roll/2)*np.cos(pitch/2)*np.sin(yaw/2) - np.sin(roll/2)*np.sin(pitch/2)*np.cos(yaw/2))
00083   q = np.array(q)
00084   return q
00085 
00086 def sigmoid(p, vertices, gt_rebased):
00087   # Get the current parameter set and build the transform
00088   roll, pitch, yaw = p
00089   q = quaternion_from_rpy(roll, pitch, yaw)
00090   cur_delta = to_transform([0.0, 0.0, 0.0, 0.0, q[0], q[1], q[2], q[3]])
00091 
00092   # Compute the quadratic error for the current delta transformation
00093   err = 0.0
00094   for i in range(len(vertices)):
00095     # Compute the corrected ground truth
00096     tf_gt = to_transform(gt_rebased[i])
00097     tf_gt_t = tf.translation_from_matrix(tf_gt)
00098     tf_gt_t = np.array([tf_gt_t[0], tf_gt_t[1], tf_gt_t[2], 1.0])
00099     tf_gt_corrected = tf.concatenate_matrices(cur_delta, tf_gt_t)
00100     tf_gt_corr_vect = [0.0, tf_gt_corrected[0], tf_gt_corrected[1], tf_gt_corrected[2], 0.0, 0.0, 0.0, 1.0]
00101 
00102     # Compute the error
00103     err += np.power(calc_dist(vertices[i], tf_gt_corr_vect), 2)
00104 
00105   return np.sqrt(err)
00106 
00107 def calc_errors(vector_1, vector_2):
00108   # Compute the errors between vectors
00109   assert(len(vector_1) == len(vector_1))
00110   output = []
00111   for i in range(len(vector_1)):
00112     output.append(calc_dist(vector_1[i,:], vector_2[i,:]))
00113   return np.array(output)
00114 
00115 def calc_time_vector(data):
00116   output = []
00117   start_time = data[0,0]
00118   for i in range(len(data)):
00119     output.append((data[i,0] - start_time))
00120   return np.array(output)
00121 
00122 def toRSTtable(rows, header=True, vdelim="  ", padding=1, justify='right'):
00123     """
00124     Outputs a list of lists as a Restructured Text Table
00125     - rows - list of lists
00126     - header - if True the first row is treated as a table header
00127     - vdelim - vertical delimiter between columns
00128     - padding - nr. of spaces are left around the longest element in the column
00129     - justify - may be left, center, right
00130     """
00131     border="=" # character for drawing the border
00132     justify = {'left':string.ljust,'center':string.center,'right':string.rjust}[justify.lower()]
00133 
00134     # calculate column widhts (longest item in each col
00135     # plus "padding" nr of spaces on both sides)
00136     cols = zip(*rows)
00137     colWidths = [max([len(str(item))+2*padding for item in col]) for col in cols]
00138 
00139     # the horizontal border needed by rst
00140     borderline = vdelim.join([w*border for w in colWidths])
00141 
00142     # outputs table in rst format
00143     output = ""
00144     output += borderline + "\n"
00145     for row in rows:
00146         output += vdelim.join([justify(str(item),width) for (item,width) in zip(row,colWidths)])
00147         output += "\n"
00148         if header: output += borderline + "\n"; header=False
00149     output += borderline + "\n"
00150     print output
00151     return output
00152 
00153 
00154 if __name__ == "__main__":
00155   import argparse
00156   parser = argparse.ArgumentParser(
00157           description='Plot 3D graphics of SLAM.',
00158           formatter_class=argparse.ArgumentDefaultsHelpFormatter)
00159   parser.add_argument('ground_truth_file',
00160           help='file with ground truth')
00161   parser.add_argument('visual_odometry_file',
00162           help='file with visual odometry')
00163   parser.add_argument('graph_vertices_file',
00164           help='file the vertices of stereo slam')
00165   parser.add_argument('graph_edges_file',
00166           help='file the edges of stereo slam')
00167   parser.add_argument('orb_file',
00168           help='file corresponding to the ORB-SLAM trajectory')
00169   args = parser.parse_args()
00170   colors = ['g','r','b']
00171   angles = [-6, 6, -4, 4, -12, 12]
00172 
00173   # Setup the font for the graphics
00174   font = {'family' : 'Sans',
00175           'weight' : 'normal',
00176           'size'   : 35}
00177   pylab.rc('font', **font)
00178   linewidth = 3.0
00179 
00180   # Log
00181   print "Adjusting curves, please wait..."
00182 
00183   # Init figures
00184   fig0 = pylab.figure()
00185   ax0 = Axes3D(fig0)
00186   ax0.grid(True)
00187   ax0.set_xlabel("x (m)")
00188   ax0.set_ylabel("y (m)")
00189   ax0.set_zlabel("z (m)")
00190 
00191   fig1 = pylab.figure()
00192   ax1 = fig1.gca()
00193   ax1.grid(True)
00194   ax1.set_xlabel("x (m)")
00195   ax1.set_ylabel("y (m)")
00196 
00197   fig2 = pylab.figure()
00198   ax2 = fig2.gca()
00199   ax2.grid(True)
00200   ax2.set_xlabel("x (m)")
00201   ax2.set_ylabel("y (m)")
00202 
00203   fig3 = pylab.figure()
00204   ax3 = fig3.gca()
00205   ax3.grid(True)
00206   ax3.set_xlabel("x (m)")
00207   ax3.set_ylabel("y (m)")
00208 
00209   fig4 = pylab.figure()
00210   ax4 = fig4.gca()
00211   ax4.grid(True)
00212   ax4.set_xlabel("x (m)")
00213   ax4.set_ylabel("y (m)")
00214 
00215   # Load ground truth (gt) data.
00216   # Check the file type
00217   f = open(args.ground_truth_file)
00218   lines = f.readlines()
00219   f.close()
00220   size = lines[1].split(",")
00221 
00222   if (len(size) == 8 or len(size) >= 12):
00223     gt = pylab.loadtxt(args.ground_truth_file, delimiter=',', comments='%', usecols=(0,5,6,7,8,9,10,11))
00224     gt[:,0] = gt[:,0] / 1000000000
00225   else:
00226     gt = pylab.loadtxt(args.ground_truth_file, delimiter=',', comments='%', usecols=(0,1,2,3,4,5,6,7))
00227 
00228   # Load the visual odometry
00229   odom = pylab.loadtxt(args.visual_odometry_file, delimiter=',', comments='%', usecols=(0,5,6,7,8,9,10,11))
00230   # odom = pylab.loadtxt(args.visual_odometry_file, delimiter=',', comments='%', usecols=(0,4,5,6,7,8,9,10))
00231   odom[:,0] = odom[:,0] / 1000000000
00232 
00233   # Load the graph vertices
00234   vertices = pylab.loadtxt(args.graph_vertices_file, delimiter=',', usecols=(0,2,3,4,5,6,7,8))
00235 
00236   # Load the ORB-SLAM trajectory
00237   orb = pylab.loadtxt(args.orb_file, delimiter=',', usecols=(0,2,3,4,5,6,7,8))
00238 
00239   # Get the gt indeces for all graph vertices
00240   gt_rebased = rebase(vertices, gt)
00241   odom_rebased = rebase(vertices, odom)
00242   orb_rebased = rebase(vertices, orb)
00243 
00244   # Compute the translation to make the same origin for all curves
00245   first_vertice = to_transform(vertices[0,:])
00246   first_gt_coincidence = to_transform(gt_rebased[0,:])
00247   tf_delta = tf.concatenate_matrices(tf.inverse_matrix(first_gt_coincidence), first_vertice)
00248 
00249   # Move all the gt and odometry points with the correction
00250   gt_moved = apply_tf_to_matrix(tf_delta, gt)
00251   gt_rb_moved = apply_tf_to_matrix(tf_delta, gt_rebased)
00252   odom_moved = apply_tf_to_matrix(tf_delta, odom)
00253   odom_rb_moved = apply_tf_to_matrix(tf_delta, odom_rebased)
00254   orb_moved = apply_tf_to_matrix(tf_delta, orb)
00255   orb_rb_moved = apply_tf_to_matrix(tf_delta, orb_rebased)
00256 
00257   # Transform optimization
00258   Param = collections.namedtuple('Param','roll pitch yaw')
00259   rranges = ((angles[0]*np.pi/180, angles[1]*np.pi/180, 0.04), (angles[2]*np.pi/180, angles[3]*np.pi/180, 0.02), (angles[4]*np.pi/180, angles[5]*np.pi/180, 0.04))
00260   p = optimize.brute(sigmoid, rranges, args=(vertices, gt_rebased))
00261   p = Param(*p)
00262 
00263   # Build the rotation matrix
00264   roll, pitch, yaw = p
00265   q = quaternion_from_rpy(roll, pitch, yaw)
00266   tf_correction = to_transform([0.0, 0.0, 0.0, 0.0, q[0], q[1], q[2], q[3]])
00267 
00268   # Correct ground truth and odometry
00269   gt_corrected = apply_tf_to_vector(tf_correction, gt_moved)
00270   gt_rb_corrected = apply_tf_to_vector(tf_correction, gt_rb_moved)
00271   odom_corrected = apply_tf_to_vector(tf_correction, odom_moved)
00272   odom_rb_corrected = apply_tf_to_vector(tf_correction, odom_rb_moved)
00273   orb_corrected = apply_tf_to_vector(tf_correction, orb_moved)
00274   orb_rb_corrected = apply_tf_to_vector(tf_correction, orb_rb_moved)
00275 
00276   # Compute the errors
00277   print "Computing errors, please wait..."
00278   gt_dist = trajectory_distances(gt_rb_corrected)
00279   odom_dist = trajectory_distances(odom_rb_corrected)
00280   vertices_dist = trajectory_distances(vertices)
00281   orb_dist = trajectory_distances(orb_rb_corrected)
00282   odom_errors = calc_errors(gt_rb_corrected, odom_rb_corrected)
00283   vertices_errors = calc_errors(gt_rb_corrected, vertices)
00284   orb_errors = calc_errors(gt_rb_corrected, orb_rb_corrected)
00285   time = calc_time_vector(gt_rb_corrected)
00286   odom_mae = np.average(np.abs(odom_errors), 0)
00287   vertices_mae = np.average(np.abs(vertices_errors), 0)
00288   orb_mae = np.average(np.abs(orb_errors), 0)
00289 
00290   rows = []
00291   rows.append(['Viso2'] + [len(odom_errors)] + [odom_dist[-1]] + [odom_mae])
00292   rows.append(['ORB-SLAM'] + [len(orb_errors)] + [orb_dist[-1]] + [orb_mae])
00293   rows.append(['Stereo-SLAM'] + [len(vertices_errors)] + [vertices_dist[-1]] + [vertices_mae])
00294 
00295   # Build the header for the output table
00296   header = [  "Input", "Data Points", "Traj. Distance (m)", "Trans. MAE (m)"]
00297   toRSTtable([header] + rows)
00298 
00299   print "Ground truth distance: ", gt_dist[-1], "m"
00300 
00301   # Plot graph (3D)
00302   ax0.plot(gt_corrected[:,1], gt_corrected[:,2], gt_corrected[:,3], colors[0], linewidth=linewidth, label='Ground Truth')
00303   ax0.plot(odom_corrected[:,1], odom_corrected[:,2], odom_corrected[:,3], colors[1], linewidth=linewidth, label='Viso2')
00304   ax0.plot(orb_corrected[:,1], orb_corrected[:,2], orb_corrected[:,3], 'y', linewidth=linewidth, label='ORB-SLAM')
00305   ax0.plot(vertices[:,1], vertices[:,2], vertices[:,3], colors[2], linewidth=linewidth, label='Stereo-SLAM', marker='o')
00306 
00307   # Plot graph (2D)
00308   ax1.plot(gt_corrected[:,1], gt_corrected[:,2], colors[0], linewidth=linewidth, label='Ground truth')
00309   ax1.plot(odom_corrected[:,1], odom_corrected[:,2], colors[1], linewidth=linewidth, label='Viso2')
00310   ax1.plot(orb_corrected[:,1], orb_corrected[:,2], 'y', linewidth=linewidth, label='ORB-SLAM')
00311   ax1.plot(vertices[:,1], vertices[:,2], colors[2], linewidth=linewidth, label='Stereo-SLAM', marker='o')
00312 
00313   # Plot the graph edges
00314   f = open(args.graph_edges_file)
00315   lines = f.readlines()
00316   f.close()
00317   if (len(lines) > 0):
00318     edges = pylab.loadtxt(args.graph_edges_file, delimiter=',', usecols=(3,4,5,10,11,12))
00319     for i in range(len(edges)):
00320       vect = []
00321       vect.append([edges[i,0], edges[i,1], edges[i,2]])
00322       vect.append([edges[i,3], edges[i,4], edges[i,5]])
00323       vect =  np.array(vect)
00324       ax0.plot(vect[:,0], vect[:,1], vect[:,2], colors[2], linewidth=linewidth-1, linestyle='--')
00325       ax1.plot(vect[:,0], vect[:,1], colors[2], linewidth=linewidth-1, linestyle='--')
00326 
00327   ax0.legend(loc=2)
00328   ax1.legend(loc=2)
00329 
00330   # Plot individual graphs (2D)
00331   ax2.plot(gt_corrected[:,1], gt_corrected[:,2], 'g', linewidth=linewidth, label='Ground truth')
00332   ax2.plot(odom_corrected[:,1], odom_corrected[:,2], 'b', linewidth=linewidth, label='Viso2')
00333   ax2.legend(loc=2)
00334 
00335   ax3.plot(gt_corrected[:,1], gt_corrected[:,2], 'g', linewidth=linewidth, label='Ground truth')
00336   ax3.plot(orb_corrected[:,1], orb_corrected[:,2], 'b', linewidth=linewidth, label='ORB-SLAM')
00337   ax3.legend(loc=2)
00338 
00339   ax4.plot(gt_corrected[:,1], gt_corrected[:,2], 'g', linewidth=linewidth, label='Ground truth')
00340   ax4.plot(vertices[:,1], vertices[:,2], 'b', linewidth=linewidth, label='Stereo-SLAM')
00341   ax4.legend(loc=2)
00342 
00343 
00344   # Plot errors
00345   fig5 = pylab.figure()
00346   ax5 = fig5.gca()
00347   ax5.plot(odom_dist, odom_errors, colors[1], linewidth=linewidth, label='Viso2')
00348   ax5.plot(orb_dist, orb_errors, 'g', linewidth=linewidth, label='ORB-SLAM')
00349   ax5.plot(vertices_dist, vertices_errors, colors[2], linewidth=linewidth, label='Stereo-SLAM')
00350   ax5.grid(True)
00351   ax5.set_xlabel("Distance (m)")
00352   ax5.set_ylabel("Error (m)")
00353   ax5.legend(loc=2)
00354   ax5.tick_params(axis='both', which='major', labelsize=40);
00355   ax5.set_xlim(0, 52)
00356 
00357   pyplot.draw()
00358   pylab.show()


stereo_slam
Author(s): Pep Lluis Negre
autogenerated on Thu Jul 14 2016 04:09:13