init GPU
This commit is contained in:
gfuse
@@ -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'
|
||||
|
||||
|
||||
@@ -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)
|
||||
@@ -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))
|
||||
|
||||
@@ -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")
|
||||
@@ -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)
|
||||
@@ -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()
|
||||
@@ -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))
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,2 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
+130
@@ -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)
|
||||
|
||||
+194
@@ -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)
|
||||
|
||||
+382
@@ -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)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user