plotting trajectory error

svo_analysis/scripts/benchmark.py

@@ -29,18 +29,28 @@ def run_experiment(dataset, params):
     algo_params = yaml.load(open(algo_params_file,'r'))
     
     # combine all parameters
-    if dataset_params['rig_size'] == 1:
+    if 'rig_size' not in dataset_params or dataset_params['rig_size'] == 1:
         params = dict(params.items() + algo_params.items() + dataset_params['cam0'].items())
     else:
         params = dict(params.items() + algo_params.items() + dataset_params.items())
     if 'dataset_is_blender' in dataset_params:
         params['dataset_is_blender'] = dataset_params['dataset_is_blender']
+    params['dataset_first_frame'] = dataset_params['dataset_first_frame']
     
     # dump experiment params to file and copy the other parameter files:
     params_dump_file = os.path.join(params['trace_dir'],'params.yaml')
     with open(params_dump_file,'w') as outfile:
         outfile.write(yaml.dump(params, default_flow_style=False))
-    shutil.copyfile(dataset_params_file, os.path.join(params['trace_dir'], 'dataset_params.yaml'))
+    shutil.copyfile(dataset_params_file,
+                    os.path.join(params['trace_dir'], 'dataset_params.yaml'))
+    
+    # copy the groundtruth trajectory to the trace dir for later evaluation
+    if params['dataset_is_blender']:
+        shutil.copyfile(os.path.join(params['dataset_directory'], 'trajectory.txt'),
+                        os.path.join(params['trace_dir'], 'groundtruth_matched.txt'))
+    else:
+        shutil.copyfile(os.path.join(params['dataset_directory'], 'groundtruth_matched.txt'),
+                        os.path.join(params['trace_dir'], 'groundtruth_matched.txt'))
     
     # execute ros node
     node = ros_node.RosNode(args.version, args.executable)

svo_analysis/src/svo_analysis/analyse_trajectory.py

@@ -1,14 +1,13 @@
-# -*- coding: utf-8 -*-
+#!/usr/bin/python
 
 import os
 import yaml
-from .tum_benchmark_tools import associate
+import argparse
+import numpy as np
 import matplotlib.pyplot as plt
+import svo_analysis.tum_benchmark_tools.associate as associate
 import vikit_py.transformations as transformations
 import vikit_py.align_trajectory as align_trajectory
-import numpy as np
-
-# plot options
 from matplotlib import rc
 rc('font',**{'family':'serif','serif':['Cardo']})
 rc('text', usetex=True)
@@ -33,89 +32,35 @@ def plot_rotation_error(timestamps, rotation_error, results_dir):
     fig.tight_layout()
     fig.savefig(results_dir+'/orientation_error.pdf')
 
-def analyse_trajectory(results_dir):
+def analyse_trajectory(results_dir, n_align_frames = 20, use_hand_eye_calib = True):
-  # plot translation error
-  data = np.loadtxt(os.path.join(results_dir, 'translation_error.txt'))
-  timestamps = data[:,0]
-  translation_error = data[:,1:4]
-  plot_translation_error(timestamps, translation_error, results_dir)
   
-  # plot orientation error
+    if not os.path.isfile(os.path.join(results_dir, 'translation_error.txt')):
-  data = np.loadtxt(os.path.join(results_dir, 'orientation_error.txt'))
-  timestamps = data[:,0]
-  orientation_error = data[:,1:4]
-  plot_rotation_error(timestamps, orientation_error, results_dir)
                 
-def get_rigid_body_trafo(quat,trans):
+        T_cm, T_mc = load_hand_eye_calib(results_dir)
-  T = transformations.quaternion_matrix(quat)
+        print('loading datasets.')        
-  T[0:3,3] = trans
+        t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir)
-  return T
-
-if __name__ == '__main__':
-  
-  # config
-  dataset='20131003_1937_nslam_i7_asl2_fast'
-  n_align_frames = 100
-  save = True
-  results_dir = '../results/'+dataset
-  
-  # 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 = get_rigid_body_trafo(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]):
+        print('align Sim3 using '+str(n_align_frames)+' first frames.')
-    raise NameError('n_matches_for_scale is too large')
+        scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_frames,:], p_es[0:n_align_frames,:])
-  scale,rot,trans = align_trajectory.align_sim3(p_gt, p_es, n_align_frames)
         print 'scale = '+str(scale)
   
         # get trafo between (v)ision and (o)ptitrack frame
+        p_es_aligned = np.zeros(np.shape(p_es))
+        if use_hand_eye_calib:
+            print q_gt[0,:]
+            print p_gt[0,:]
             T_om = get_rigid_body_trafo(q_gt[0,:], p_gt[0,:])
             T_vc = get_rigid_body_trafo(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))
+            # apply transformation to estimated trajectory
             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))       
-  
+            for i in range(np.shape(p_es)[0]):
-  # rotate trajectory
-  for i in range(np.shape(matches)[0]):
                 T_vc = get_rigid_body_trafo(q_es[i,:],p_es[i,:])
                 T_vc[0:3,3] *= scale
                 T_om = np.dot(T_ov, np.dot(T_vc, T_cm))
@@ -124,41 +69,14 @@ if __name__ == '__main__':
                 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)
             plot_translation_error(t_es, translation_error, results_dir)
       
-  if save:
-    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)
             plot_rotation_error(t_es, orientation_error, results_dir)
     
-  if save:  
-    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)
@@ -170,10 +88,93 @@ if __name__ == '__main__':
             ax.plot(t_es, scale_drift, 'b-')
             fig.tight_layout()
         
-  if save:
+        else:
-    fig.savefig(results_dir+'/scale_drift.pdf')
+            for i in range(np.shape(p_es)[0]):
-    f = open(scale_drift_filename,'w')
+                p_es_aligned[i,:] = scale*rot.dot(p_es[i,:]) + trans
-    for i in range(orientation_error.shape[0]):
-      f.write('%.7f %.5f\n'%(t_es[i], scale_drift[i]))
-    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')
+        ax.legend()
+        fig.tight_layout()
+        fig.savefig(results_dir+'/trajectory.pdf')
+      
+
+
+    else:
+        data = np.loadtxt(os.path.join(results_dir, 'translation_error.txt'))
+        timestamps = data[:,0]
+        translation_error = data[:,1:4]
+        plot_translation_error(timestamps, translation_error, results_dir)
+          
+        # plot orientation error
+        data = np.loadtxt(os.path.join(results_dir, 'orientation_error.txt'))
+        timestamps = data[:,0]
+        orientation_error = data[:,1:4]
+        plot_rotation_error(timestamps, orientation_error, results_dir)
+  
+
+def get_rigid_body_trafo(quat,trans):
+    T = transformations.quaternion_matrix(quat)
+    T[0:3,3] = trans
+    return T
+    
+def load_hand_eye_calib(results_dir):
+    P = yaml.load(open(os.path.join(results_dir, 'dataset_params.yaml'),'r'))['hand_eye_calib']
+    
+    T_cm_quat = np.array([P['Tcm_qx'], P['Tcm_qy'], P['Tcm_qz'], P['Tcm_qw']])
+    T_cm_tran = np.array([P['Tcm_tx'], P['Tcm_ty'], P['Tcm_tz']])
+    T_cm = get_rigid_body_trafo(T_cm_quat, T_cm_tran)
+    T_mc = transformations.inverse_matrix(T_cm)
+    return T_cm, T_mc
+  
+def load_dataset(results_dir):
+    """
+    Assumes that groundtruth measurements are already matched with the
+    images. 
+    """
+
+    data_gt = open(os.path.join(results_dir, 'groundtruth_matched.txt')).read()
+    lines = data_gt.replace(","," ").replace("\t"," ").split("\n") 
+    data_gt = np.array([[np.float(v.strip()) for i,v in enumerate(line.split(" ")) if i != 1 and v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"])
+    data_gt = [(float(l[0]),l[1:]) for l in data_gt]
+    data_gt = dict(data_gt)    
+    
+    data_es = open(os.path.join(results_dir, 'traj_estimate.txt')).read()
+    lines = data_es.replace(","," ").replace("\t"," ").split("\n") 
+    data_es = np.array([[np.float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"])
+    data_es = [(float(l[0]),l[1:]) for l in data_es]
+    data_es = dict(data_es)
+    
+    matches = associate.associate(data_gt, data_es, 0.0, 0.0001)
+    p_gt = np.array([[np.float(value) for value in data_gt[a][0:3]] for a,b in matches])
+    q_gt = np.array([[np.float(value) for value in data_gt[a][3:7]] for a,b in matches])
+    p_es = np.array([[np.float(value) for value in data_es[b][0:3]] for a,b in matches])
+    q_es = np.array([[np.float(value) for value in data_es[b][3:7]] for a,b in matches])
+    t_gt = np.array([np.float(a) for a,b in matches])
+    t_es = np.array([np.float(b) for a,b in matches])
+  
+    # set start time to zero
+    start_time = min(t_es[0], t_gt[0])
+    t_es -= start_time
+    t_gt -= start_time
+    return t_es, p_es, q_es, t_gt, p_gt, q_gt
+  
+if __name__ == '__main__':
+  
+    # parse command line
+    parser = argparse.ArgumentParser(description='''
+    Analyse trajectory
+    ''')
+    parser.add_argument('results_dir', help='folder with the results')
+    parser.add_argument('--use_hand_eye_calib', help='', action='store_true')
+    parser.add_argument('--n_align_frames', help='', default=20)
+    args = parser.parse_args()
+    
+    print('analyse trajectory for dataset: '+str(args.results_dir))
+    analyse_trajectory(args.results_dir,
+                       n_align_frames = int(args.n_align_frames),
+                       use_hand_eye_calib = args.use_hand_eye_calib)