From d0a240604174a0c016b876b5334e3cb2c5547b4a Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Mon, 31 Mar 2014 21:21:34 +0200 Subject: [PATCH] updated trajectory comparison --- svo_analysis/experiments/.gitignore | 2 + svo_analysis/experiments/asl2_acc.yaml | 3 +- svo_analysis/experiments/asl2_fast.yaml | 3 +- svo_analysis/scripts/comparison.py | 160 +++++++++++------------- 4 files changed, 78 insertions(+), 90 deletions(-) create mode 100644 svo_analysis/experiments/.gitignore mode change 100644 => 100755 svo_analysis/scripts/comparison.py diff --git a/svo_analysis/experiments/.gitignore b/svo_analysis/experiments/.gitignore new file mode 100644 index 0000000..5b74354 --- /dev/null +++ b/svo_analysis/experiments/.gitignore @@ -0,0 +1,2 @@ +*.yaml +!.gitignore diff --git a/svo_analysis/experiments/asl2_acc.yaml b/svo_analysis/experiments/asl2_acc.yaml index 0b5bf3b..9e05b2e 100644 --- a/svo_analysis/experiments/asl2_acc.yaml +++ b/svo_analysis/experiments/asl2_acc.yaml @@ -1,2 +1,3 @@ dataset: asl_vicon_d2 -param_settings: vo_accurate \ No newline at end of file +param_settings: vo_accurate +experiment_label: 'svo acc' \ No newline at end of file diff --git a/svo_analysis/experiments/asl2_fast.yaml b/svo_analysis/experiments/asl2_fast.yaml index 34285b5..529b6bf 100644 --- a/svo_analysis/experiments/asl2_fast.yaml +++ b/svo_analysis/experiments/asl2_fast.yaml @@ -1,2 +1,3 @@ dataset: asl_vicon_d2 -param_settings: vo_fast \ No newline at end of file +param_settings: vo_fast +experiment_label: 'svo fast' \ No newline at end of file diff --git a/svo_analysis/scripts/comparison.py b/svo_analysis/scripts/comparison.py old mode 100644 new mode 100755 index 1bc219e..5605fe3 --- a/svo_analysis/scripts/comparison.py +++ b/svo_analysis/scripts/comparison.py @@ -1,32 +1,32 @@ -# -*- coding: utf-8 -*- +#!/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 plotTrajectory(ax, filename, label, color, linewidth): +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) - -if __name__ == '__main__': +def compare_results(experiments, results_dir, comparison_dir, + plot_scale_drift = False): - results_dir = '../results' - experiments = ['20130911_2113_nslam_i7_asl2_acc', - '20130911_2229_nslam_i7_asl2_fast', - '20130906_2149_ptam_i7_asl2'] - - # tell matplotlib to use latex font - rc('font',**{'family':'serif','serif':['Cardo']}) - rc('text', usetex=True) - # ------------------------------------------------------------------------------ # position error fig_poserr = plt.figure(figsize=(8,6)) @@ -37,28 +37,25 @@ if __name__ == '__main__': for exp in experiments: # load dataset parameters - params_stream = open(results_dir+'/'+exp+'/params.yaml') + params_stream = open(os.path.join(results_dir, exp, 'params.yaml')) params = yaml.load(params_stream) # plot translation error - trans_error_filename = results_dir+'/'+exp+'/translation_error.txt' - file = open(trans_error_filename) - data = file.read() - lines = data.replace(","," ").replace("\t"," ").split("\n") - trans_error = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64) + 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_y.set_xlim([0, trans_error[-1,0]+4]) ax_poserr_z.set_xlim([0, trans_error[-1,0]+4]) ax_poserr_x.legend(bbox_to_anchor=[0, 0], loc='lower left', ncol=3) ax_poserr_x.grid() ax_poserr_y.grid() ax_poserr_z.grid() fig_poserr.tight_layout() - fig_poserr.savefig('../results/translation_error.pdf') + fig_poserr.savefig(os.path.join(comparison_dir, 'translation_error.pdf')) # ------------------------------------------------------------------------------ # orientation error @@ -70,15 +67,12 @@ if __name__ == '__main__': for exp in experiments: # load dataset parameters - params_stream = open(results_dir+'/'+exp+'/params.yaml') + params_stream = open(os.path.join(results_dir, exp, 'params.yaml')) params = yaml.load(params_stream) # plot translation error - rot_error_filename = results_dir+'/'+exp+'/orientation_error.txt' - file = open(rot_error_filename) - data = file.read() - lines = data.replace(","," ").replace("\t"," ").split("\n") - rot_error = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64) + 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]) @@ -91,76 +85,66 @@ if __name__ == '__main__': ax_roterr_p.grid() ax_roterr_y.grid() fig_roterr.tight_layout() - fig_roterr.savefig('../results/orientation_error.pdf') + fig_roterr.savefig(os.path.join(comparison_dir, 'orientation_error.pdf')) # ------------------------------------------------------------------------------ # scale error - fig_scale = plt.figure(figsize=(8,2.5)) - ax_scale = fig_scale.add_subplot(111, xlabel='time [s]', ylabel='scale change [\%]') - - for exp in experiments: + 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 [\%]') - # load dataset parameters - params_stream = open(results_dir+'/'+exp+'/params.yaml') - params = yaml.load(params_stream) + 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']) - # plot translation error - scale_drift_filename = results_dir+'/'+exp+'/scale_drift.txt' - file = open(scale_drift_filename) - data = file.read() - lines = data.replace(","," ").replace("\t"," ").split("\n") - scale_drift = np.array([[v.strip() for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"], dtype=np.float64) - ax_scale.plot(scale_drift[:,0], scale_drift[:,1], label=params['experiment_label']) - - ax_scale.set_xlim([0, rot_error[-1,0]+4]) - ax_scale.legend(bbox_to_anchor=[0, 1], loc='upper left', ncol=3) - ax_scale.grid() - fig_scale.tight_layout() - fig_scale.savefig('../results/scale_drift.pdf') + 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) - - # show detail: - axins = zoomed_inset_axes(ax_traj, 5.0, loc=1) # zoom = 6 - plotTrajectory(axins, '/home/cforster/Datasets/asl_vicon_d2/groundtruth_filtered.txt', 'Groundtruth', 'k', 1.5) - plotTrajectory(axins, results_dir+'/20130911_2229_nslam_i7_asl2_fast/traj_estimate_rotated.txt', 'Fast', 'g', 1) - plotTrajectory(axins, results_dir+'/20130906_2149_ptam_i7_asl2/traj_estimate_rotated.txt', 'PTAM', 'r', 1) - axins.set_xlim(1.2, 1.70) - axins.set_ylim(-0.8, -0.3) - axins.patch.set_facecolor('0.9') - plt.xticks(visible=False) - plt.yticks(visible=False) - - mark_inset(ax_traj, axins, loc1=2, loc2=4, fc="none", ec='b') - plt.draw() - plt.show() - ax_traj.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=3) - ax_traj.grid() - fig_traj.tight_layout() - fig_traj.savefig('../results/trajectory_asl.pdf') +# 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') - # ------------------------------------------------------------------------------ - # number of matches +if __name__ == '__main__': - # plot number of klt tracked and matched points -# fig = plt.figure(figsize=(8,2)) -# ax = fig.add_subplot(111, xlabel='time [s]', ylabel='no. features', ylim=[0,220]) -# ax.plot(np.arange(len(n_fts_1))/25.0, n_fts_1, 'b-', label='Accurate') -# ax.plot(np.arange(len(n_fts_2))/25.0, n_fts_2, 'g-', label='Fast') -# ax.legend(bbox_to_anchor=[1, 0], loc='lower right', ncol=2) -# fig.tight_layout() -# fig.savefig('../results/number_of_features2.pdf', bbox_inches="tight") -# + 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) - - \ No newline at end of file + + +