public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

init GPU

Browse Source
This commit is contained in:
gfuse
2021-05-05 11:19:06 +02:00
parent 85470c37b6
commit fa3b7f1e1a
145 changed files with 37 additions and 842 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CPU_version/svo_analysis/scripts/__init__.py
View File
CPU_version/svo_analysis/scripts/benchmark.py
Executable
+111
View File
@@ -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
Executable
+150
View File
@@ -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
Executable
+159
View File
@@ -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
Executable
+58
View File
@@ -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)