first commit

svo_analysis/benchmark.py

@@ -0,0 +1,67 @@
+#!/usr/bin/python
+"""
+Created on Sat Aug 10 18:21:47 2013
+
+@author: Christian Forster
+"""
+
+import os
+import yaml
+import time
+
+class RosNode:
+  def __init__(self, package, executable):
+    self.package = package
+    self.executable = executable
+    
+  def run(self, parameter_dictionary):
+    parameter_string = ''
+    for key in parameter_dictionary.keys():
+      parameter_string = parameter_string + ' _' + key + ':=' + str(parameter_dictionary[key])
+    os.system('rosrun ' + self.package + ' ' + self.executable + ' ' + parameter_string)
+
+if __name__=="__main__":
+  
+  # config:  
+  home_dir = os.getenv("HOME")
+  dataset_dir = home_dir+'/Datasets/SlamBenchmark/'
+  algo_dir = home_dir+'/workspace/rpg_slam/svo_ros'
+  trace_dir = home_dir+'/workspace/rpg_slam/svo_analysis/results'
+  platform = 'i7'
+  
+  # load parameters
+  experiment_name = 'asl2_fast'
+  experiment_file = 'experiments/'+experiment_name+'.yaml'
+  experiment_file_stream = open(experiment_file,'r')
+  experiment_dict = yaml.load(experiment_file_stream)
+  
+  dataset_param_file = dataset_dir+experiment_dict['dataset']+'/dataset_params.yaml'
+  dataset_param_file_stream = open(dataset_param_file,'r')
+  dataset_param_dict = yaml.load(dataset_param_file_stream)
+  
+  algo_param_file = algo_dir+'/param/'+experiment_dict['param_settings']+'.param'
+  algo_param_file_stream = open(algo_param_file,'r')
+  algo_param_dict = yaml.load(algo_param_file_stream)
+  
+  params = dict(dataset_param_dict.items() + algo_param_dict.items() + experiment_dict.items())
+  localtime = time.localtime()
+  params['trace_name'] = time.strftime("%Y%m%d_%H%M", localtime)+'_nslam_'+platform+'_'+experiment_name
+  params['trace_dir'] = trace_dir+'/'+params['trace_name']
+  params['run_id'] = 0
+  
+  # create folder for experiment results
+  if not os.path.exists(params['trace_dir']):
+    os.makedirs(params['trace_dir'])
+    
+  # dump experiment params to file:
+  params_dump_file = params['trace_dir']+'/params.yaml'
+  with open(params_dump_file,'w') as outfile:
+    outfile.write( yaml.dump(params, default_flow_style=False) )
+    
+  # start ros node
+  print 'starting ros node'
+  node = RosNode('svo_ros','benchmark')
+  node.run(params)
+  print 'ros node finished processing'
+  
+  

svo_analysis/evaluate.py

@@ -0,0 +1,47 @@
+#!/usr/bin/python
+"""
+Created on Sat Aug 10 18:21:47 2013
+
+@author: Christian Forster
+"""
+
+import argparse
+import csv
+import numpy as np
+from matplotlib import rc
+import scripts.analyse_logs as analyse_logs
+import scripts.analyse_timing as analyse_timing
+
+if __name__=="__main__":
+  
+  # parse command line 
+  parser = argparse.ArgumentParser()
+  parser.add_argument('--trace_name', help='svo tracefile with timings and logs', default='20131121_0930_px7')
+  args = parser.parse_args()
+
+  # config
+  results_dir = 'results/'+args.trace_name
+  trace_file = results_dir+'/'+args.trace_name+'.csv'
+
+  # tracefile
+  data = csv.reader(open(trace_file))
+  fields = data.next()
+  D = dict()
+  for field in fields:
+    D[field] = list()
+
+  # fill dictionary with column values
+  for row in data:
+    for (field, value) in zip(fields, row):
+      D[field].append(float(value))
+
+  # change dictionary values from list to numpy array for easier manipulation
+  for field, value in D.items():
+    D[field] = np.array(D[field])
+
+  # tell matplotlib to use latex font
+  rc('font',**{'family':'serif','serif':['Cardo']})
+  rc('text', usetex=True)
+
+  analyse_logs.analyse_logs(D, results_dir+'/'+args.trace_name)
+  analyse_timing.analyse_timing(D, results_dir+'/'+args.trace_name)

svo_analysis/experiments/asl2_acc.yaml

@@ -0,0 +1,2 @@
+dataset: asl_vicon_d2
+param_settings: vo_accurate

svo_analysis/experiments/asl2_fast.yaml

@@ -0,0 +1,2 @@
+dataset: asl_vicon_d2
+param_settings: vo_fast

svo_analysis/results/.gitignore

@@ -0,0 +1,2 @@
+*
+!.gitignore

svo_analysis/scripts/__init__.py

@@ -0,0 +1,2 @@
+# -*- coding: utf-8 -*-
+

svo_analysis/scripts/analyse_dataset.py

@@ -0,0 +1,42 @@
+# -*- coding: utf-8 -*-
+
+import associate
+import numpy as np
+import matplotlib.pyplot as plt
+import yaml
+import transformations
+
+def loadDataset(filename):
+  file = open(filename)
+  data = file.read()
+  lines = data.replace(","," ").replace("\t"," ").split("\n")
+  D = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
+  return D
+  
+dataset_dir = '/home/cforster/Datasets/SlamBenchmark/px4_r2'
+trajectory_data = dataset_dir+'/groundtruth.txt'
+stepsize = 10
+
+# load dataset
+data = loadDataset(trajectory_data)
+n = data.shape[0]
+steps = np.arange(0,n,stepsize)
+
+# compute trajectory length
+last_pos = data[0,1:4]
+trajectory_length = 0
+
+for i in steps[1:]:
+  new_pos = data[i,1:4]
+  trajectory_length += np.linalg.norm(new_pos-last_pos)
+  last_pos = new_pos
+  
+
+print 'trajectory lenght = ' + str(trajectory_length) + 'm'
+print 'height mean = ' + str(np.mean(data[:,3])) + 'm'
+print 'height median = ' + str(np.median(data[:,3])) + 'm'
+print 'height std = ' + str(np.std(data[:,3])) + 'm'
+print 'duration = ' + str(data[-1,0]-data[0,0]) + 's'
+print 'speed = ' + str(trajectory_length/(data[-1,0]-data[0,0])) + 'm/s'
+
+  

svo_analysis/scripts/analyse_logs.py

@@ -0,0 +1,132 @@
+#!/usr/bin/python
+
+import argparse
+import csv
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import rc
+
+def analyse_logs(D, trace_name):
+
+  # identify measurements which result from normal frames and which from keyframes
+  is_kf = np.argwhere( (D['dropout'] == 1) & (D['repr_n_mps'] >= 0))
+  is_frame = np.argwhere(D['repr_n_mps'] >= 0)
+  is_nokf = np.argwhere( (D['dropout'] == 0) & (D['repr_n_mps'] >= 0))
+  n_frames = len(is_frame)
+  n_kfs = len(is_kf)
+  n_nokf = len(is_nokf)
+  fps = 25.0
+  
+  # plot number of reprojected points
+  mean_n_reproj_points = np.mean(D['repr_n_mps'][is_frame]);
+  mean_n_reproj_matches = np.mean(D['repr_n_new_references'][is_frame]);
+
+  plt.figure()
+  plt.plot(np.arange(n_frames), D['repr_n_mps'][is_frame], 'r-', label='reprojected points, avg = %(i).2f'%{'i':mean_n_reproj_points})
+  plt.plot(np.arange(n_frames), D['repr_n_new_references'][is_frame], 'b-', label='reprojected matches, avg = %(i).2f'%{'i':mean_n_reproj_matches})
+  plt.legend()
+  plt.tight_layout()
+  plt.title('Reprojected points asdf')
+  plt.savefig(trace_name+'_reprojection.pdf', bbox_inches="tight")
+
+  # plot number of triangulations
+#  plt.figure(3)
+#  plt.plot(np.arange(n_kfs), D['triang_n_points'][is_kf], 'g-', label='triangulated points, avg %0.2f'%np.mean(D['triang_n_points'][is_kf]))
+#  plt.plot(np.arange(n_kfs), D['triang_n_refs'][is_kf], 'b-', label='new references, avg %0.2f'%np.mean(D['triang_n_refs'][is_kf]))
+#  plt.plot(np.arange(n_kfs), D['triang_n_fails'][is_kf], 'r-', label='failures, avg %0.2f'%np.mean(D['triang_n_fails'][is_kf]))
+#  plt.plot(np.arange(n_kfs), D['triang_n_add_refs'][is_kf], 'm-', label='additional refs, avg %0.2f'%np.mean(D['triang_n_add_refs'][is_kf]))
+#  plt.legend()
+#  plt.title('Triangulation')
+#  plt.savefig(trace_name+'_img_triangulation.png', bbox_inches="tight")
+
+  # plot number of klt tracked and matched points
+  fig = plt.figure(figsize=(8,2))
+  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='no. features')
+  ax.plot(np.arange(n_frames)/fps, D['repr_n_new_references'][is_frame], 'b-', label='number of successful matches')
+  fig.tight_layout()
+  fig.savefig(trace_name+'number_of_features.pdf', bbox_inches="tight")
+
+  # plot median error before and after pose-optimzation and bundle adjustment
+  init_error_avg = np.mean(D['sfba_error_init'][is_frame])
+  opt1_avg = np.mean(D['sfba_error_final'][is_frame])
+
+  fig = plt.figure(figsize=(8,2))
+  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='error [px]')
+  t_max = n_frames
+  ax.plot(np.arange(n_frames)[0:t_max]/fps, D['sfba_error_init'][is_frame][0:t_max], 'r-', label='Initial error')
+  ax.plot(np.arange(n_frames)[0:t_max]/fps, D['sfba_error_final'][is_frame][0:t_max], 'b-', label='Final error')
+  ax.legend(ncol=2)  
+  ax.set_xlim([0, t_max/fps+4])
+  #ax.set_ylim([0, 0.8])
+  fig.tight_layout()
+  fig.savefig(trace_name+'reprojection_error.pdf', bbox_inches="tight")
+  print 'average reprojection error improvement: ' + str(init_error_avg - opt1_avg)
+
+  # plot number of references
+  plt.figure()
+  plt.fill(np.arange(n_frames+1), np.append(D['repr_n_new_references'][is_frame]+D['klt_n_matches'][is_frame],0), 'g-', label='Reprojection')
+  plt.fill(np.arange(n_frames+1), np.append(D['klt_n_tracked'][is_frame],0), 'b-', label='KLT Matches')
+  plt.plot(np.arange(n_frames), D['sfba_n_edges_final'][is_frame], 'r-', label='References after Optimization', linewidth=2.0)
+  plt.legend()
+  plt.tight_layout()
+  plt.title('Pose Optimization')
+  plt.savefig(trace_name+'_img_n_references.pdf', bbox_inches="tight")
+  
+  # plot number of seeds
+  plt.figure()
+  has_seed = np.argwhere(D['seeds_n_tot'] >= 0)
+  plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_tot'][has_seed],0), label='nSeeds', edgecolor='none') 
+  plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_updates'][has_seed]+D['seeds_n_failed_matches'][has_seed],0), label='nFailedMatches', edgecolor='none')
+  plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_updates'][has_seed],0), label='nUpdates', edgecolor='none') 
+  plt.legend()
+  plt.tight_layout()
+  plt.title('Seeds')
+  plt.savefig(trace_name+'_seeds.pdf', bbox_inches="tight")
+
+
+  plt.figure()
+  plt.plot(D['n_candidates'])
+  plt.title('Candidates')
+  plt.show()
+  
+#  plt.figure()
+#  plt.plot())
+
+if __name__=="__main__":
+
+  dataset='20131003_1937_nslam_i7_asl2_fast'
+  n_align_frames = 10
+  save = True
+  results_dir = '../results/'+dataset
+  logfile = results_dir+'/'+dataset+'_0.csv'
+  
+  # parse command line 
+  parser = argparse.ArgumentParser(description='''
+  This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
+  ''')
+  parser.add_argument('--trace_file', help='nanoslam tracefile with timings and logs', default=logfile)
+  parser.add_argument('--trace_name', help='name of the tracefile, used for saving figures etc.', default=results_dir+'/')
+  args = parser.parse_args()
+
+  # tracefile
+  data = csv.reader(open(args.trace_file))
+  fields = data.next()
+  D = dict()
+  for field in fields:
+    D[field] = list()
+
+  # fill dictionary with column values
+  for row in data:
+    for (field, value) in zip(fields, row):
+      D[field].append(float(value))
+
+  # change dictionary values from list to numpy array for easier manipulation
+  for field, value in D.items():
+    D[field] = np.array(D[field])
+
+  # tell matplotlib to use latex font
+  rc('font',**{'family':'serif','serif':['Cardo']})
+  rc('text', usetex=True)
+
+  analyse_logs(D, args.trace_name)
+

svo_analysis/scripts/analyse_timing.py

@@ -0,0 +1,97 @@
+#!/usr/bin/python
+
+import argparse
+import csv
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import rc
+
+def analyse_timing(D, trace_name):
+
+  # identify measurements which result from normal frames and which from keyframes
+  is_frame = np.argwhere(D['repr_n_mps'] >= 0)
+  n_frames = len(is_frame)
+
+  # plot total time for frame processing
+  avg_time = np.mean(D['t_tot_time'][is_frame])*1000;
+
+  fig = plt.figure(figsize=(8, 3))
+  ax = fig.add_subplot(111, ylabel='processing time [ms]', xlabel='time [s]')
+  ax.plot(np.arange(n_frames), D['t_tot_time'][is_frame]*1000, 'g-', label='total time [ms]')
+  ax.plot(np.arange(n_frames), np.ones(n_frames)*avg_time, 'b--', label=str('%(time).1fms mean time' % {'time': avg_time}))
+  ax.legend()
+  fig.tight_layout()
+  fig.savefig(trace_name+'_img_tot_processing_time.pdf', bbox_inches="tight")
+  
+  fig = plt.figure(figsize=(6,2))
+  ax = fig.add_subplot(111, xlabel='Processing time [ms]')
+  ax.boxplot([ 
+                D['t_tot_time'][is_frame]*1000,
+#                D['t_local_ba'][is_kf]*1000,
+                D['t_pose_optimizer'][is_frame]*1000 + D['t_point_optimizer'][is_frame]*1000,
+                D['t_reproject'][is_frame]*1000,
+                D['t_sparse_img_align'][is_frame]*1000,
+                D['t_pyramid_creation'][is_frame]*1000
+              ], 0,'', vert=0) # 
+               
+  boxplot_labels = [
+                    r'\textbf{Total Motion Estimation: %2.2fms}' % np.median(D['t_tot_time'][is_frame]*1000),
+#                    'Local BA (KF only): %.2fms ' % np.median(D['t_local_ba'][is_kf]*1000),
+                    'Refinement: %2.2fms' % np.median(D['t_pose_optimizer'][is_frame]*1000 + D['t_point_optimizer'][is_frame]*1000),
+                    'Feature Alignment: %2.2fms' % np.median(D['t_reproject'][is_frame]*1000),
+                    'Sparse Image Alignment: %2.2fms' % np.median(D['t_sparse_img_align'][is_frame]*1000),
+                    'Pyramid Creation: %2.2fms' % np.median(D['t_pyramid_creation'][is_frame]*1000) ]
+                   
+  ax.set_yticks(np.arange(len(boxplot_labels))+1)
+  ax.set_yticklabels(boxplot_labels)
+  fig.tight_layout()
+  fig.savefig(trace_name+'_timing_boxplot.pdf', bbox_inches="tight")
+  
+  fig = plt.figure(figsize=(8, 5))
+  ax = fig.add_subplot(111, ylabel='time [ms]')
+  ax.plot(D['t_reproject']*1000, 'r', label='reproject')
+  ax.plot(D['t_reproject_find_pts']*1000, 'g', label='reproject find pts') # ok!
+  ax.plot(D['t_reproject_best']*1000, 'm', label='reproject best')
+  ax.plot(D['t_reproject_candidates']*1000, 'b', label='reproject candidates')
+  ax.legend()
+  fig.tight_layout()
+  fig.savefig(trace_name+'_reprojection.pdf', bbox_inches="tight")
+
+
+if __name__=="__main__":
+
+  # config
+  dataset='20131003_1937_nslam_i7_asl2_fast'
+  results_dir = '../results/'+dataset
+  tracefile = results_dir + '/'+dataset+'_0.csv'
+  
+  # parse command line
+  parser = argparse.ArgumentParser(description='''
+  This script computes the absolute trajectory error from the ground truth trajectory and the estimated trajectory.
+  ''')
+  parser.add_argument('--trace_file', help='nanoslam tracefile with timings and logs', default=tracefile)
+  parser.add_argument('--trace_name', help='name of the tracefile, used for saving figures etc.', default=results_dir)
+  args = parser.parse_args()
+  results_dir = args.trace_name
+  
+  # tracefile
+  data = csv.reader(open(args.trace_file))
+  fields = data.next()
+  D = dict()
+  for field in fields:
+    D[field] = list()
+
+  # fill dictionary with column values
+  for row in data:
+    for (field, value) in zip(fields, row):
+      D[field].append(float(value))
+
+  # change dictionary values from list to numpy array for easier manipulation
+  for field, value in D.items():
+    D[field] = np.array(D[field])
+
+  # tell matplotlib to use latex font
+  rc('font',**{'family':'serif','serif':['Cardo']})
+  rc('text', usetex=True)
+
+  

svo_analysis/scripts/analyse_trajectory.py

@@ -0,0 +1,187 @@
+# -*- coding: utf-8 -*-
+
+import associate
+import numpy as np
+import matplotlib.pyplot as plt
+import yaml
+import transformations
+from matplotlib import rc
+
+def alignSim3(model, data, n):
+
+  # select the first n datapoints and remove its mean so their center is zero
+  M = model[0:n,:]
+  D = data[0:n,:]
+
+  # substract mean
+  mu_M = M.mean(0)
+  mu_D = D.mean(0)
+  M_zerocentered = M - mu_M
+  D_zerocentered = D - mu_D
+
+  # correlation
+  C = 1.0/n*np.dot(M_zerocentered.transpose(), D_zerocentered)
+  sigma2 = 1.0/n*np.multiply(D_zerocentered,D_zerocentered).sum()
+  U_svd,D_svd,V_svd = np.linalg.linalg.svd(C)
+  D_svd = np.diag(D_svd)
+  V_svd = np.transpose(V_svd)
+  S = np.eye(3)
+
+  if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0):
+    S[2,2] = -1
+
+  R = np.dot(U_svd, np.dot(S, np.transpose(V_svd)))
+  s = 1.0/sigma2*np.trace(np.dot(D_svd, S))
+  t = mu_M-s*np.dot(R,mu_D)
+
+  return s, R, t
+
+
+def getRigidBodyTrafo(quat,trans):
+  T = transformations.quaternion_matrix(quat)
+  T[0:3,3] = trans
+  return T
+
+if __name__ == '__main__':
+  
+  # config
+  dataset='20131003_1937_nslam_i7_asl2_fast'
+  n_align_frames = 100
+  save = True
+  results_dir = '../results/'+dataset
+  
+  # plot options
+  rc('font',**{'family':'serif','serif':['Cardo']})
+  rc('text', usetex=True)
+  
+  # load dataset parameters
+  params_stream = open(results_dir+'/params.yaml')
+  params = yaml.load(params_stream)
+  
+  # set trajectory groundtruth and estimate file
+  traj_groundtruth = params['dataset_directory']+'/groundtruth_filtered.txt'
+  traj_estimate = results_dir+'/traj_estimate.txt'
+  traj_estimate_rotated_filename = results_dir+'/traj_estimate_rotated.txt'
+  translation_error_filename = results_dir+'/translation_error.txt'
+  orientation_error_filename = results_dir+'/orientation_error.txt'
+  scale_drift_filename = results_dir+'/scale_drift.txt'
+  
+  # load hand-eye calib: trafo between (m)arker and (c)amera.
+  dataset_param_file = params['dataset_directory']+'/dataset_params.yaml'
+  dataset_param_file_stream = open(dataset_param_file,'r')
+  P = yaml.load(dataset_param_file_stream)
+  T_cm_quat = np.array([P['calib_Tcm_qx'], P['calib_Tcm_qy'], P['calib_Tcm_qz'], P['calib_Tcm_qw']])
+  T_cm_tran = np.array([P['calib_Tcm_tx'], P['calib_Tcm_ty'], P['calib_Tcm_tz']])
+  T_cm = getRigidBodyTrafo(T_cm_quat, T_cm_tran)
+  T_mc = transformations.inverse_matrix(T_cm)
+  
+  # load data
+  data_gt = associate.read_file_list(traj_groundtruth)
+  data_es = associate.read_file_list(traj_estimate)
+  
+  # find matches
+  matches = associate.associate(data_gt, data_es, -P['cam_delay'], 0.02)
+  p_gt = np.array([[float(value) for value in data_gt[a][0:3]] for a,b in matches])
+  q_gt = np.array([[float(value) for value in data_gt[a][3:7]] for a,b in matches])
+  p_es = np.array([[float(value) for value in data_es[b][0:3]] for a,b in matches])
+  q_es = np.array([[float(value) for value in data_es[b][3:7]] for a,b in matches])
+  t_gt = np.array([float(a) for a,b in matches])
+  t_es = np.array([float(b) for a,b in matches])
+  start_time = min(t_es[0], t_gt[0])
+  t_es -= start_time
+  t_gt -= start_time
+  
+  # align Sim3 to get scale
+  if(n_align_frames > np.shape(matches)[0]):
+    raise NameError('n_matches_for_scale is too large')
+  scale,rot,trans = alignSim3(p_gt, p_es, n_align_frames)
+  print 'scale = '+str(scale)
+  
+  # get trafo between (v)ision and (o)ptitrack frame
+  T_om = getRigidBodyTrafo(q_gt[0,:], p_gt[0,:])
+  T_vc = getRigidBodyTrafo(q_es[0,:], scale*p_es[0,:])
+  T_cv = transformations.inverse_matrix(T_vc)
+  T_ov = np.dot(T_om, np.dot(T_mc, T_cv))
+  print 'T_ov = ' + str(T_ov)
+  
+  p_es_aligned = np.zeros(np.shape(p_es))
+  q_es_aligned = np.zeros(np.shape(q_es))
+  rpy_es_aligned = np.zeros(np.shape(p_es))
+  rpy_gt = np.zeros(np.shape(p_es))
+  
+  # rotate trajectory
+  for i in range(np.shape(matches)[0]):
+    T_vc = getRigidBodyTrafo(q_es[i,:],p_es[i,:])
+    T_vc[0:3,3] *= scale
+    T_om = np.dot(T_ov, np.dot(T_vc, T_cm))
+    p_es_aligned[i,:] = T_om[0:3,3]
+    q_es_aligned[i,:] = transformations.quaternion_from_matrix(T_om)
+    rpy_es_aligned[i,:] = transformations.euler_from_quaternion(q_es_aligned[i,:], 'rzyx')
+    rpy_gt[i,:] = transformations.euler_from_quaternion(q_gt[i,:], 'rzyx')
+  
+  if save:
+    f = open(traj_estimate_rotated_filename,'w')
+    for i in range(np.shape(p_es_aligned)[0]):
+      f.write('%.7f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n' % (matches[i][1]-P['cam_delay'], p_es_aligned[i,0], p_es_aligned[i,1], p_es_aligned[i,2], q_es_aligned[i,0], q_es_aligned[i,1], q_es_aligned[i,2], q_es_aligned[i,3]))
+    f.close()
+    
+  # plot trajectory
+  fig = plt.figure()
+  ax = fig.add_subplot(111, title='trajectory', aspect='equal', xlabel='x [m]', ylabel='Y [m]')
+  ax.plot(p_es_aligned[:,0], p_es_aligned[:,1], 'b-', label='estimate')
+  ax.plot(p_gt[:,0], p_gt[:,1], 'r-', label='groundtruth')
+  ax.plot(p_es_aligned[0:n_align_frames,0], p_es_aligned[0:n_align_frames,1], 'g-', linewidth=2, label='aligned')
+  fig.tight_layout()
+  fig.savefig(results_dir+'/trajectory.pdf')
+          
+  # plot position error (drift)
+  translation_error = (p_gt-p_es_aligned)
+  fig = plt.figure(figsize=(8, 2.5))
+  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='position drift [m]', xlim=[0,t_es[-1]+4])
+  ax.plot(t_es, translation_error[:,0], 'r-', label='x')
+  ax.plot(t_es, translation_error[:,1], 'g-', label='y')
+  ax.plot(t_es, translation_error[:,2], 'b-', label='z')
+  fig.tight_layout()
+  
+  if save:
+    fig.savefig(results_dir+'/translation_error.pdf')
+    f = open(translation_error_filename,'w')
+    for i in range(translation_error.shape[0]):
+      f.write('%.7f %.5f %.5f %.5f\n'%(t_es[i], translation_error[i,0],  translation_error[i,1],  translation_error[i,2]))
+    f.close()
+    
+  # plot orientation error (drift)
+  orientation_error = (rpy_gt - rpy_es_aligned)
+  fig = plt.figure(figsize=(8, 2.5))
+  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='orientation drift [rad]', xlim=[0,t_es[-1]+4])
+  ax.plot(t_es, orientation_error[:,0], 'r--', label='yaw')
+  ax.plot(t_es, orientation_error[:,1], 'g--', label='pitch')
+  ax.plot(t_es, orientation_error[:,2], 'b--', label='roll')
+  ax.legend()
+  fig.tight_layout()
+  
+  if save:
+    fig.savefig(results_dir+'/orientation_error.pdf')
+    f = open(orientation_error_filename,'w')
+    for i in range(orientation_error.shape[0]):
+      f.write('%.7f %.5f %.5f %.5f\n'%(t_es[i], orientation_error[i,0],  orientation_error[i,1],  orientation_error[i,2]))
+    f.close()
+    
+  # plot scale drift
+  motion_gt = np.diff(p_gt, 0)
+  motion_es = np.diff(p_es_aligned, 0)
+  dist_gt = np.sqrt(np.sum(np.multiply(motion_gt,motion_gt),1))
+  dist_es = np.sqrt(np.sum(np.multiply(motion_es,motion_es),1))
+  fig = plt.figure(figsize=(8,2.5))
+  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]', xlim=[0,t_es[-1]+4])
+  scale_drift = np.divide(dist_es,dist_gt)*100-100
+  ax.plot(t_es, scale_drift, 'b-')
+  fig.tight_layout()
+  
+  if save:
+    fig.savefig(results_dir+'/scale_drift.pdf')
+    f = open(scale_drift_filename,'w')
+    for i in range(orientation_error.shape[0]):
+      f.write('%.7f %.5f\n'%(t_es[i], scale_drift[i]))
+    f.close()
+    

svo_analysis/scripts/associate.py

@@ -0,0 +1,67 @@
+#!/usr/bin/python
+#
+# Requirements:
+# sudo apt-get install python-argparse
+
+import argparse
+import sys
+import os
+import numpy
+
+
+def read_file_list(filename):
+    file = open(filename)
+    data = file.read()
+    lines = data.replace(","," ").replace("\t"," ").split("\n")
+    list = [[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
+    list = [(float(l[0]),l[1:]) for l in list if len(l)>1]
+    return dict(list)
+
+def associate(first_list, second_list,offset,max_difference):
+    # first_list  = groundtruth
+    # second_list = estimate
+    first_keys = first_list.keys()
+    second_keys = second_list.keys()
+    potential_matches = [(abs(a - (b + offset)), a, b)
+                         for a in first_keys
+                         for b in second_keys
+                         if abs(a - (b + offset)) < max_difference]
+
+    potential_matches.sort()
+    matches = []
+    for diff, a, b in potential_matches:
+        if a in first_keys and b in second_keys:
+            first_keys.remove(a)
+            second_keys.remove(b)
+            matches.append((a, b))
+
+    matches.sort()
+
+    return matches
+
+if __name__ == '__main__':
+
+    # parse command line
+    parser = argparse.ArgumentParser(description='''
+    This script takes two data files with timestamps and associates them
+    ''')
+    parser.add_argument('first_file', help='first text file (format: timestamp data)')
+    parser.add_argument('second_file', help='second text file (format: timestamp data)')
+    parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
+    parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
+    parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
+    args = parser.parse_args()
+
+    first_list = read_file_list(args.first_file)
+    second_list = read_file_list(args.second_file)
+
+    matches = associate(first_list, second_list,float(args.offset),float(args.max_difference))
+
+    if args.first_only:
+        for a,b in matches:
+            print("%f %s"%(a," ".join(first_list[a])))
+    else:
+        for a,b in matches:
+            print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
+
+

svo_analysis/scripts/evaluate_rpe.py

@@ -0,0 +1,355 @@
+#!/usr/bin/python
+
+import yaml
+import argparse
+import random
+import numpy
+import sys
+import transformations
+import matplotlib
+import matplotlib.pyplot as plt
+import matplotlib.pylab as pylab
+from matplotlib import rc
+
+def transform44(l):
+    t = l[1:4]
+    q = l[4:8]
+    return numpy.dot(transformations.translation_matrix(t),transformations.quaternion_matrix(q))
+
+def read_trajectory(filename, matrix=True):
+    file = open(filename)
+    data = file.read()
+    lines = data.replace(","," ").replace("\t"," ").split("\n")
+    list = [[float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]
+    list_ok = []
+    for i,l in enumerate(list):
+        if l[4:8]==[0,0,0,0]:
+            continue
+        isnan = False
+        for v in l:
+            if numpy.isnan(v):
+                isnan = True
+                break
+        if isnan:
+            sys.stderr.write("Warning: line %d of file '%s' has NaNs, skipping line\n"%(i,filename))
+            continue
+        list_ok.append(l)
+    if matrix :
+      traj = dict([(l[0],transform44(l[0:])) for l in list_ok])
+    else:
+      traj = dict([(l[0],l[1:8]) for l in list_ok])
+    return traj
+
+def find_closest_index(L,t):
+    beginning = 0
+    difference = abs(L[0] - t)
+    best = 0
+    end = len(L)
+    while beginning < end:
+        middle = int((end+beginning)/2)
+        if abs(L[middle] - t) < difference:
+            difference = abs(L[middle] - t)
+            best = middle
+        if t == L[middle]:
+            return middle
+        elif L[middle] > t:
+            end = middle
+        else:
+            beginning = middle + 1
+    return best
+
+def ominus(a,b):
+    return numpy.dot(numpy.linalg.inv(a),b)
+
+def scale(a,scalar):
+    return numpy.array(
+        [[a[0,0], a[0,1], a[0,2], a[0,3]*scalar],
+         [a[1,0], a[1,1], a[1,2], a[1,3]*scalar],
+         [a[2,0], a[2,1], a[2,2], a[2,3]*scalar],
+         [a[3,0], a[3,1], a[3,2], a[3,3]]]
+                       )
+
+def compute_distance(transform):
+    return numpy.linalg.norm(transform[0:3,3])
+
+def compute_angle(transform):
+    # an invitation to 3-d vision, p 27
+    return numpy.arccos( min(1,max(-1, (numpy.trace(transform[0:3,0:3]) - 1)/2) ))
+
+def distances_along_trajectory(traj):
+    keys = traj.keys()
+    keys.sort()
+    motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)]
+    distances = [0]
+    sum = 0
+    for t in motion:
+        sum += compute_distance(t)
+        distances.append(sum)
+    return distances
+
+def rotations_along_trajectory(traj,scale):
+    keys = traj.keys()
+    keys.sort()
+    motion = [ominus(traj[keys[i+1]],traj[keys[i]]) for i in range(len(keys)-1)]
+    distances = [0]
+    sum = 0
+    for t in motion:
+        sum += compute_angle(t)*scale
+        distances.append(sum)
+    return distances
+
+def interpolate_transformation(alpha,a,b):
+    qa,ta = transformations.quaternion_from_matrix(a),transformations.translation_from_matrix(a)
+    qb,tb = transformations.quaternion_from_matrix(b),transformations.translation_from_matrix(b)
+    qr = transformations.quaternion_slerp(qa,qb,alpha)
+    tr = (1-alpha) * ta + (alpha) * tb
+    r = numpy.dot(transformations.translation_matrix(tr),transformations.quaternion_matrix(qr))
+    return r
+
+def interpolate_and_resample(traj_gt,stamps_est,param_offset,gt_max_time_difference):
+    traj_new = {}
+
+    stamps_gt = list(traj_gt.keys())
+    stamps_gt.sort()
+
+    for t_est in stamps_est:
+        t_new = t_est + param_offset
+        i_nearest = find_closest_index(stamps_gt,t_est + param_offset)
+        if stamps_gt[i_nearest] >= t_new and i_nearest>0:
+            i_low = i_nearest-1
+            i_high = i_nearest
+        else:
+            i_low = i_nearest
+            i_high = i_nearest+1
+        if i_low<0 or i_high>=len(stamps_gt):
+            continue
+
+        t_low = stamps_gt[i_low]
+        t_high = stamps_gt[i_high]
+
+        if(abs( t_new - (t_low + param_offset) ) > gt_max_time_difference or
+           abs( t_new - (t_high + param_offset) ) > gt_max_time_difference):
+            continue
+
+        alpha = (t_new - (t_low + param_offset)) / (t_high - t_low)
+        transform = interpolate_transformation(alpha,traj_gt[t_low],traj_gt[t_high])
+        traj_new[t_new] = transform
+    return traj_new
+
+
+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,param_interpolation=True):
+    stamps_est = list(traj_est.keys())
+    stamps_est.sort()
+    stamps_gt = list(traj_gt.keys())
+    stamps_gt.sort()
+
+    gt_interval = numpy.median([s-t for s,t in zip(stamps_gt[1:],stamps_gt[:-1])])
+    gt_max_time_difference = 2*gt_interval
+
+    if param_interpolation:
+        traj_gt = interpolate_and_resample(traj_gt,stamps_est,param_offset,gt_max_time_difference)
+        stamps_gt = list(traj_gt.keys())
+        stamps_gt.sort()
+
+    stamps_est_return = []
+    for t_est in stamps_est:
+        t_gt = stamps_gt[find_closest_index(stamps_gt,t_est + param_offset)]
+        t_est_return = stamps_est[find_closest_index(stamps_est,t_gt - param_offset)]
+        t_gt_return = stamps_gt[find_closest_index(stamps_gt,t_est_return + param_offset)]
+        if not t_est_return in stamps_est_return:
+            stamps_est_return.append(t_est_return)
+    if(len(stamps_est_return)<2):
+        raise Exception("Number of overlap in the timestamps is too small. Did you run the evaluation on the right files?")
+
+    if param_delta_unit=="s":
+        index_est = list(traj_est.keys())
+        index_est.sort()
+    elif param_delta_unit=="m":
+        index_est = distances_along_trajectory(traj_est)
+    elif param_delta_unit=="rad":
+        index_est = rotations_along_trajectory(traj_est,1)
+    elif param_delta_unit=="deg":
+        index_est = rotations_along_trajectory(traj_est,180/numpy.pi)
+    elif param_delta_unit=="f":
+        index_est = range(len(traj_est))
+    else:
+        raise Exception("Unknown unit for delta: '%s'"%param_delta_unit)
+
+    if not param_fixed_delta:
+        if(param_max_pairs==0 or len(traj_est)<numpy.sqrt(param_max_pairs)):
+            pairs = [(i,j) for i in range(len(traj_est)) for j in range(len(traj_est))]
+        else:
+            pairs = [(random.randint(0,len(traj_est)-1),random.randint(0,len(traj_est)-1)) for i in range(param_max_pairs)]
+    else:
+        pairs = []
+        for i in range(len(traj_est)):
+            j = find_closest_index(index_est,index_est[i] + param_delta)
+            if j!=len(traj_est)-1:
+                pairs.append((i,j))
+        if(param_max_pairs!=0 and len(pairs)>param_max_pairs):
+            pairs = random.sample(pairs,param_max_pairs)
+
+    result = []
+    for i,j in pairs:
+        stamp_est_0 = stamps_est[i]
+        stamp_est_1 = stamps_est[j]
+
+        stamp_gt_0 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_0 + param_offset) ]
+        stamp_gt_1 = stamps_gt[ find_closest_index(stamps_gt,stamp_est_1 + param_offset) ]
+
+        if(abs(stamp_gt_0 - (stamp_est_0 + param_offset)) > gt_max_time_difference  or
+           abs(stamp_gt_1 - (stamp_est_1 + param_offset)) > gt_max_time_difference):
+            continue
+
+        error44 = ominus(  scale(
+                           ominus( traj_est[stamp_est_1], traj_est[stamp_est_0] ),param_scale),
+                           ominus( traj_gt[stamp_gt_1], traj_gt[stamp_gt_0] ) )
+
+        
+        trans = compute_distance(error44)
+        rot = compute_angle(error44)
+
+        motion_gt = ominus( traj_gt[stamp_gt_1], traj_gt[stamp_gt_0] )
+        trans_gt = compute_distance(motion_gt)
+        rot_gt = compute_angle(motion_gt)
+        
+        if(rot < 0.07):
+          result.append([stamp_est_0,stamp_est_1,stamp_gt_0,stamp_gt_1,trans,rot,trans_gt,rot_gt])
+
+    if len(result)<2:
+        raise Exception("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory!")
+
+    return result
+
+def percentile(seq,q):
+    seq_sorted = list(seq)
+    seq_sorted.sort()
+    return seq_sorted[int((len(seq_sorted)-1)*q)]
+
+if __name__ == '__main__':
+    random.seed(0)
+
+    # config -------------------------------------------------------------------------
+    
+    dataset='20131003_1937_nslam_i7_asl2_fast'
+    dataset_dir = '../results/'+dataset
+    
+    #---------------------------------------------------------------------------------
+    
+    # load dataset parameters
+    params_stream = open(dataset_dir+'/params.yaml')
+    params = yaml.load(params_stream)
+    
+    # set trajectory groundtruth and estimate file
+    traj_groundtruth = params['dataset_directory']+'/groundtruth_filtered.txt'
+    traj_estimate_rotated = dataset_dir+'/traj_estimate_rotated.txt'
+
+    parser = argparse.ArgumentParser(description='''
+    This script computes the relative pose error from the ground truth trajectory and the estimated trajectory.
+    ''')
+    parser.add_argument('--groundtruth_file', help='ground-truth trajectory file (format: "timestamp tx ty tz qx qy qz qw")', default=traj_groundtruth)
+    parser.add_argument('--estimated_file', help='estimated trajectory file (format: "timestamp tx ty tz qx qy qz qw")', default=traj_estimate_rotated)
+    parser.add_argument('--interpolate', help='interpolate and resample the ground truth trajectory', default=True)
+    parser.add_argument('--max_pairs', help='maximum number of pose comparisons (default: 10000, set to zero to disable downsampling)', default=10000)
+    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)', default=True)
+    parser.add_argument('--delta', help='delta for evaluation (default: 1.0)',default=1.0)
+    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')
+    parser.add_argument('--offset', help='time offset between ground-truth and estimated trajectory (default: 0.0)',default=0.0)
+    parser.add_argument('--scale', help='scaling factor for the estimated trajectory (default: 1.0)',default=1.0)
+    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)')
+    parser.add_argument('--plot', help='plot the result to a file (requires --fixed_delta, output format: png)', default=True)
+    parser.add_argument('--plotname', help='plot the result to a file (requires --fixed_delta, output format: png)', default='rpe.png')
+    parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the mean translational error measured in meters will be printed)', default=True)
+    args = parser.parse_args()
+
+    if args.plot and not args.fixed_delta:
+        sys.exit("The '--plot' option can only be used in combination with '--fixed_delta'")
+
+    traj_gt = read_trajectory(args.groundtruth_file)
+    traj_est = read_trajectory(args.estimated_file)
+
+    result = evaluate_trajectory(traj_gt,
+                                 traj_est,
+                                 int(args.max_pairs),
+                                 args.fixed_delta,
+                                 float(args.delta),
+                                 args.delta_unit,
+                                 float(args.offset),
+                                 float(args.scale),
+                                 args.interpolate)
+
+    stamps = numpy.array(result)[:,0]
+    trans_error = numpy.array(result)[:,4]
+    rot_error = numpy.array(result)[:,5]
+    
+    
+    results = dict()
+    results['trans_rmse'] = float(numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error)))
+    results['trans_mean'] = float(numpy.mean(trans_error))
+    results['trans_median'] = float(numpy.median(trans_error))
+    results['trans_std'] = float(numpy.std(trans_error))
+    results['trans_min'] = float(numpy.min(trans_error))
+    results['trans_max'] = float(numpy.max(trans_error))
+    results['rot_rmse'] = float((numpy.sqrt(numpy.dot(rot_error,rot_error) / len(rot_error)) * 180.0 / numpy.pi))
+    results['rot_mean'] = float((numpy.mean(rot_error) * 180.0 / numpy.pi))
+    results['rot_median'] = float(numpy.median(rot_error) * 180.0 / numpy.pi)
+    results['rot_std'] = float((numpy.std(rot_error) * 180.0 / numpy.pi))
+    results['rot_min'] = float((numpy.min(rot_error) * 180.0 / numpy.pi))
+    results['rot_max'] = float((numpy.max(rot_error) * 180.0 / numpy.pi))
+    
+    # dump experiment results to file:
+    results_file = params['trace_dir']+'/rpe_results.yaml'
+    with open(results_file,'w') as outfile:
+      outfile.write( yaml.dump(results, default_flow_style=False) )
+    
+    if args.save: 
+        f = open(args.save,"w")
+        f.write("\n".join([" ".join(["%f"%v for v in line]) for line in result]))
+        f.close()
+
+    if args.verbose:
+        print "compared_pose_pairs %d pairs"%(len(trans_error))
+
+        print "translational_error.rmse %f m"%results['trans_rmse']
+        print "translational_error.mean %f m"%results['trans_mean']
+        print "translational_error.median %f m"%results['trans_median']
+        print "translational_error.std %f m"%results['trans_std']
+        print "translational_error.min %f m"%results['trans_min']
+        print "translational_error.max %f m"%results['trans_max']
+
+        print "rotational_error.rmse %f deg"%results['rot_rmse']
+        print "rotational_error.mean %f deg"%results['rot_mean']
+        print "rotational_error.median %f deg"%results['rot_median']
+        print "rotational_error.std %f deg"%results['rot_std']
+        print "rotational_error.min %f deg"%results['rot_min']
+        print "rotational_error.max %f deg"%results['rot_max']
+    else:
+        print numpy.mean(trans_error)
+
+    if args.plot:
+      
+        # tell matplotlib to use latex font
+        rc('font',**{'family':'serif','serif':['Cardo']})
+        rc('text', usetex=True)
+  
+        #matplotlib.use('Agg') # no window should appear?
+
+        fig = plt.figure(figsize=(8, 3))
+        ax = fig.add_subplot(111)
+        ax.plot(stamps - stamps[0],trans_error,'-',color="blue")
+        #ax.plot([t for t,e in err_rot],[e for t,e in err_rot],'-',color="red")
+        ax.set_xlabel('time [s]')
+        ax.set_ylabel('translational error [m]')
+        plt.tight_layout()
+        plt.savefig(dataset_dir+'/rpe_trans_error.pdf')
+        plt.show()
+
+        fig = plt.figure(figsize=(8, 3))
+        ax = fig.add_subplot(111)
+        ax.plot(stamps - stamps[0],rot_error* 180.0 / numpy.pi,'-',color="blue")
+        #ax.plot([t for t,e in err_rot],[e for t,e in err_rot],'-',color="red")
+        ax.set_xlabel('time [s]')
+        ax.set_ylabel('rot error [deg]')
+        plt.tight_layout()
+        plt.savefig(dataset_dir+'/rpe_rot_error.pdf')
+        plt.show()

svo_analysis/scripts/filter_groundtruth_smooth.py

@@ -0,0 +1,60 @@
+#!/usr/bin/python
+
+
+import numpy as np
+import matplotlib.pyplot as plt
+import transformations
+from scipy import signal
+
+save = True
+data_filename = '/home/cforster/Datasets/SlamBenchmark/asl_vicon_d2/groundtruth.txt'
+filtered_data_filename = '/home/cforster/Datasets/SlamBenchmark/asl_vicon_d2/groundtruth_filtered.txt'
+file = open(data_filename)
+data = file.read()
+lines = data.replace(","," ").replace("\t"," ").split("\n")
+D = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
+
+n = np.shape(D)[0]
+rpy = np.empty([n,3])
+for i in range(n):
+  quat = D[i,4:8]
+  rpy[i,:] = transformations.euler_from_quaternion(quat, axes='sxyz')
+
+# filter rpy
+f_sensor = 200.0; # sampling frequency in hz
+f_cut = 15.0; # cutoff frequency in hz
+b,a = signal.butter(5,f_cut/(f_sensor/2));
+print a
+print b
+
+rpy_filt = np.empty([n,3])
+rpy_filt[:,0] = signal.filtfilt(b, a, rpy[:,0])
+rpy_filt[:,1] = signal.filtfilt(b, a, rpy[:,1])
+rpy_filt[:,2] = signal.filtfilt(b, a, rpy[:,2])
+
+fig = plt.figure()
+ax = fig.add_subplot(111, title='orientation filtered')
+ax.plot(rpy[:,0], 'r-')
+ax.plot(rpy[:,1], 'g-')
+ax.plot(rpy[:,2], 'b-')
+ax.plot(rpy_filt[:,0], 'k-', linewidth=2)
+ax.plot(rpy_filt[:,1], 'k-', linewidth=2)
+ax.plot(rpy_filt[:,2], 'k-', linewidth=2)
+
+fig = plt.figure()
+ax = fig.add_subplot(111, title='position')
+ax.plot(D[:,1], 'r')
+ax.plot(D[:,2], 'g')
+ax.plot(D[:,3], 'b')
+
+fig = plt.figure()
+ax = fig.add_subplot(111, title='trajectory from top')
+ax.plot(D[:,1], D[:,2])
+
+if save:
+  f = open(filtered_data_filename,'w')
+  for i in range(np.shape(D)[0]):
+    quat = transformations.quaternion_from_euler(rpy_filt[i,0], rpy_filt[i,1], rpy_filt[i,2], axes='sxyz')
+    f.write('%.7f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n' % (D[i,0], D[i,1], D[i,2], D[i,3], quat[0], quat[1], quat[2], quat[3]))
+  
+  f.close()

svo_analysis/scripts/hand_eye_calib.py

@@ -0,0 +1,287 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Aug  4 15:47:55 2013
+
+@author: cforster
+"""
+
+import yaml
+import numpy as np
+import associate
+import rotation_utils as ru
+import matplotlib.pyplot as plt
+
+def alignSim3(model, data, n):
+
+  # select the first n datapoints and remove its mean so their center is zero
+  M = model[0:n,:]
+  D = data[0:n,:]
+
+  # substract mean
+  mu_M = M.mean(0)
+  mu_D = D.mean(0)
+  M_zerocentered = M - mu_M
+  D_zerocentered = D - mu_D
+
+  # correlation
+  C = 1.0/n*np.dot(M_zerocentered.transpose(), D_zerocentered)
+  sigma2 = 1.0/n*np.multiply(D_zerocentered,D_zerocentered).sum()
+  U_svd,D_svd,V_svd = np.linalg.linalg.svd(C)
+  D_svd = np.diag(D_svd)
+  V_svd = np.transpose(V_svd)
+  S = np.eye(3)
+
+  if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0):
+    S[2,2] = -1
+
+  R = np.dot(U_svd, np.dot(S, np.transpose(V_svd)))
+  s = 1.0/sigma2*np.trace(np.dot(D_svd, S))
+  t = mu_M-s*np.dot(R,mu_D)
+
+  return s, R, t
+
+def matrixLog(A):
+  theta = np.arccos((np.trace(A)-1.0)/2.0)
+  log_theta = 0.5*theta/np.sin(theta) * (A - A.transpose())
+  x = np.array([log_theta[2,1], log_theta[0,2], log_theta[1,0]])
+  return x
+
+def handEyeCalib(q_gt, q_es, p_gt, p_es, I, delta=10, verbose=True):
+  ''' Implementation of the least squares solution described in the paper:
+  Robot Sensor Calibration: Solving AX=XB on the Euclidean Group
+  by Frank C. Park and Bryan J. Martin
+  '''
+  n = np.shape(I)[0]
+  M = np.zeros([3,3])
+  C = np.zeros([3*n, 3])
+  b_A = np.zeros([3*n,1])
+  b_B = np.zeros([3*n,1])
+  for ix, i in enumerate(I):
+    A1 = ru.quat2dcm(q_es[i,:])
+    A2 = ru.quat2dcm(q_es[i+delta,:])
+    A  = np.dot(A1.transpose(), A2)
+    B1 = ru.quat2dcm(q_gt[i,:])
+    B2 = ru.quat2dcm(q_gt[i+delta,:])
+    B  = np.dot(B1.transpose(), B2)
+    alpha = matrixLog(A)
+    beta = matrixLog(B)
+    M = M + np.dot(np.matrix(beta).transpose(), np.matrix(alpha))
+    C[3*ix:3*ix+3,:] = np.eye(3) - A
+    b_A[3*ix:3*ix+3,0] = np.dot(np.transpose(A1), p_es[i+delta,:]-p_es[i,:])
+    b_B[3*ix:3*ix+3,0] = np.dot(np.transpose(B1), p_gt[i+delta,:]-p_gt[i,:])
+
+  # compute rotation  
+  D,V = np.linalg.linalg.eig(np.dot(M.transpose(), M))
+  Lambda = np.diag([np.sqrt(1.0/D[0]), np.sqrt(1.0/D[1]), np.sqrt(1.0/D[2])])
+  Vinv = np.linalg.linalg.inv(V)
+  X = np.dot(V, np.dot(Lambda, np.dot(Vinv, M.transpose())))
+
+  # compute translation
+  d = np.zeros([3*n,1])
+  for i in range(n):
+    d[3*i:3*i+3,:] = b_A[3*i:3*i+3,:] - np.dot(X, b_B[3*i:3*i+3,:])
+
+  b = np.dot(np.linalg.inv(np.dot(np.transpose(C),C)),  np.dot(np.transpose(C),d))
+
+  return np.array(X),b
+
+
+# user config
+display = True
+dataset = '20130906_2149_ptam_i7_asl2'
+n_measurements = 1000
+delta = 20
+
+# load dataset parameters
+dataset_dir = '../results/'+dataset
+params_stream = open(dataset_dir+'/params.yaml')
+params = yaml.load(params_stream)
+
+# set trajectory groundtruth and estimate file
+traj_groundtruth = params['dataset_directory']+'/groundtruth_filtered.txt'
+traj_estimate = dataset_dir+'/traj_estimate.txt'
+
+# load dataset params
+dataset_param_file_stream = open(params['dataset_directory']+'/dataset_params.yaml','r')
+dataset_param = yaml.load(dataset_param_file_stream)
+
+# load data
+data_gt = associate.read_file_list(traj_groundtruth)
+data_es = associate.read_file_list(traj_estimate)
+
+# select matches
+#offset = -dataset_param['cam_delay']
+offset = -0.03
+matches = associate.associate(data_gt, data_es, offset, 0.02)
+#matches = matches[0:470]
+p_gt = np.array([[float(value) for value in data_gt[a][0:3]] for a,b in matches])
+q_gt = np.array([[float(value) for value in data_gt[a][3:7]] for a,b in matches])
+p_es = np.array([[float(value) for value in data_es[b][0:3]] for a,b in matches])
+q_es = np.array([[float(value) for value in data_es[b][3:7]] for a,b in matches])
+
+# --------------------------------------------------------------------------------
+# align Sim3 to get scale
+scale,rot,trans = alignSim3(p_gt, p_es, np.shape(matches)[0])
+p_es = scale*p_es
+#p_es = np.transpose(scale*np.dot(rot,np.transpose(p_es)))+trans
+
+print 's='+str(scale)
+#print 't='+str(trans)
+#print 'R='+str(rot)
+
+# plot sim3 aligned trajectory
+if display:
+  fig = plt.figure()
+  ax = fig.add_subplot(111, aspect='equal')
+  ax.plot(p_es[:,0], p_es[:,1], 'r-', label='estimate')
+  ax.plot(p_gt[:,0], p_gt[:,1], 'b-', label='groundtruth')
+  ax.legend()
+
+# --------------------------------------------------------------------------------
+# hand-eye-calib
+
+# select random measurements
+I = np.array(rand(n_measurements,1)*(np.shape(matches)[0]-delta), dtype=int)[:,0]
+R,b = handEyeCalib(q_gt, q_es, p_gt, p_es, I, delta, True)
+
+print 'quat = ' + str(ru.dcm2quat(R))
+print 'b = ' + str(b)
+
+rpy_es = np.zeros([q_es.shape[0]-1, 3])
+rpy_gt = np.zeros([q_gt.shape[0]-1, 3])
+t_gt = np.zeros([q_es.shape[0]-1,3])
+t_es = np.zeros([q_es.shape[0]-1,3])
+
+for i in range(delta,np.shape(q_es)[0]):
+  A1 = ru.quat2dcm(q_es[i-delta,:])
+  A2 = ru.quat2dcm(q_es[i,:])
+  A  = np.dot(A1.transpose(), A2)
+  B1 = ru.quat2dcm(q_gt[i-delta,:])
+  B2 = ru.quat2dcm(q_gt[i,:])
+  B  = np.dot(B1.transpose(), B2)
+  B_es  = np.dot(np.transpose(R), np.dot(A, R))
+  rpy_gt[i-delta,:] = ru.dcm2rpy(B)
+  rpy_es[i-delta,:] = ru.dcm2rpy(B_es)
+  t_B = np.dot(np.transpose(B1),(p_gt[i,:]-p_gt[i-delta,:]))
+  t_A = np.dot(np.transpose(A1),(p_es[i,:]-p_es[i-delta,:]))
+  t_gt[i-delta,:] = t_B
+  t_es[i-delta,:] = np.dot(np.transpose(R), np.dot(A,b[:,0]) + t_A - b[:,0])
+
+alignment_error = (t_gt-t_es)
+error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),1))
+I_accurate = np.argwhere(error < np.percentile(error, 90))[:,0]
+
+if display:
+  plt.figure()
+  plt.plot(rpy_es[:,0], 'r-', label='es roll')
+  plt.plot(rpy_es[:,1], 'g-', label='es pitch')
+  plt.plot(rpy_es[:,2], 'b-', label='es yaw')
+  plt.plot(rpy_gt[:,0], 'r--', label='gt roll')
+  plt.plot(rpy_gt[:,1], 'g--', label='gt pitch')
+  plt.plot(rpy_gt[:,2], 'b--', label='gt yaw')
+  plt.legend()
+  plt.figure()
+  plt.plot(t_gt[:,0], 'r-', label='gt x')
+  plt.plot(t_gt[:,1], 'g-', label='gt y')
+  plt.plot(t_gt[:,2], 'b-', label='gt z')
+  plt.plot(t_es[:,0], 'r--', label='es x')
+  plt.plot(t_es[:,1], 'g--', label='es y')
+  plt.plot(t_es[:,2], 'b--', label='es z')
+  plt.legend()
+  plt.figure()
+  plt.plot(error,'-g')
+
+
+print 'e_rms = ' + str(np.sqrt(np.dot(error,error) / len(error)))
+print 'e_mean = ' + str(np.mean(error))
+print 'e_median = ' + str(np.median(error))
+print 'e_std = ' + str(np.std(error))
+
+
+# now sample again from the filtered list:
+N = 10
+display = False
+n_acc_meas = I_accurate.size
+n_measurements = 500
+
+#for i in range(5):
+#  print '-------------------------------------'
+#  i0 = np.array(rand(n_measurements,1)*np.shape(I_accurate)[0], dtype=int)[:,0]
+#  i1 = np.minimum(i0+N, n_acc_meas-1)
+#  I = np.empty(i0.size*2, dtype=int)
+#  I[0::2] = I_accurate[i0]
+#  I[1::2] = I_accurate[i1]
+#  R,b = handEyeCalib(q_gt[I,:], q_es[I,:], p_gt[I,:], p_es[I,:], True)
+#  print 'quat = ' + str(ru.dcm2quat(R))
+#  print 'b = ' + str(b)
+#  rpy_es = np.zeros([q_es.shape[0]-1, 3])
+#  rpy_gt = np.zeros([q_gt.shape[0]-1, 3])
+#  t_gt = np.zeros([q_es.shape[0]-1,3])
+#  t_es = np.zeros([q_es.shape[0]-1,3])
+#
+#  delta = 10
+#  for i in range(delta,np.shape(q_es)[0]):
+#    A1 = ru.quat2dcm(q_es[i-delta,:])
+#    A2 = ru.quat2dcm(q_es[i,:])
+#    A  = np.dot(A1.transpose(), A2)
+#    B1 = ru.quat2dcm(q_gt[i-delta,:])
+#    B2 = ru.quat2dcm(q_gt[i,:])
+#    B  = np.dot(B1.transpose(), B2)
+#    B_es  = np.dot(np.transpose(R), np.dot(A, R))
+#    rpy_gt[i-delta,:] = ru.dcm2rpy(B)
+#    rpy_es[i-delta,:] = ru.dcm2rpy(B_es)
+#    t_B = np.dot(np.transpose(B1),(p_gt[i,:]-p_gt[i-delta,:]))
+#    t_A = np.dot(np.transpose(A1),(p_es[i,:]-p_es[i-delta,:]))
+#    t_gt[i-delta,:] = t_B
+#    t_es[i-delta,:] = np.dot(np.transpose(R), np.dot(A,b[:,0]) + t_A - b[:,0])
+#  alignment_error = (t_gt-t_es)
+#  error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),1))
+#
+#  if display:
+#    plt.figure()
+#    plt.plot(rpy_es[:,0], 'r-', label='es roll')
+#    plt.plot(rpy_es[:,1], 'g-', label='es pitch')
+#    plt.plot(rpy_es[:,2], 'b-', label='es yaw')
+#    plt.plot(rpy_gt[:,0], 'r--', label='gt roll')
+#    plt.plot(rpy_gt[:,1], 'g--', label='gt pitch')
+#    plt.plot(rpy_gt[:,2], 'b--', label='gt yaw')
+#    plt.legend()
+#    plt.figure()
+#    plt.plot(t_gt[:,0], 'r-', label='es x')
+#    plt.plot(t_gt[:,1], 'g-', label='es y')
+#    plt.plot(t_gt[:,2], 'b-', label='es z')
+#    plt.plot(t_es[:,0], 'r--', label='gt x')
+#    plt.plot(t_es[:,1], 'g--', label='gt y')
+#    plt.plot(t_es[:,2], 'b--', label='gt z')
+#    plt.legend()
+#    plt.figure()
+#    plt.plot(error,'-g')
+#
+#  print 'e_rms = ' + str(np.sqrt(np.dot(error,error) / len(error)))
+#  print 'e_mean = ' + str(np.mean(error))
+#  print 'e_median = ' + str(np.median(error))
+#  print 'e_std = ' + str(np.std(error))
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

svo_analysis/scripts/rotation_utils.py

@@ -0,0 +1,143 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Fri Jun 28 10:42:37 2013
+
+@author: cforster
+"""
+
+import math
+import numpy as np
+
+# epsilon for testing whether a number is close to zero
+_EPS = np.finfo(float).eps * 4.0
+
+# axis sequences for Euler angles
+_NEXT_AXIS = [1, 2, 0, 1]
+
+# map axes strings to/from tuples of inner axis, parity, repetition, frame
+_AXES2TUPLE = {
+    'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
+    'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
+    'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
+    'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
+    'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
+    'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
+    'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
+    'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
+
+_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
+
+
+def quat2dcm(quaternion):
+    """Convert Quaternion [x y z w] to Direct Cosine Matrix representation """
+    q = np.array(quaternion[:4], dtype=np.float64, copy=True)
+    nq = np.dot(q, q)
+    if nq < _EPS:
+      return np.identity(4)
+    q *= math.sqrt(2.0 / nq)
+    q = np.outer(q, q)
+    return np.array([ [1.0-q[1, 1]-q[2, 2],     q[0, 1]-q[2, 3],     q[0, 2]+q[1, 3]],
+                      [    q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2],     q[1, 2]-q[0, 3]],
+                      [    q[0, 2]-q[1, 3],     q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1]]],
+                      dtype=np.float64)
+        
+def dcm2quat(matrix):
+    """Convert Direct Cosine Matrix to Quaternion
+    
+       This algorithm comes from  "Quaternion Calculus and Fast Animation",
+       Ken Shoemake, 1987 SIGGRAPH course notes
+    """
+    q = np.empty((4, ), dtype=np.float64)
+    M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
+    t = np.trace(M)
+    if (t > 0):
+      t = math.sqrt(t + 1.0);
+      q[3] = 0.5*t;
+      t = 0.5/t;
+      q[0] = (M[2,1] - M[1,2]) * t;
+      q[1] = (M[0,2] - M[2,0]) * t;
+      q[2] = (M[1,0] - M[0,1]) * t;
+    else:
+      i = 0;
+      if (M[1,1] > M[0,0]):
+        i = 1;
+      if (M[2,2] > M[i,i]):
+        i = 2;
+      j = (i+1)%3;
+      k = (j+1)%3;
+
+      t = math.sqrt(M[i,i]-M[j,j]-M[k,k] + 1.0);
+      q[i] = 0.5 * t;
+      t = 0.5/t;
+      q[3] = (M[k,j]-M[j,k])*t;
+      q[j] = (M[j,i]+M[i,j])*t;
+      q[k] = (M[k,i]+M[i,k])*t;
+    return q
+
+def dcm2rpy(matrix, axes='sxyz'):
+    """Return Euler angles from rotation matrix for specified axis sequence.
+
+    axes : One of 24 axis sequences as string or encoded tuple
+
+    Note that many Euler angle triplets can describe one matrix.
+
+    >>> R0 = euler_matrix(1, 2, 3, 'syxz')
+    >>> al, be, ga = euler_from_matrix(R0, 'syxz')
+    >>> R1 = euler_matrix(al, be, ga, 'syxz')
+    >>> numpy.allclose(R0, R1)
+    True
+    >>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
+    >>> for axes in _AXES2TUPLE.keys():
+    ...    R0 = euler_matrix(axes=axes, *angles)
+    ...    R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
+    ...    if not numpy.allclose(R0, R1): print axes, "failed"
+
+    """
+    try:
+        firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
+    except (AttributeError, KeyError):
+        _ = _TUPLE2AXES[axes]
+        firstaxis, parity, repetition, frame = axes
+
+    i = firstaxis
+    j = _NEXT_AXIS[i+parity]
+    k = _NEXT_AXIS[i-parity+1]
+
+    M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
+    if repetition:
+        sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
+        if sy > _EPS:
+            ax = math.atan2( M[i, j],  M[i, k])
+            ay = math.atan2( sy,       M[i, i])
+            az = math.atan2( M[j, i], -M[k, i])
+        else:
+            ax = math.atan2(-M[j, k],  M[j, j])
+            ay = math.atan2( sy,       M[i, i])
+            az = 0.0
+    else:
+        cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
+        if cy > _EPS:
+            ax = math.atan2( M[k, j],  M[k, k])
+            ay = math.atan2(-M[k, i],  cy)
+            az = math.atan2( M[j, i],  M[i, i])
+        else:
+            ax = math.atan2(-M[j, k],  M[j, j])
+            ay = math.atan2(-M[k, i],  cy)
+            az = 0.0
+
+    if parity:
+        ax, ay, az = -ax, -ay, -az
+    if frame:
+        ax, az = az, ax
+    return ax, ay, az
+    
+def quat2rpy(quaternion, axes='sxyz'):
+    """Return Euler angles from quaternion for specified axis sequence.
+
+    >>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947])
+    >>> numpy.allclose(angles, [0.123, 0, 0])
+    True
+
+    """
+    return dcm2rpy(quat2dcm(quaternion), axes)
+    

svo_analysis/scripts/trajectory_comparison.py

@@ -0,0 +1,166 @@
+# -*- coding: utf-8 -*-
+
+import numpy as np
+import matplotlib.pyplot as plt
+import yaml
+from matplotlib import rc
+from mpl_toolkits.axes_grid1.inset_locator import zoomed_inset_axes
+from mpl_toolkits.axes_grid1.inset_locator import mark_inset
+
+
+def plotTrajectory(ax, filename, label, color, linewidth):
+  file = open(filename)
+  data = file.read()
+  lines = data.replace(","," ").replace("\t"," ").split("\n")
+  trajectory = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
+  ax.plot(trajectory[:,1], trajectory[:,2], label=label, color=color, linewidth=linewidth)
+  
+
+if __name__ == '__main__':
+  
+  results_dir = '../results'
+  experiments = ['20130911_2113_nslam_i7_asl2_acc',
+                 '20130911_2229_nslam_i7_asl2_fast',
+                 '20130906_2149_ptam_i7_asl2']
+                 
+  # tell matplotlib to use latex font
+  rc('font',**{'family':'serif','serif':['Cardo']})
+  rc('text', usetex=True)               
+
+  # ------------------------------------------------------------------------------
+  # position error 
+  fig_poserr = plt.figure(figsize=(8,6))
+  ax_poserr_x = fig_poserr.add_subplot(311, ylabel='x-error [m]')
+  ax_poserr_y = fig_poserr.add_subplot(312, ylabel='y-error [m]')
+  ax_poserr_z = fig_poserr.add_subplot(313, ylabel='z-error [m]', xlabel='time [s]')
+  
+  for exp in experiments:
+    
+    # load dataset parameters
+    params_stream = open(results_dir+'/'+exp+'/params.yaml')
+    params = yaml.load(params_stream)
+  
+    # plot translation error
+    trans_error_filename = results_dir+'/'+exp+'/translation_error.txt'
+    file = open(trans_error_filename)
+    data = file.read()
+    lines = data.replace(","," ").replace("\t"," ").split("\n")
+    trans_error = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
+    ax_poserr_x.plot(trans_error[:,0], trans_error[:,1], label=params['experiment_label'])
+    ax_poserr_y.plot(trans_error[:,0], trans_error[:,2])
+    ax_poserr_z.plot(trans_error[:,0], trans_error[:,3])
+    
+  ax_poserr_x.set_xlim([0, trans_error[-1,0]+4])
+  ax_poserr_y.set_xlim([0, trans_error[-1,0]+4])
+  ax_poserr_z.set_xlim([0, trans_error[-1,0]+4])
+  ax_poserr_x.legend(bbox_to_anchor=[0, 0], loc='lower left', ncol=3)
+  ax_poserr_x.grid()
+  ax_poserr_y.grid()
+  ax_poserr_z.grid()
+  fig_poserr.tight_layout()
+  fig_poserr.savefig('../results/translation_error.pdf')
+  
+  # ------------------------------------------------------------------------------
+  # orientation error 
+  fig_roterr = plt.figure(figsize=(8,6))
+  ax_roterr_r = fig_roterr.add_subplot(311, ylabel='roll-error [rad]')
+  ax_roterr_p = fig_roterr.add_subplot(312, ylabel='pitch-error [rad]')
+  ax_roterr_y = fig_roterr.add_subplot(313, ylabel='yaw-error [rad]', xlabel='time [s]')
+  
+  for exp in experiments:
+    
+    # load dataset parameters
+    params_stream = open(results_dir+'/'+exp+'/params.yaml')
+    params = yaml.load(params_stream)
+  
+    # plot translation error
+    rot_error_filename = results_dir+'/'+exp+'/orientation_error.txt'
+    file = open(rot_error_filename)
+    data = file.read()
+    lines = data.replace(","," ").replace("\t"," ").split("\n")
+    rot_error = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
+    ax_roterr_r.plot(rot_error[:,0], rot_error[:,3], label=params['experiment_label'])
+    ax_roterr_p.plot(rot_error[:,0], rot_error[:,2])
+    ax_roterr_y.plot(rot_error[:,0], rot_error[:,1])
+    
+  ax_roterr_r.set_xlim([0, rot_error[-1,0]+4])
+  ax_roterr_p.set_xlim([0, rot_error[-1,0]+4])
+  ax_roterr_y.set_xlim([0, rot_error[-1,0]+4])
+  ax_roterr_r.legend(bbox_to_anchor=[0, 1], loc='upper left', ncol=3)
+  ax_roterr_r.grid()
+  ax_roterr_p.grid()
+  ax_roterr_y.grid()
+  fig_roterr.tight_layout()
+  fig_roterr.savefig('../results/orientation_error.pdf')
+  
+  # ------------------------------------------------------------------------------
+  # scale error 
+  fig_scale = plt.figure(figsize=(8,2.5))
+  ax_scale = fig_scale.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]')
+  
+  for exp in experiments:
+    
+    # load dataset parameters
+    params_stream = open(results_dir+'/'+exp+'/params.yaml')
+    params = yaml.load(params_stream)
+  
+    # plot translation error
+    scale_drift_filename = results_dir+'/'+exp+'/scale_drift.txt'
+    file = open(scale_drift_filename)
+    data = file.read()
+    lines = data.replace(","," ").replace("\t"," ").split("\n")
+    scale_drift = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64)
+    ax_scale.plot(scale_drift[:,0], scale_drift[:,1], label=params['experiment_label'])
+
+  ax_scale.set_xlim([0, rot_error[-1,0]+4])
+  ax_scale.legend(bbox_to_anchor=[0, 1], loc='upper left', ncol=3)
+  ax_scale.grid()
+  fig_scale.tight_layout()
+  fig_scale.savefig('../results/scale_drift.pdf')
+  
+  # ------------------------------------------------------------------------------
+  # trajectory
+  
+
+  fig_traj = plt.figure(figsize=(8,4.8))
+  ax_traj = fig_traj.add_subplot(111, xlabel='x [m]', ylabel='y [m]', aspect='equal', xlim=[-3.1, 4], ylim=[-1.5, 2.6])
+
+  plotTrajectory(ax_traj, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5)
+  plotTrajectory(ax_traj, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1)
+  plotTrajectory(ax_traj, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1)
+
+  # show detail:
+  axins = zoomed_inset_axes(ax_traj, 5.0, loc=1) # zoom = 6
+  plotTrajectory(axins, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5)
+  plotTrajectory(axins, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1)
+  plotTrajectory(axins, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1)
+  axins.set_xlim(1.2, 1.70)
+  axins.set_ylim(-0.8, -0.3)
+  axins.patch.set_facecolor('0.9')
+  plt.xticks(visible=False)
+  plt.yticks(visible=False)
+
+  mark_inset(ax_traj, axins, loc1=2, loc2=4, fc="none", ec='b')
+  plt.draw()
+  plt.show()
+  ax_traj.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=3)
+  ax_traj.grid()
+  fig_traj.tight_layout()
+  fig_traj.savefig('../results/trajectory_asl.pdf')
+  
+  
+  # ------------------------------------------------------------------------------
+  # number of matches
+  
+  # plot number of klt tracked and matched points
+#  fig = plt.figure(figsize=(8,2))
+#  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='no. features', ylim=[0,220])
+#  ax.plot(np.arange(len(n_fts_1))/25.0, n_fts_1, 'b-', label='Accurate')
+#  ax.plot(np.arange(len(n_fts_2))/25.0, n_fts_2, 'g-', label='Fast')
+#  ax.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=2)
+#  fig.tight_layout()
+#  fig.savefig('../results/number_of_features2.pdf', bbox_inches="tight")
+#  
+  
+  
+