init GPU

CPU_version/svo_analysis/.gitignore

@@ -0,0 +1 @@
+*.pyc

CPU_version/svo_analysis/CMakeLists.txt

@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(svo_analysis)
+
+find_package(catkin REQUIRED COMPONENTS rospy)
+include_directories(${catkin_INCLUDE_DIRS})
+
+catkin_python_setup()
+#catkin_package(CATKIN_DEPENDS sensor_msgs)

CPU_version/svo_analysis/README.md

@@ -0,0 +1,27 @@
+## SVO Analysis
+
+#### Download Test Data
+
+Create a folder where you will store all datasets, e.g.:
+
+    mkdir Datasets
+
+In your `.bashrc` script, create a new environment variable pointing to the dataset folder:
+
+    export SVO_DATASET_DIR=${HOME}/Datasets
+
+Source your new `.bashrc` script, go to the new dataset folder and download the test data:
+
+    source ~/.bashrc
+    cd ${SVO_DATASET_DIR}
+    wget http://rpg.ifi.uzh.ch/datasets/sin2_tex2_h1_v8_d.tar.gz -O - | tar -xz
+
+#### Run Benchmark
+
+In the _CMakeLists.txt_ of SVO you need to set the `TRACE` flag to `TRUE` and recompile SVO.
+Afterwards run
+
+    rosrun svo_analysis benchmark.py sin2_tex2_h1_v8_d
+
+The script will create a new folder in `svo_analysis/results/`, run svo on the dataset and save the tracefile in this folder and at the end it will run some scripts to generate plots from the tracefile.
+  

CPU_version/svo_analysis/comparisons/speed.yaml

@@ -0,0 +1,14 @@
+comparison_name: speed
+experiment_sets:
+- label: 'old'
+  experiments:
+  - 20140415_1028_svo_ros_synthetic_speed_1404150920_plane_straight_speed4
+  - 20140415_1028_svo_ros_synthetic_speed_1404150928_plane_straight_speed5
+  - 20140415_1028_svo_ros_synthetic_speed_1404150935_plane_straight_speed6
+  - 20140415_1028_svo_ros_synthetic_speed_1404150940_plane_straight_speed7
+- label: 'new'
+  experiments:
+  - 20140513_0949_svo_ros_synthetic_speed_1404150920_plane_straight_speed4
+  - 20140513_0949_svo_ros_synthetic_speed_1404150928_plane_straight_speed5
+  - 20140513_0949_svo_ros_synthetic_speed_1404150935_plane_straight_speed6
+  - 20140513_0949_svo_ros_synthetic_speed_1404150940_plane_straight_speed7

CPU_version/svo_analysis/comparisons/textures.yaml

@@ -0,0 +1,22 @@
+comparison_name: textures
+experiment_sets:
+- label: 'T1'
+  experiments:
+  - 20140516_1449_svo_ros_synthetic_all_str_tex1_h1_v6_d
+  - 20140516_1449_svo_ros_synthetic_all_str_tex1_h1_v9_d
+  - 20140516_1449_svo_ros_synthetic_all_str_tex1_h1_v12_d
+- label: 'T2'
+  experiments:
+  - 20140516_1449_svo_ros_synthetic_all_str_tex2_h1_v6_d
+  - 20140516_1449_svo_ros_synthetic_all_str_tex2_h1_v9_d
+  - 20140516_1449_svo_ros_synthetic_all_str_tex2_h1_v12_d
+- label: 'T3'
+  experiments:
+  - 20140516_1449_svo_ros_synthetic_all_str_tex3_h1_v6_d
+  - 20140516_1449_svo_ros_synthetic_all_str_tex3_h1_v9_d
+  - 20140516_1449_svo_ros_synthetic_all_str_tex3_h1_v12_d
+# - label: 'T4'
+#   experiments:
+#   - 20140516_1449_svo_ros_synthetic_all_str_tex4_h1_v6_d
+#   - 20140516_1449_svo_ros_synthetic_all_str_tex4_h1_v9_d
+#   - 20140516_1449_svo_ros_synthetic_all_str_tex4_h1_v12_d

CPU_version/svo_analysis/experiments/.gitignore

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

CPU_version/svo_analysis/package.xml

@@ -0,0 +1,19 @@
+<package>
+
+  <name>svo_analysis</name>
+  <version>0.0.1</version>
+  <description>
+    Python scripts to analyse performance of SVO.
+  </description>
+  <maintainer email="[email protected]">Christian Forster</maintainer>
+  <author>Christian Forster</author>
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rospy</build_depend>
+
+  <run_depend>python-rospkg</run_depend>
+  <run_depend>python-yaml</run_depend>
+  <run_depend>rospy</run_depend>
+
+</package>

CPU_version/svo_analysis/results/.gitignore

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

CPU_version/svo_analysis/scripts/benchmark.py

@@ -0,0 +1,111 @@
+#!/usr/bin/python
+"""
+Created on Sat Aug 10 18:21:47 2013
+
+@author: Christian Forster
+"""
+
+import os
+import yaml
+import rospkg
+import argparse
+import time
+import vikit_py.cpu_info as cpu_info
+import vikit_py.ros_node as ros_node
+import evaluate
+import shutil
+
+def run_experiment(dataset, params):
+    # load dataset parameters
+    params['dataset_directory'] = os.path.join(os.environ['SVO_DATASET_DIR'], dataset)
+    if not os.path.exists(params['dataset_directory']):
+        raise Exception("Provided dataset folder does not exist.")
+    dataset_params_file = os.path.join(params['dataset_directory'], 'dataset_params.yaml')
+    dataset_params = yaml.load(open(dataset_params_file,'r'))
+      
+    # load algorithm parameters
+    algo_params_file = os.path.join(rospkg.RosPack().get_path('svo_ros'), 
+                                   'param', params['param_settings']+'.yaml')
+    algo_params = yaml.load(open(algo_params_file,'r'))
+    
+    # combine all parameters
+    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']
+    else:
+        params['dataset_is_blender'] = False
+    if 'dataset_first_frame' in dataset_params:
+        params['dataset_first_frame'] = dataset_params['dataset_first_frame']
+    else:
+        params['dataset_first_frame'] = 0
+    
+    # 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'))
+    
+    # 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_nominal.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)
+    node.run(params)
+    
+if __name__=="__main__":
+  
+    # parse command line
+    parser = argparse.ArgumentParser(description='''
+    Runs SVO with the dataset and parameters specified in the provided experiment file.
+    ''')
+    parser.add_argument('experiment_file', help='experiment file in svo_analysis/experiments folder')
+    parser.add_argument('--evaluate', help='evaluate tracefile after running SVO', action='store_true')
+    parser.add_argument('--version', help='version of svo to evaluate', default='svo_ros')
+    parser.add_argument('--executable', help='the executable to be called', default='benchmark')
+    args = parser.parse_args()
+    
+    # load experiment parameters
+    args.experiment_file = args.experiment_file.replace('.yaml','')
+    experiment_params_file = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 
+                               'experiments', args.experiment_file+'.yaml')
+    if os.path.exists(experiment_params_file):
+        experiment_params = yaml.load(open(experiment_params_file, 'r'))
+    else:
+        print('experiment file does not exist. run with default setttings')
+        experiment_params = dict({'experiment_label':'default','param_settings':'vo_fast',
+                                  'datasets':list([args.experiment_file])})
+    params = dict()
+    params['experiment_label'] = experiment_params['experiment_label']
+    params['param_settings'] = experiment_params['param_settings']
+    params['time'] = time.strftime("%Y%m%d_%H%M", time.localtime())
+    params['platform'] = cpu_info.get_cpu_info()
+    params['trace_name'] = 'trace'
+    
+    trace_dirs = list()
+    for dataset in experiment_params['datasets']:
+        params['experiment_name'] = params['time']+'_'+args.version+'_'+args.experiment_file+'_'+dataset
+        params['trace_dir'] = os.path.join(rospkg.RosPack().get_path('svo_analysis'),
+                                           'results', params['experiment_name'])
+        if not os.path.exists(params['trace_dir']):
+            os.makedirs(params['trace_dir'])
+        trace_dirs.append(params['trace_dir'])
+        run_experiment(dataset, params)
+        
+    # TODO: check if it is a synthetic dataset 
+    for trace in trace_dirs:
+        evaluate.evaluate_dataset(trace)
+      
+      
+  
+  
+  
+  

CPU_version/svo_analysis/scripts/compare_results.py

@@ -0,0 +1,150 @@
+#!/usr/bin/python
+
+import os
+import sys
+import time
+import rospkg
+import numpy as np
+import matplotlib.pyplot as plt
+import yaml
+import argparse
+from matplotlib import rc
+
+# tell matplotlib to use latex font
+rc('font',**{'family':'serif','serif':['Cardo']})
+rc('text', usetex=True)    
+  
+from mpl_toolkits.axes_grid1.inset_locator import zoomed_inset_axes
+from mpl_toolkits.axes_grid1.inset_locator import mark_inset
+
+def plot_trajectory(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)
+
+def compare_results(experiments, results_dir, comparison_dir, 
+                    plot_scale_drift = False):
+  
+  # ------------------------------------------------------------------------------
+  # 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(os.path.join(results_dir, exp, 'params.yaml'))
+    params = yaml.load(params_stream)
+  
+    # plot translation error
+    trans_error = np.loadtxt(os.path.join(results_dir, exp, 'translation_error.txt'))
+    trans_error[:,0] = trans_error[:,0]-trans_error[0,0]
+    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(os.path.join(comparison_dir, '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(os.path.join(results_dir, exp, 'params.yaml'))
+    params = yaml.load(params_stream)
+  
+    # plot translation error
+    rot_error = np.loadtxt(os.path.join(results_dir, exp, 'orientation_error.txt'))
+    rot_error[:,0] = rot_error[:,0]-rot_error[0,0]
+    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(os.path.join(comparison_dir, 'orientation_error.pdf'))
+  
+  # ------------------------------------------------------------------------------
+  # scale error 
+  if plot_scale_drift:
+    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 = yaml.load(open(os.path.join(results_dir, exp, 'params.yaml')))
+    
+      # plot translation error
+      scale_drift = open(os.path.join(results_dir, exp, 'scale_drift.txt'))
+      scale_drift[:,0] = scale_drift[:,0]-scale_drift[0,0]
+      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(os.path.join(comparison_dir, '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)
+# 
+#   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')
+  
+  
+if __name__ == '__main__':
+  
+  default_name = time.strftime("%Y%m%d_%H%M", time.localtime())+'_comparison'
+  parser = argparse.ArgumentParser(description='Compare results.')
+  parser.add_argument('result_directories', nargs='+', help='list of result directories to compare')
+  parser.add_argument('--name', help='name of the comparison', default=default_name)
+  args = parser.parse_args()
+ 
+  # create folder for comparison results
+  results_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 'results')
+  comparison_dir = os.path.join(results_dir, args.name)
+  if not os.path.exists(comparison_dir):
+    os.makedirs(comparison_dir)
+    
+  # run comparison
+  compare_results(args.result_directories, results_dir, comparison_dir) 
+  
+ 
+                 
+           

CPU_version/svo_analysis/scripts/comparison.py

@@ -0,0 +1,159 @@
+#!/usr/bin/python
+
+import os
+import sys
+import time
+import rospkg
+import numpy as np
+import matplotlib.pyplot as plt
+import yaml
+import argparse
+import svo_analysis.analyse_depth as analyse_depth
+from matplotlib import rc
+
+# tell matplotlib to use latex font
+rc('font',**{'family':'serif','serif':['Cardo']})
+rc('text', usetex=True)    
+  
+from mpl_toolkits.axes_grid1.inset_locator import zoomed_inset_axes
+from mpl_toolkits.axes_grid1.inset_locator import mark_inset
+
+def plot_trajectory(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)
+
+def save_figure(fig, name, directory):
+    fig.tight_layout()
+    fig.savefig(os.path.join(directory, name+'.pdf'), bbox_inches="tight")
+
+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 get_distance_from_start(gt):
+    distances = np.diff(gt[:,1:4],axis=0)
+    distances = np.sqrt(np.sum(np.multiply(distances,distances),1))
+    distances = np.cumsum(distances)
+    distances = np.concatenate(([0], distances))
+    return distances
+    
+def compare_results(comp_params, results_dir, comparison_dir):
+    print('run comparison: '+comp_params['comparison_name'])
+           
+    line_styles = ['-','--',':']
+    line_colors = ['b','g','r','m','c']
+    
+    # -------------------------------------------------------------------------
+    # plot trajectory:
+    fig = plt.figure(figsize=(6,2))
+    ax = fig.add_subplot(111, xlabel='x [m]', ylabel='y [m]')
+    for exp_set in comp_params['experiment_sets']:
+        print('processing experiment set: ' + exp_set['label'])
+        
+        for i, exp in enumerate(exp_set['experiments']):
+            data = np.loadtxt(os.path.join(results_dir, exp, 'traj_estimate.txt'))
+            if i == 0:
+                base_plot, = ax.plot(data[:,1], data[:,2], label=exp_set['label'], linestyle=line_styles[np.mod(i, len(line_styles))])
+            else:
+                ax.plot(data[:,1], data[:,2], color=base_plot.get_color(), linestyle= line_styles[np.mod(i, len(line_styles))])
+    ax.legend(loc='upper left', ncol=3)
+    save_figure(fig, 'trajectory', comparison_dir)
+    
+    # -------------------------------------------------------------------------
+    # plot translation error:
+    fig = plt.figure(figsize=(6,2))
+    ax = fig.add_subplot(111, xlabel='distance [m]', ylabel='translation drift [mm]')
+    for exp_set in comp_params['experiment_sets']:
+        print('processing experiment set: ' + exp_set['label'])
+        
+        for i, exp in enumerate(exp_set['experiments']):
+            gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt'))
+            distances = get_distance_from_start(gt)
+            data = np.loadtxt(os.path.join(results_dir, exp, 'translation_error.txt'))
+            e = np.sqrt(np.sum(np.multiply(data[:,1:4],data[:,1:4]),1))
+            
+            if np.shape(e)[0] > np.shape(distances)[0]:
+                print('WARNING: estimate has more measurement than groundtruth: '
+                      +str(np.shape(e)[0]-np.shape(distances)[0]))
+                e = e[0:np.shape(distances)[0]]
+                
+            distances = distances[0:np.shape(e)[0]]
+            
+            print '--'
+            print np.shape(e)
+            print np.shape(distances)
+            
+            if i == 0:
+                base_plot, = ax.plot(distances, e*1000, label=exp_set['label'], linestyle= line_styles[np.mod(i, len(line_styles))])
+            else:
+                ax.plot(distances, e*1000, color=base_plot.get_color(), linestyle= line_styles[np.mod(i, len(line_styles))])
+                
+    ax.legend(loc='upper left', ncol=3)
+    save_figure(fig, 'translation_error', comparison_dir)
+    
+    # -------------------------------------------------------------------------
+    # plot depth estimation error:
+    fig = plt.figure(figsize=(6,5))
+    ax = fig.add_subplot(111, xlabel='Travelled distance [m]', ylabel='Error [m]')
+    for k, exp_set in enumerate(comp_params['experiment_sets']):
+        print('plot depth error for experiment: ' + exp)
+        exp = exp_set['experiments'][0]
+        gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt'))
+        x_axis_data = get_distance_from_start(gt)
+        analyse_depth.plot_depth_over_time(os.path.join(results_dir, exp), ax,
+                                           x_axis_data[1:], line_colors[k], exp_set['label'])
+        ax.legend(loc='upper left', ncol=3)
+    save_figure(fig, 'depth_error_textures', comparison_dir)
+    
+    fig = plt.figure(figsize=(6,5))
+    ax = fig.add_subplot(111, xlabel='Travelled distance [m]', ylabel='Error [m]')
+    exp_set = comp_params['experiment_sets'][0]
+    for i, exp in enumerate(exp_set['experiments']):
+        print('plot depth error for speed: ' + exp)
+        gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt'))
+        x_axis_data = get_distance_from_start(gt)
+        params = yaml.load(open(os.path.join(results_dir, exp, 'dataset_params.yaml')))
+        analyse_depth.plot_depth_over_time(os.path.join(results_dir, exp), ax,
+                                           x_axis_data[1:], line_colors[i],
+                                           str(params['trajectory_modifiers']['speed'])+' m/s')
+    ax.legend(loc='upper left', ncol=3)
+    save_figure(fig, 'depth_error_speed', comparison_dir)
+  
+if __name__ == '__main__':
+  
+    parser = argparse.ArgumentParser(description='Compare results of a VO pipeline.')
+    parser.add_argument('comparison_file', help='A YAML file that contains the details of the comparison')
+    args = parser.parse_args()
+     
+    # load comparison file
+    args.experiment_file = args.comparison_file.replace('.yaml','')
+    comparison_params_file = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 
+                                          'comparisons', args.comparison_file+'.yaml')
+    if os.path.exists(comparison_params_file):
+        comp_params = yaml.load(open(comparison_params_file, 'r'))
+    else:
+        raise Exception("Provided comparison file does not exist.")
+        
+    # create folder for comparison results
+    comparison_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 
+                                  'comparisons', comp_params['comparison_name'])
+    if not os.path.exists(comparison_dir):
+        os.makedirs(comparison_dir)
+
+    # folder where the results of previous experiments are saved    
+    results_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 'results')
+    
+    # run comparison
+    compare_results(comp_params, results_dir, comparison_dir) 
+                
+           

CPU_version/svo_analysis/scripts/evaluate.py

@@ -0,0 +1,58 @@
+#!/usr/bin/python
+"""
+Created on Sat Aug 10 18:21:47 2013
+
+@author: Christian Forster
+"""
+
+import os
+import csv
+import numpy as np
+import rospkg
+import argparse
+import yaml
+import svo_analysis.analyse_logs as analyse_logs
+import svo_analysis.analyse_timing as analyse_timing
+import svo_analysis.analyse_trajectory as analyse_trajectory
+import svo_analysis.analyse_depth as analyse_depth
+
+def evaluate_dataset(trace_dir):
+  
+    # read tracefile
+    data = csv.reader(open(os.path.join(trace_dir, 'trace.csv')))
+    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])
+
+    # generate plots
+    analyse_logs.analyse_logs(D, trace_dir)
+    analyse_timing.analyse_timing(D, trace_dir)
+    analyse_trajectory.analyse_trajectory(trace_dir)
+
+    param = yaml.load(open(os.path.join(trace_dir, 'dataset_params.yaml'), 'r'))
+    if param['dataset_is_blender']:
+        analyse_depth.analyse_depth(trace_dir)
+        analyse_depth.analyse_depth_over_time(trace_dir)
+
+
+if __name__=="__main__":
+    parser = argparse.ArgumentParser(description='''
+    Evaluates tracefiles of SVO and generates the plots.
+    ''')
+    parser.add_argument('experiment_name', help='directory name of the tracefiles')
+    args = parser.parse_args()
+
+    trace_dir = os.path.join(rospkg.RosPack().get_path('svo_analysis'), 
+                             'results',
+                             args.experiment_name)
+    evaluate_dataset(trace_dir)

CPU_version/svo_analysis/setup.py

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['svo_analysis'],
+    package_dir={'': 'src'},
+    install_requires=['rospkg', 'yaml'],
+    )
+
+setup(**d)

CPU_version/svo_analysis/src/svo_analysis/analyse_dataset.py

@@ -0,0 +1,41 @@
+# -*- coding: utf-8 -*-
+
+import associate
+import numpy as np
+import matplotlib.pyplot as plt
+import yaml
+
+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'
+
+  

CPU_version/svo_analysis/src/svo_analysis/analyse_depth.py

@@ -0,0 +1,66 @@
+#!/usr/bin/python
+
+import os
+import argparse
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import rc
+
+rc('font',**{'family':'serif','serif':['Cardo']})
+rc('text', usetex=True)
+
+def precision_plot(ax, errors, max_error, color, linestyle):
+    number_of_error_ranges = 500
+    error_step = max_error / number_of_error_ranges
+    precision_array = np.zeros(number_of_error_ranges)
+    error_array = np.zeros(number_of_error_ranges)
+    n_measurements = len(errors)
+    for error_ind in range(0, number_of_error_ranges):
+        precision_array[error_ind] = np.sum(errors < error_step*error_ind) / float(n_measurements) * 100.0
+        error_array[error_ind] = error_step*error_ind
+    plot_obj, = ax.plot(error_array, precision_array, label='error', linestyle=linestyle, color=color)
+    return plot_obj
+    
+def analyse_depth(results_dir):
+    D = np.loadtxt(os.path.join(results_dir, 'depth_error.txt'), delimiter=' ')
+    D = D[D[:,0] > 1,:]
+
+    fig = plt.figure(figsize=(6,5))
+    ax = fig.add_subplot(111, xlabel='depth error [m]', ylabel='Precision [\%]')
+    errors = np.abs(D[:,1])
+    precision_plot(ax, errors, 0.05, 'r', '-')    
+    fig.tight_layout()
+    fig.savefig(os.path.join(results_dir,'depth_error.pdf'), bbox_inches="tight")
+    
+
+def plot_depth_over_time(results_dir, ax, x_axis_data, color, label=''):
+    D = np.loadtxt(os.path.join(results_dir, 'depth_error.txt'), delimiter=' ')
+    D = D[D[:,0] > 1,:]
+    idxs = np.unique(D[:,0])
+    percentile_10 = np.zeros(len(idxs))
+    percentile_50 = np.zeros(len(idxs))
+    percentile_90 = np.zeros(len(idxs))
+    for i in range(len(idxs)):
+        errors = np.abs(D[D[:,0]==idxs[i],1])
+        percentile_10[i] = np.percentile(errors, 10)
+        percentile_50[i] = np.percentile(errors, 50)
+        percentile_90[i] = np.percentile(errors, 90)
+    if len(x_axis_data) == 0:
+        x_axis_data = idxs
+    print np.shape(x_axis_data)
+    print np.shape(idxs)
+    print '--'
+    ax.plot(x_axis_data, percentile_50, linewidth=2, color=color, label=label)
+    ax.plot(x_axis_data, percentile_10, linewidth=0.5, color=color, alpha=0.5)
+    ax.plot(x_axis_data, percentile_90, linewidth=0.5, color=color, alpha=0.5)
+    ax.fill_between(x_axis_data, percentile_10, percentile_90, color=color, alpha=0.1)
+     
+if __name__ == '__main__':
+    parser = argparse.ArgumentParser(description='Analyse depth estimate')
+    parser.add_argument('results_dir', help='folder with the results')
+    args = parser.parse_args()
+    analyse_depth(args.results_dir)
+    
+    fig = plt.figure(figsize=(6,5))
+    ax = fig.add_subplot(111, xlabel='Measurement', ylabel='Scale-Drift')
+    plot_depth_over_time(args.results_dir, [], ax)

CPU_version/svo_analysis/src/svo_analysis/analyse_logs.py

@@ -0,0 +1,74 @@
+#!/usr/bin/python
+
+import os
+import yaml
+import numpy as np
+import matplotlib.pyplot as plt
+  
+def analyse_logs(D, trace_dir):
+
+  # 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))
+   
+  # set initial time to zero
+  D['timestamp'] = D['timestamp'] - D['timestamp'][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]);
+  mean_n_edges_final = np.mean(D['sfba_n_edges_final'][is_frame]);
+
+  fig = plt.figure(figsize=(8,3))
+  ax = fig.add_subplot(111, xlabel='time [s]')
+  ax.plot(D['timestamp'][is_frame], D['repr_n_mps'][is_frame], 'r-',
+          label='Reprojected Points, avg = %.2f'%mean_n_reproj_points)
+  ax.plot(D['timestamp'][is_frame], D['repr_n_new_references'][is_frame], 'b-', 
+          label='Feature Matches, avg = %.2f'%mean_n_reproj_matches)
+  ax.plot(D['timestamp'][is_frame], D['sfba_n_edges_final'][is_frame], 'g-', 
+          label='Points after Optimization, avg = %.2f'%mean_n_edges_final)
+  ax.set_ylim(bottom=0)
+  ax.legend(loc='lower right')
+  fig.tight_layout()
+  fig.savefig(os.path.join(trace_dir,'num_reprojected.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]')
+  ax.plot(D['timestamp'][is_frame], D['sfba_error_init'][is_frame], 'r-', label='Initial error')
+  ax.plot(D['timestamp'][is_frame], D['sfba_error_final'][is_frame], 'b-', label='Final error')
+  ax.legend(ncol=2)  
+  fig.tight_layout()
+  fig.savefig(os.path.join(trace_dir,'reprojection_error.pdf'), bbox_inches="tight")
+  print 'average reprojection error improvement: ' + str(init_error_avg - opt1_avg)
+
+  # ----------------------------------------------------------------------------
+  # plot number of candidate points
+  fig = plt.figure(figsize=(8,3))
+  ax = fig.add_subplot(111, xlabel='time [s]')
+  ax.plot(D['timestamp'][is_frame], D['n_candidates'][is_frame], 'r-', label='Candidate Points')
+  fig.tight_layout()
+  fig.savefig(os.path.join(trace_dir,'candidate_points.pdf'), bbox_inches="tight")
+  
+  # ----------------------------------------------------------------------------
+  # plot number of candidate points
+  fig = plt.figure(figsize=(8,2))
+  ax = fig.add_subplot(111, xlabel='time [s]', ylabel='px')
+  ax.plot(D['timestamp'][is_frame], D['sfba_thresh'][is_frame], 'r-', label='Threshold')
+  fig.tight_layout()
+  fig.savefig(os.path.join(trace_dir,'optimization_thresh.pdf'), bbox_inches="tight")
+  
+  # ----------------------------------------------------------------------------
+  # write other statistics to file
+  stat = {'num_frames': len(is_frame), 
+          'num_kfs': len(is_kf),
+          'reproj_error_avg_improvement': float(init_error_avg - opt1_avg)}
+  with open(os.path.join(trace_dir,'dataset_stats.yaml'),'w') as outfile:          
+    outfile.write(yaml.dump(stat, default_flow_style=False))
+

CPU_version/svo_analysis/src/svo_analysis/analyse_timing.py

@@ -0,0 +1,68 @@
+#!/usr/bin/python
+
+import os
+import numpy as np
+import matplotlib.pyplot as plt
+
+def analyse_timing(D, trace_dir):
+
+  # 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)
+  
+  # set initial time to zero
+  D['timestamp'] = D['timestamp'] - D['timestamp'][0]
+
+  # ----------------------------------------------------------------------------
+  # plot total time for frame processing
+  avg_time = np.mean(D['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(D['timestamp'][is_frame], D['tot_time'][is_frame]*1000, 'g-', label='total time [ms]')
+  ax.plot(D['timestamp'][is_frame], np.ones(n_frames)*avg_time, 'b--', label=str('%(time).1fms mean time' % {'time': avg_time}))
+  ax.legend()
+  fig.tight_layout()
+  fig.savefig(os.path.join(trace_dir,'timing.pdf'), bbox_inches="tight")
+  
+  # ----------------------------------------------------------------------------
+  # plot boxplot
+  fig = plt.figure(figsize=(6,2))
+  ax = fig.add_subplot(111, xlabel='Processing time [ms]')
+  ax.boxplot([ 
+                D['tot_time'][is_frame]*1000,
+#                D['t_local_ba'][is_kf]*1000,
+                D['pose_optimizer'][is_frame]*1000 + D['point_optimizer'][is_frame]*1000,
+                D['reproject'][is_frame]*1000,
+                D['sparse_img_align'][is_frame]*1000,
+                D['pyramid_creation'][is_frame]*1000
+              ], 0,'', vert=0) 
+               
+  boxplot_labels = [
+                    r'\textbf{Total Motion Estimation: %2.2fms}' % np.median(D['tot_time'][is_frame]*1000),
+#                    'Local BA (KF only): %.2fms ' % np.median(D['local_ba'][is_kf]*1000),
+                    'Refinement: %2.2fms' % np.median(D['pose_optimizer'][is_frame]*1000 + D['point_optimizer'][is_frame]*1000),
+                    'Feature Alignment: %2.2fms' % np.median(D['reproject'][is_frame]*1000),
+                    'Sparse Image Alignment: %2.2fms' % np.median(D['sparse_img_align'][is_frame]*1000),
+                    'Pyramid Creation: %2.2fms' % np.median(D['pyramid_creation'][is_frame]*1000) ]
+                   
+  ax.set_yticks(np.arange(len(boxplot_labels))+1)
+  ax.set_yticklabels(boxplot_labels)
+  fig.tight_layout()
+  fig.savefig(os.path.join(trace_dir,'timing_boxplot.pdf'), bbox_inches="tight")
+  
+  # ----------------------------------------------------------------------------
+  # plot boxplot reprojection
+  fig = plt.figure(figsize=(6,2))
+  ax = fig.add_subplot(111, xlabel='Processing time [ms]')
+  ax.boxplot([ D['reproject'][is_frame]*1000,
+               D['feature_align'][is_frame]*1000,
+               D['reproject_candidates'][is_frame]*1000,
+               D['reproject_kfs'][is_frame]*1000 ], 0, '', vert=0) 
+  boxplot_labels = [r'\textbf{Total Reprojection: %2.2fms}' % np.median(D['reproject'][is_frame]*1000),
+                     'Feature Alignment: %2.2fms' % np.median(D['feature_align'][is_frame]*1000),
+                     'Reproject Candidates: %2.2fms' % np.median(D['reproject_candidates'][is_frame]*1000),
+                     'Reproject Keyframes: %2.2fms' % np.median(D['reproject_kfs'][is_frame]*1000) ]             
+  ax.set_yticks(np.arange(len(boxplot_labels))+1)
+  ax.set_yticklabels(boxplot_labels)
+  fig.tight_layout()
+  fig.savefig(os.path.join(trace_dir,'timing_reprojection.pdf'), bbox_inches="tight")

CPU_version/svo_analysis/src/svo_analysis/analyse_trajectory.py

@@ -0,0 +1,185 @@
+#!/usr/bin/python
+
+import os
+import yaml
+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
+from matplotlib import rc
+rc('font',**{'family':'serif','serif':['Cardo']})
+rc('text', usetex=True)
+
+def plot_translation_error(timestamps, translation_error, results_dir):
+    fig = plt.figure(figsize=(8, 2.5))
+    ax = fig.add_subplot(111, xlabel='time [s]', ylabel='position drift [mm]', xlim=[0,timestamps[-1]-timestamps[0]+4])
+    ax.plot(timestamps-timestamps[0], translation_error[:,0]*1000, 'r-', label='x')
+    ax.plot(timestamps-timestamps[0], translation_error[:,1]*1000, 'g-', label='y')
+    ax.plot(timestamps-timestamps[0], translation_error[:,2]*1000, 'b-', label='z')
+    ax.legend()
+    fig.tight_layout()
+    fig.savefig(results_dir+'/translation_error.pdf')
+
+def plot_rotation_error(timestamps, rotation_error, results_dir):
+    fig = plt.figure(figsize=(8, 2.5))
+    ax = fig.add_subplot(111, xlabel='time [s]', ylabel='orientation drift [rad]', xlim=[0,timestamps[-1]-timestamps[0]+4])
+    ax.plot(timestamps-timestamps[0], rotation_error[:,0], 'r-', label='yaw')
+    ax.plot(timestamps-timestamps[0], rotation_error[:,1], 'g-', label='pitch')
+    ax.plot(timestamps-timestamps[0], rotation_error[:,2], 'b-', label='roll')
+    ax.legend()
+    fig.tight_layout()
+    fig.savefig(results_dir+'/orientation_error.pdf')
+
+def analyse_synthetic_trajectory(results_dir):
+    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 analyse_optitrack_trajectory_with_hand_eye_calib(results_dir, params, n_align_frames = 200):
+    print('loading hand-eye-calib')
+    T_cm_quat = np.array([params['hand_eye_calib']['Tcm_qx'],
+                          params['hand_eye_calib']['Tcm_qy'],
+                          params['hand_eye_calib']['Tcm_qz'],
+                          params['hand_eye_calib']['Tcm_qw']])
+    T_cm_tran = np.array([params['hand_eye_calib']['Tcm_tx'],
+                          params['hand_eye_calib']['Tcm_ty'],
+                          params['hand_eye_calib']['Tcm_tz']])
+    T_cm = get_rigid_body_trafo(T_cm_quat, T_cm_tran)
+    T_mc = transformations.inverse_matrix(T_cm)
+
+    t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir, params['cam_delay'])
+    
+    # align Sim3 to get scale
+    print('align Sim3 using '+str(n_align_frames)+' first frames.')
+    scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_frames,:], p_es[0:n_align_frames,:])
+    print 'scale = '+str(scale)
+  
+    # get trafo between (v)ision and (o)ptitrack frame
+    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)
+  
+    # 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))
+    p_es_aligned = np.zeros(np.shape(p_es))
+    for i in range(np.shape(p_es)[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))
+        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') 
+  
+    # plot position error (drift)
+    translation_error = (p_gt-p_es_aligned)
+    plot_translation_error(t_es, translation_error, results_dir)
+  
+    # plot orientation error (drift)
+    orientation_error = (rpy_gt - rpy_es_aligned)
+    plot_rotation_error(t_es, orientation_error, results_dir)
+
+    # 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()
+    fig.savefig(results_dir+'/scale_drift.pdf')
+    
+    # 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')
+    
+def analyse_trajectory(results_dir, n_align_frames = 200, use_hand_eye_calib = True):
+    params = yaml.load(open(os.path.join(results_dir, 'dataset_params.yaml'),'r'))
+    if params['dataset_is_blender']:
+        analyse_synthetic_trajectory(results_dir)
+    elif use_hand_eye_calib:
+        analyse_optitrack_trajectory_with_hand_eye_calib(results_dir, params, n_align_frames)
+    else:
+        t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir, params['cam_delay'])
+        scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_frames,:], p_es[0:n_align_frames,:])
+        p_es_aligned = np.zeros(np.shape(p_es))
+        for i in range(np.shape(p_es)[0]):
+            p_es_aligned[i,:] = scale*rot.dot(p_es[i,:]) + trans
+        # plot position error (drift)
+        translation_error = (p_gt-p_es_aligned)
+        plot_translation_error(t_es, translation_error, results_dir)
+        
+def get_rigid_body_trafo(quat,trans):
+    T = transformations.quaternion_matrix(quat)
+    T[0:3,3] = trans
+    return T
+     
+def load_dataset(results_dir, cam_delay):
+    print('loading dataset in '+results_dir)   
+    print('cam_delay = '+str(cam_delay))
+
+    data_gt = open(os.path.join(results_dir, 'groundtruth.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 v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"])
+    #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, -cam_delay, 0.02)
+    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=200)
+    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)

CPU_version/svo_analysis/src/svo_analysis/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()

CPU_version/svo_analysis/src/svo_analysis/hand_eye_calib.py

@@ -0,0 +1,227 @@
+# -*- coding: utf-8 -*-
+"""
+Created on Sun Aug  4 15:47:55 2013
+
+@author: cforster
+"""
+import os
+import yaml
+import numpy as np
+import svo_analysis.tum_benchmark_tools.associate as associate
+import vikit_py.align_trajectory as align_trajectory
+import vikit_py.transformations as transformations
+import matplotlib.pyplot as plt
+
+# user config
+display = True
+dataset_dir = '/home/cforster/catkin_ws/src/rpg_svo/svo_analysis/results/rpg_circle_1'
+n_measurements = 400
+n_align_sim3 = 600
+delta = 50
+
+# load dataset parameters
+params = yaml.load(open(os.path.join(dataset_dir, 'dataset_params.yaml')))
+
+# set trajectory groundtruth and estimate file
+traj_groundtruth = os.path.join(dataset_dir, 'groundtruth.txt')
+traj_estimate = os.path.join(dataset_dir,'traj_estimate.txt')
+
+# load data
+data_gt = associate.read_file_list(traj_groundtruth)
+data_es = associate.read_file_list(traj_estimate)
+
+# select matches
+offset = -params['cam_delay']
+print ('offset = '+str(offset))
+matches = associate.associate(data_gt, data_es, offset, 0.02)
+#matches = matches[500:]
+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 = align_trajectory.align_sim3(p_gt[0:n_align_sim3,:], p_es[0:n_align_sim3,:])
+
+#model_aligned = s * R * model + t
+#alignment_error = model_aligned - data
+#t_error = np.sqrt(np.sum(np.multiply(alignment_error,alignment_error),0)).A[0]
+  
+p_es_aligned = np.transpose(scale*np.dot(rot,np.transpose(p_es)))+trans
+p_es = scale*p_es
+
+             
+print 's='+str(scale)
+print 't='+str(trans)
+print 'R='+str(rot)
+
+# plot sim3 aligned trajectory
+fig = plt.figure()
+ax = fig.add_subplot(111, aspect='equal')
+ax.plot(p_es_aligned[:,0], p_es_aligned[:,1], 'r-', label='estimate', alpha=0.2)
+ax.plot(p_gt[:,0], p_gt[:,1], 'b-', label='groundtruth', alpha=0.2)
+ax.plot(p_es_aligned[:n_align_sim3,0], p_es_aligned[:n_align_sim3,1], 'g-', label='aligned', linewidth=2)
+ax.plot(p_gt[:n_align_sim3,0], p_gt[:n_align_sim3,1], 'm-', label='aligned', linewidth=2)  
+ax.legend()
+for (x1,y1,z1),(x2,y2,z2) in zip(p_es_aligned[:n_align_sim3:10],p_gt[:n_align_sim3:10]):
+    ax.plot([x1,x2],[y1,y2],'-',color="red")
+    
+fig = plt.figure()
+ax = fig.add_subplot(111)
+ax.plot(p_gt[:,0], 'r-')
+ax.plot(p_gt[:,1], 'g-')
+ax.plot(p_gt[:,2], 'b-')
+ax.plot(p_es_aligned[:,0], 'r--')
+ax.plot(p_es_aligned[:,1], 'g--')
+ax.plot(p_es_aligned[:,2], 'b--') 
+            
+
+# --------------------------------------------------------------------------------
+# hand-eye-calib
+
+# select random measurements
+I = np.array(np.random.rand(n_measurements,1)*(np.shape(matches)[0]-delta), dtype=int)[:,0]
+R,b = align_trajectory.hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta, True)
+
+print 'quat = ' + str(transformations.quaternion_from_matrix(transformations.convert_3x3_to_4x4(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 = transformations.quaternion_matrix(q_es[i-delta,:])[:3,:3]
+  A2 = transformations.quaternion_matrix(q_es[i,:])[:3,:3]
+  A  = np.dot(A1.transpose(), A2)
+  B1 = transformations.quaternion_matrix(q_gt[i-delta,:])[:3,:3]
+  B2 = transformations.quaternion_matrix(q_gt[i,:])[:3,:3]
+  B  = np.dot(B1.transpose(), B2)
+  B_es  = np.dot(np.transpose(R), np.dot(A, R))
+  rpy_gt[i-delta,:] = transformations.euler_from_matrix(B, 'rzyx')
+  rpy_es[i-delta,:] = transformations.euler_from_matrix(B_es, 'rzyx')
+  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))
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/__init__.py

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

CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py

@@ -0,0 +1,130 @@
+#!/usr/bin/python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Juergen Sturm, TUM
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of TUM nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Requirements: 
+# sudo apt-get install python-argparse
+
+"""
+The Kinect provides the color and depth images in an un-synchronized way. This means that the set of time stamps from the color images do not intersect with those of the depth images. Therefore, we need some way of associating color images to depth images.
+
+For this purpose, you can use the ''associate.py'' script. It reads the time stamps from the rgb.txt file and the depth.txt file, and joins them by finding the best matches.
+"""
+
+import argparse
+import sys
+import os
+import numpy
+
+
+def read_file_list(filename):
+    """
+    Reads a trajectory from a text file. 
+    
+    File format:
+    The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
+    and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. 
+    
+    Input:
+    filename -- File name
+    
+    Output:
+    dict -- dictionary of (stamp,data) tuples
+    
+    """
+    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):
+    """
+    Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 
+    to find the closest match for every input tuple.
+    
+    Input:
+    first_list -- first dictionary of (stamp,data) tuples
+    second_list -- second dictionary of (stamp,data) tuples
+    offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
+    max_difference -- search radius for candidate generation
+
+    Output:
+    matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))
+    
+    """
+    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='estimated trajectory (format: timestamp data)')
+    parser.add_argument('second_file', help='groundtruth (format: timestamp data)')
+    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))    
+    
+    filename = 'groundtruth_matched.txt' 
+    associates_file = open(filename, 'w')                             
+    for a,b in matches:
+      img = first_list[a]
+      pos = second_list[b]
+      associates_file.write('%.6f %s %s %s %s %s %s %s %s\n' %
+                           (a, img[0],
+                            pos[0], pos[1], pos[2],
+                            pos[3], pos[4], pos[5], pos[6])) 
+    print('Wrote matches to file: ' + filename)
+        

CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_ate.py

@@ -0,0 +1,194 @@
+#!/usr/bin/python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Juergen Sturm, TUM
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of TUM nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Requirements: 
+# sudo apt-get install python-argparse
+
+"""
+This script computes the absolute trajectory error from the ground truth
+trajectory and the estimated trajectory.
+"""
+
+import sys
+import numpy
+import argparse
+import associate
+
+def align(model,data):
+    """Align two trajectories using the method of Horn (closed-form).
+    
+    Input:
+    model -- first trajectory (3xn)
+    data -- second trajectory (3xn)
+    
+    Output:
+    rot -- rotation matrix (3x3)
+    trans -- translation vector (3x1)
+    trans_error -- translational error per point (1xn)
+    
+    """
+    numpy.set_printoptions(precision=3,suppress=True)
+    model_zerocentered = model - model.mean(1)
+    data_zerocentered = data - data.mean(1)
+    
+    W = numpy.zeros( (3,3) )
+    for column in range(model.shape[1]):
+        W += numpy.outer(model_zerocentered[:,column],data_zerocentered[:,column])
+    U,d,Vh = numpy.linalg.linalg.svd(W.transpose())
+    S = numpy.matrix(numpy.identity( 3 ))
+    if(numpy.linalg.det(U) * numpy.linalg.det(Vh)<0):
+        S[2,2] = -1
+    rot = U*S*Vh
+    trans = data.mean(1) - rot * model.mean(1)
+    
+    model_aligned = rot * model + trans
+    alignment_error = model_aligned - data
+    
+    trans_error = numpy.sqrt(numpy.sum(numpy.multiply(alignment_error,alignment_error),0)).A[0]
+        
+    return rot,trans,trans_error
+
+def plot_traj(ax,stamps,traj,style,color,label):
+    """
+    Plot a trajectory using matplotlib. 
+    
+    Input:
+    ax -- the plot
+    stamps -- time stamps (1xn)
+    traj -- trajectory (3xn)
+    style -- line style
+    color -- line color
+    label -- plot legend
+    
+    """
+    stamps.sort()
+    interval = numpy.median([s-t for s,t in zip(stamps[1:],stamps[:-1])])
+    x = []
+    y = []
+    last = stamps[0]
+    for i in range(len(stamps)):
+        if stamps[i]-last < 2*interval:
+            x.append(traj[i][0])
+            y.append(traj[i][1])
+        elif len(x)>0:
+            ax.plot(x,y,style,color=color,label=label)
+            label=""
+            x=[]
+            y=[]
+        last= stamps[i]
+    if len(x)>0:
+        ax.plot(x,y,style,color=color,label=label)
+            
+
+if __name__=="__main__":
+    # 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('first_file', help='ground truth trajectory (format: timestamp tx ty tz qx qy qz qw)')
+    parser.add_argument('second_file', help='estimated trajectory (format: timestamp tx ty tz qx qy qz qw)')
+    parser.add_argument('--offset', help='time offset added to the timestamps of the second file (default: 0.0)',default=0.0)
+    parser.add_argument('--scale', help='scaling factor for the second trajectory (default: 1.0)',default=1.0)
+    parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
+    parser.add_argument('--save', help='save aligned second trajectory to disk (format: stamp2 x2 y2 z2)')
+    parser.add_argument('--save_associations', help='save associated first and aligned second trajectory to disk (format: stamp1 x1 y1 z1 stamp2 x2 y2 z2)')
+    parser.add_argument('--plot', help='plot the first and the aligned second trajectory to an image (format: png)')
+    parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the RMSE absolute translational error in meters after alignment will be printed)', action='store_true')
+    args = parser.parse_args()
+
+    first_list = associate.read_file_list(args.first_file)
+    second_list = associate.read_file_list(args.second_file)
+
+    matches = associate.associate(first_list, second_list, float(args.offset), float(args.max_difference))
+    if len(matches)<2:
+        sys.exit("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory! Did you choose the correct sequence?")
+
+
+    first_xyz = numpy.matrix([[float(value) for value in first_list[a][0:3]] for a,b in matches]).transpose()
+    second_xyz = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for a,b in matches]).transpose()
+    rot,trans,trans_error = align(second_xyz,first_xyz)
+    
+    second_xyz_aligned = rot * second_xyz + trans
+    
+    first_stamps = first_list.keys()
+    first_stamps.sort()
+    first_xyz_full = numpy.matrix([[float(value) for value in first_list[b][0:3]] for b in first_stamps]).transpose()
+    
+    second_stamps = second_list.keys()
+    second_stamps.sort()
+    second_xyz_full = numpy.matrix([[float(value)*float(args.scale) for value in second_list[b][0:3]] for b in second_stamps]).transpose()
+    second_xyz_full_aligned = rot * second_xyz_full + trans
+    
+    if args.verbose:
+        print "compared_pose_pairs %d pairs"%(len(trans_error))
+
+        print "absolute_translational_error.rmse %f m"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
+        print "absolute_translational_error.mean %f m"%numpy.mean(trans_error)
+        print "absolute_translational_error.median %f m"%numpy.median(trans_error)
+        print "absolute_translational_error.std %f m"%numpy.std(trans_error)
+        print "absolute_translational_error.min %f m"%numpy.min(trans_error)
+        print "absolute_translational_error.max %f m"%numpy.max(trans_error)
+    else:
+        print "%f"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
+        
+    if args.save_associations:
+        file = open(args.save_associations,"w")
+        file.write("\n".join(["%f %f %f %f %f %f %f %f"%(a,x1,y1,z1,b,x2,y2,z2) for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A)]))
+        file.close()
+        
+    if args.save:
+        file = open(args.save,"w")
+        file.write("\n".join(["%f "%stamp+" ".join(["%f"%d for d in line]) for stamp,line in zip(second_stamps,second_xyz_full_aligned.transpose().A)]))
+        file.close()
+
+    if args.plot:
+        import matplotlib
+        matplotlib.use('Agg')
+        import matplotlib.pyplot as plt
+
+        fig = plt.figure()
+        ax = fig.add_subplot(111)
+        plot_traj(ax,first_stamps,first_xyz_full.transpose().A,'-',"black","ground truth")
+        plot_traj(ax,second_stamps,second_xyz_full_aligned.transpose().A,'-',"blue","estimated")
+
+        label="difference"
+        for (a,b),(x1,y1,z1),(x2,y2,z2) in zip(matches,first_xyz.transpose().A,second_xyz_aligned.transpose().A):
+            ax.plot([x1,x2],[y1,y2],'-',color="red",label=label)
+            label=""
+            
+        ax.legend()
+            
+        ax.set_xlabel('x [m]')
+        ax.set_ylabel('y [m]')
+        plt.savefig(args.plot,dpi=90)
+        

CPU_version/svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_rpe.py

@@ -0,0 +1,382 @@
+#!/usr/bin/python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2013, Juergen Sturm, TUM
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of TUM nor the names of its
+#    contributors may be used to endorse or promote products derived
+#    from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+"""
+This script computes the relative pose error from the ground truth trajectory
+and the estimated trajectory.
+"""
+
+import argparse
+import random
+import numpy
+import sys
+
+_EPS = numpy.finfo(float).eps * 4.0
+
+def transform44(l):
+    """
+    Generate a 4x4 homogeneous transformation matrix from a 3D point and unit quaternion.
+    
+    Input:
+    l -- tuple consisting of (stamp,tx,ty,tz,qx,qy,qz,qw) where
+         (tx,ty,tz) is the 3D position and (qx,qy,qz,qw) is the unit quaternion.
+         
+    Output:
+    matrix -- 4x4 homogeneous transformation matrix
+    """
+    t = l[1:4]
+    q = numpy.array(l[4:8], dtype=numpy.float64, copy=True)
+    nq = numpy.dot(q, q)
+    if nq < _EPS:
+        return numpy.array((
+        (                1.0,                 0.0,                 0.0, t[0])
+        (                0.0,                 1.0,                 0.0, t[1])
+        (                0.0,                 0.0,                 1.0, t[2])
+        (                0.0,                 0.0,                 0.0, 1.0)
+        ), dtype=numpy.float64)
+    q *= numpy.sqrt(2.0 / nq)
+    q = numpy.outer(q, q)
+    return numpy.array((
+        (1.0-q[1, 1]-q[2, 2],     q[0, 1]-q[2, 3],     q[0, 2]+q[1, 3], t[0]),
+        (    q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2],     q[1, 2]-q[0, 3], t[1]),
+        (    q[0, 2]-q[1, 3],     q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], t[2]),
+        (                0.0,                 0.0,                 0.0, 1.0)
+        ), dtype=numpy.float64)
+
+def read_trajectory(filename, matrix=True):
+    """
+    Read a trajectory from a text file. 
+    
+    Input:
+    filename -- file to be read
+    matrix -- convert poses to 4x4 matrices
+    
+    Output:
+    dictionary of stamped 3D poses
+    """
+    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):
+    """
+    Find the index of the closest value in a list.
+    
+    Input:
+    L -- the list
+    t -- value to be found
+    
+    Output:
+    index of the closest element
+    """
+    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):
+    """
+    Compute the relative 3D transformation between a and b.
+    
+    Input:
+    a -- first pose (homogeneous 4x4 matrix)
+    b -- second pose (homogeneous 4x4 matrix)
+    
+    Output:
+    Relative 3D transformation from a to b.
+    """
+    return numpy.dot(numpy.linalg.inv(a),b)
+
+def scale(a,scalar):
+    """
+    Scale the translational components of a 4x4 homogeneous matrix by a scale factor.
+    """
+    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):
+    """
+    Compute the distance of the translational component of a 4x4 homogeneous matrix.
+    """
+    return numpy.linalg.norm(transform[0:3,3])
+
+def compute_angle(transform):
+    """
+    Compute the rotation angle from a 4x4 homogeneous matrix.
+    """
+    # 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):
+    """
+    Compute the translational distances along a trajectory. 
+    """
+    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):
+    """
+    Compute the angular rotations along a trajectory. 
+    """
+    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 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):
+    """
+    Compute the relative pose error between two trajectories.
+    
+    Input:
+    traj_gt -- the first trajectory (ground truth)
+    traj_est -- the second trajectory (estimated trajectory)
+    param_max_pairs -- number of relative poses to be evaluated
+    param_fixed_delta -- false: evaluate over all possible pairs
+                         true: only evaluate over pairs with a given distance (delta)
+    param_delta -- distance between the evaluated pairs
+    param_delta_unit -- unit for comparison:
+                        "s": seconds
+                        "m": meters
+                        "rad": radians
+                        "deg": degrees
+                        "f": frames
+    param_offset -- time offset between two trajectories (to model the delay)
+    param_scale -- scale to be applied to the second trajectory
+    
+    Output:
+    list of compared poses and the resulting translation and rotation error
+    """
+    stamps_gt = list(traj_gt.keys())
+    stamps_est = list(traj_est.keys())
+    stamps_gt.sort()
+    stamps_est.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)
+    
+    gt_interval = numpy.median([s-t for s,t in zip(stamps_gt[1:],stamps_gt[:-1])])
+    gt_max_time_difference = 2*gt_interval
+    
+    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)
+        
+        result.append([stamp_est_0,stamp_est_1,stamp_gt_0,stamp_gt_1,trans,rot])
+        
+    if len(result)<2:
+        raise Exception("Couldn't find matching timestamp pairs between groundtruth and estimated trajectory!")
+        
+    return result
+
+def percentile(seq,q):
+    """
+    Return the q-percentile of a list
+    """
+    seq_sorted = list(seq)
+    seq_sorted.sort()
+    return seq_sorted[int((len(seq_sorted)-1)*q)]
+
+if __name__ == '__main__':
+    random.seed(0)
+
+    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")')
+    parser.add_argument('estimated_file', help='estimated trajectory file (format: "timestamp tx ty tz qx qy qz qw")')
+    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)', action='store_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)')
+    parser.add_argument('--verbose', help='print all evaluation data (otherwise, only the mean translational error measured in meters will be printed)', action='store_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))
+    
+    stamps = numpy.array(result)[:,0]
+    trans_error = numpy.array(result)[:,4]
+    rot_error = numpy.array(result)[:,5]
+    
+    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"%numpy.sqrt(numpy.dot(trans_error,trans_error) / len(trans_error))
+        print "translational_error.mean %f m"%numpy.mean(trans_error)
+        print "translational_error.median %f m"%numpy.median(trans_error)
+        print "translational_error.std %f m"%numpy.std(trans_error)
+        print "translational_error.min %f m"%numpy.min(trans_error)
+        print "translational_error.max %f m"%numpy.max(trans_error)
+
+        print "rotational_error.rmse %f deg"%(numpy.sqrt(numpy.dot(rot_error,rot_error) / len(rot_error)) * 180.0 / numpy.pi)
+        print "rotational_error.mean %f deg"%(numpy.mean(rot_error) * 180.0 / numpy.pi)
+        print "rotational_error.median %f deg"%numpy.median(rot_error)
+        print "rotational_error.std %f deg"%(numpy.std(rot_error) * 180.0 / numpy.pi)
+        print "rotational_error.min %f deg"%(numpy.min(rot_error) * 180.0 / numpy.pi)
+        print "rotational_error.max %f deg"%(numpy.max(rot_error) * 180.0 / numpy.pi)
+    else:
+        print numpy.mean(trans_error)
+
+    if args.plot:    
+        import matplotlib
+        matplotlib.use('Agg')
+        import matplotlib.pyplot as plt
+        import matplotlib.pylab as pylab
+        fig = plt.figure()
+        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.savefig(args.plot,dpi=300)
+        
+