diff --git a/svo_analysis/comparisons/speed.yaml b/svo_analysis/comparisons/speed.yaml new file mode 100644 index 0000000..2f706d4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/svo_analysis/scripts/benchmark.py b/svo_analysis/scripts/benchmark.py index ed97040..a3733ea 100755 --- a/svo_analysis/scripts/benchmark.py +++ b/svo_analysis/scripts/benchmark.py @@ -51,7 +51,7 @@ def run_experiment(dataset, params): # 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'), + 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'), diff --git a/svo_analysis/scripts/compare_results.py b/svo_analysis/scripts/compare_results.py new file mode 100755 index 0000000..5605fe3 --- /dev/null +++ b/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) + + + + diff --git a/svo_analysis/scripts/comparison.py b/svo_analysis/scripts/comparison.py index 5605fe3..1651247 100755 --- a/svo_analysis/scripts/comparison.py +++ b/svo_analysis/scripts/comparison.py @@ -18,133 +18,105 @@ 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) + 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: +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 - # 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]) +def compare_results(comp_params, results_dir, comparison_dir): + print('run comparison: '+comp_params['comparison_name']) + - 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: + # ------------------------------------------------------------------------- + # 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']) + else: + ax.plot(data[:,1], data[:,2], color=base_plot.get_color()) + ax.legend(loc='upper left') + save_figure(fig, 'trajectory', comparison_dir) - # 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') - + # ------------------------------------------------------------------------- + # 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 = 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)) + 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]+1] + + + if i == 0: + base_plot, = ax.plot(distances, e*1000, label=exp_set['label']) + else: + ax.plot(distances, e*1000, color=base_plot.get_color()) + + ax.legend(loc='upper left') + save_figure(fig, 'translation_error', comparison_dir) 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) + 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(args.result_directories, results_dir, comparison_dir) - - - + # run comparison + compare_results(comp_params, results_dir, comparison_dir) +