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

make rospy package

Browse Source
This commit is contained in:
Christian Forster
2014-03-23 22:36:22 +01:00
parent b25f7c3f5b
commit bbf5b91bc1
21 changed files with 553 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo_analysis/src/svo_analysis/__init__.py
+2
View File
@@ -0,0 +1,2 @@
# -*- coding: utf-8 -*-
svo_analysis/src/svo_analysis/analyse_dataset.py
+42
View File
@@ -0,0 +1,42 @@
# -*- coding: utf-8 -*-
import associate
import numpy as np
import matplotlib.pyplot as plt
import yaml
import transformations
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'
svo_analysis/src/svo_analysis/analyse_logs.py
Executable
+132
View File
@@ -0,0 +1,132 @@
#!/usr/bin/python
import argparse
import csv
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import rc
def analyse_logs(D, trace_name):
# 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))
n_frames = len(is_frame)
n_kfs = len(is_kf)
n_nokf = len(is_nokf)
fps = 25.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]);
plt.figure()
plt.plot(np.arange(n_frames), D['repr_n_mps'][is_frame], 'r-', label='reprojected points, avg = %(i).2f'%{'i':mean_n_reproj_points})
plt.plot(np.arange(n_frames), D['repr_n_new_references'][is_frame], 'b-', label='reprojected matches, avg = %(i).2f'%{'i':mean_n_reproj_matches})
plt.legend()
plt.tight_layout()
plt.title('Reprojected points asdf')
plt.savefig(trace_name+'_reprojection.pdf', bbox_inches="tight")
# plot number of triangulations
# plt.figure(3)
# plt.plot(np.arange(n_kfs), D['triang_n_points'][is_kf], 'g-', label='triangulated points, avg %0.2f'%np.mean(D['triang_n_points'][is_kf]))
# plt.plot(np.arange(n_kfs), D['triang_n_refs'][is_kf], 'b-', label='new references, avg %0.2f'%np.mean(D['triang_n_refs'][is_kf]))
# plt.plot(np.arange(n_kfs), D['triang_n_fails'][is_kf], 'r-', label='failures, avg %0.2f'%np.mean(D['triang_n_fails'][is_kf]))
# plt.plot(np.arange(n_kfs), D['triang_n_add_refs'][is_kf], 'm-', label='additional refs, avg %0.2f'%np.mean(D['triang_n_add_refs'][is_kf]))
# plt.legend()
# plt.title('Triangulation')
# plt.savefig(trace_name+'_img_triangulation.png', bbox_inches="tight")
# 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')
ax.plot(np.arange(n_frames)/fps, D['repr_n_new_references'][is_frame], 'b-', label='number of successful matches')
fig.tight_layout()
fig.savefig(trace_name+'number_of_features.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]')
t_max = n_frames
ax.plot(np.arange(n_frames)[0:t_max]/fps, D['sfba_error_init'][is_frame][0:t_max], 'r-', label='Initial error')
ax.plot(np.arange(n_frames)[0:t_max]/fps, D['sfba_error_final'][is_frame][0:t_max], 'b-', label='Final error')
ax.legend(ncol=2)
ax.set_xlim([0, t_max/fps+4])
#ax.set_ylim([0, 0.8])
fig.tight_layout()
fig.savefig(trace_name+'reprojection_error.pdf', bbox_inches="tight")
print 'average reprojection error improvement: ' + str(init_error_avg - opt1_avg)
# plot number of references
plt.figure()
plt.fill(np.arange(n_frames+1), np.append(D['repr_n_new_references'][is_frame]+D['klt_n_matches'][is_frame],0), 'g-', label='Reprojection')
plt.fill(np.arange(n_frames+1), np.append(D['klt_n_tracked'][is_frame],0), 'b-', label='KLT Matches')
plt.plot(np.arange(n_frames), D['sfba_n_edges_final'][is_frame], 'r-', label='References after Optimization', linewidth=2.0)
plt.legend()
plt.tight_layout()
plt.title('Pose Optimization')
plt.savefig(trace_name+'_img_n_references.pdf', bbox_inches="tight")
# plot number of seeds
plt.figure()
has_seed = np.argwhere(D['seeds_n_tot'] >= 0)
plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_tot'][has_seed],0), label='nSeeds', edgecolor='none')
plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_updates'][has_seed]+D['seeds_n_failed_matches'][has_seed],0), label='nFailedMatches', edgecolor='none')
plt.fill(np.arange(len(has_seed)+1), np.append(D['seeds_n_updates'][has_seed],0), label='nUpdates', edgecolor='none')
plt.legend()
plt.tight_layout()
plt.title('Seeds')
plt.savefig(trace_name+'_seeds.pdf', bbox_inches="tight")
plt.figure()
plt.plot(D['n_candidates'])
plt.title('Candidates')
plt.show()
# plt.figure()
# plt.plot())
if __name__=="__main__":
dataset='20131003_1937_nslam_i7_asl2_fast'
n_align_frames = 10
save = True
results_dir = '../results/'+dataset
logfile = results_dir+'/'+dataset+'_0.csv'
# 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('--trace_file', help='nanoslam tracefile with timings and logs', default=logfile)
parser.add_argument('--trace_name', help='name of the tracefile, used for saving figures etc.', default=results_dir+'/')
args = parser.parse_args()
# tracefile
data = csv.reader(open(args.trace_file))
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])
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
analyse_logs(D, args.trace_name)
svo_analysis/src/svo_analysis/analyse_timing.py
Executable
+97
View File
@@ -0,0 +1,97 @@
#!/usr/bin/python
import argparse
import csv
import numpy as np
import matplotlib.pyplot as plt
from matplotlib import rc
def analyse_timing(D, trace_name):
# 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)
# plot total time for frame processing
avg_time = np.mean(D['t_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(np.arange(n_frames), D['t_tot_time'][is_frame]*1000, 'g-', label='total time [ms]')
ax.plot(np.arange(n_frames), np.ones(n_frames)*avg_time, 'b--', label=str('%(time).1fms mean time' % {'time': avg_time}))
ax.legend()
fig.tight_layout()
fig.savefig(trace_name+'_img_tot_processing_time.pdf', bbox_inches="tight")
fig = plt.figure(figsize=(6,2))
ax = fig.add_subplot(111, xlabel='Processing time [ms]')
ax.boxplot([
D['t_tot_time'][is_frame]*1000,
# D['t_local_ba'][is_kf]*1000,
D['t_pose_optimizer'][is_frame]*1000 + D['t_point_optimizer'][is_frame]*1000,
D['t_reproject'][is_frame]*1000,
D['t_sparse_img_align'][is_frame]*1000,
D['t_pyramid_creation'][is_frame]*1000
], 0,'', vert=0) #
boxplot_labels = [
r'\textbf{Total Motion Estimation: %2.2fms}' % np.median(D['t_tot_time'][is_frame]*1000),
# 'Local BA (KF only): %.2fms ' % np.median(D['t_local_ba'][is_kf]*1000),
'Refinement: %2.2fms' % np.median(D['t_pose_optimizer'][is_frame]*1000 + D['t_point_optimizer'][is_frame]*1000),
'Feature Alignment: %2.2fms' % np.median(D['t_reproject'][is_frame]*1000),
'Sparse Image Alignment: %2.2fms' % np.median(D['t_sparse_img_align'][is_frame]*1000),
'Pyramid Creation: %2.2fms' % np.median(D['t_pyramid_creation'][is_frame]*1000) ]
ax.set_yticks(np.arange(len(boxplot_labels))+1)
ax.set_yticklabels(boxplot_labels)
fig.tight_layout()
fig.savefig(trace_name+'_timing_boxplot.pdf', bbox_inches="tight")
fig = plt.figure(figsize=(8, 5))
ax = fig.add_subplot(111, ylabel='time [ms]')
ax.plot(D['t_reproject']*1000, 'r', label='reproject')
ax.plot(D['t_reproject_find_pts']*1000, 'g', label='reproject find pts') # ok!
ax.plot(D['t_reproject_best']*1000, 'm', label='reproject best')
ax.plot(D['t_reproject_candidates']*1000, 'b', label='reproject candidates')
ax.legend()
fig.tight_layout()
fig.savefig(trace_name+'_reprojection.pdf', bbox_inches="tight")
if __name__=="__main__":
# config
dataset='20131003_1937_nslam_i7_asl2_fast'
results_dir = '../results/'+dataset
tracefile = results_dir + '/'+dataset+'_0.csv'
# 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('--trace_file', help='nanoslam tracefile with timings and logs', default=tracefile)
parser.add_argument('--trace_name', help='name of the tracefile, used for saving figures etc.', default=results_dir)
args = parser.parse_args()
results_dir = args.trace_name
# tracefile
data = csv.reader(open(args.trace_file))
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])
# tell matplotlib to use latex font
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
svo_analysis/src/svo_analysis/analyse_trajectory.py
+187
View File
@@ -0,0 +1,187 @@
# -*- coding: utf-8 -*-
import associate
import numpy as np
import matplotlib.pyplot as plt
import yaml
import transformations
from matplotlib import rc
def alignSim3(model, data, n):
# select the first n datapoints and remove its mean so their center is zero
M = model[0:n,:]
D = data[0:n,:]
# substract mean
mu_M = M.mean(0)
mu_D = D.mean(0)
M_zerocentered = M - mu_M
D_zerocentered = D - mu_D
# correlation
C = 1.0/n*np.dot(M_zerocentered.transpose(), D_zerocentered)
sigma2 = 1.0/n*np.multiply(D_zerocentered,D_zerocentered).sum()
U_svd,D_svd,V_svd = np.linalg.linalg.svd(C)
D_svd = np.diag(D_svd)
V_svd = np.transpose(V_svd)
S = np.eye(3)
if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0):
S[2,2] = -1
R = np.dot(U_svd, np.dot(S, np.transpose(V_svd)))
s = 1.0/sigma2*np.trace(np.dot(D_svd, S))
t = mu_M-s*np.dot(R,mu_D)
return s, R, t
def getRigidBodyTrafo(quat,trans):
T = transformations.quaternion_matrix(quat)
T[0:3,3] = trans
return T
if __name__ == '__main__':
# config
dataset='20131003_1937_nslam_i7_asl2_fast'
n_align_frames = 100
save = True
results_dir = '../results/'+dataset
# plot options
rc('font',**{'family':'serif','serif':['Cardo']})
rc('text', usetex=True)
# load dataset parameters
params_stream = open(results_dir+'/params.yaml')
params = yaml.load(params_stream)
# set trajectory groundtruth and estimate file
traj_groundtruth = params['dataset_directory']+'/groundtruth_filtered.txt'
traj_estimate = results_dir+'/traj_estimate.txt'
traj_estimate_rotated_filename = results_dir+'/traj_estimate_rotated.txt'
translation_error_filename = results_dir+'/translation_error.txt'
orientation_error_filename = results_dir+'/orientation_error.txt'
scale_drift_filename = results_dir+'/scale_drift.txt'
# load hand-eye calib: trafo between (m)arker and (c)amera.
dataset_param_file = params['dataset_directory']+'/dataset_params.yaml'
dataset_param_file_stream = open(dataset_param_file,'r')
P = yaml.load(dataset_param_file_stream)
T_cm_quat = np.array([P['calib_Tcm_qx'], P['calib_Tcm_qy'], P['calib_Tcm_qz'], P['calib_Tcm_qw']])
T_cm_tran = np.array([P['calib_Tcm_tx'], P['calib_Tcm_ty'], P['calib_Tcm_tz']])
T_cm = getRigidBodyTrafo(T_cm_quat, T_cm_tran)
T_mc = transformations.inverse_matrix(T_cm)
# load data
data_gt = associate.read_file_list(traj_groundtruth)
data_es = associate.read_file_list(traj_estimate)
# find matches
matches = associate.associate(data_gt, data_es, -P['cam_delay'], 0.02)
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])
t_gt = np.array([float(a) for a,b in matches])
t_es = np.array([float(b) for a,b in matches])
start_time = min(t_es[0], t_gt[0])
t_es -= start_time
t_gt -= start_time
# align Sim3 to get scale
if(n_align_frames > np.shape(matches)[0]):
raise NameError('n_matches_for_scale is too large')
scale,rot,trans = alignSim3(p_gt, p_es, n_align_frames)
print 'scale = '+str(scale)
# get trafo between (v)ision and (o)ptitrack frame
T_om = getRigidBodyTrafo(q_gt[0,:], p_gt[0,:])
T_vc = getRigidBodyTrafo(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)
p_es_aligned = np.zeros(np.shape(p_es))
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))
# rotate trajectory
for i in range(np.shape(matches)[0]):
T_vc = getRigidBodyTrafo(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')
if save:
f = open(traj_estimate_rotated_filename,'w')
for i in range(np.shape(p_es_aligned)[0]):
f.write('%.7f %.5f %.5f %.5f %.5f %.5f %.5f %.5f\n' % (matches[i][1]-P['cam_delay'], p_es_aligned[i,0], p_es_aligned[i,1], p_es_aligned[i,2], q_es_aligned[i,0], q_es_aligned[i,1], q_es_aligned[i,2], q_es_aligned[i,3]))
f.close()
# 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')
fig.tight_layout()
fig.savefig(results_dir+'/trajectory.pdf')
# plot position error (drift)
translation_error = (p_gt-p_es_aligned)
fig = plt.figure(figsize=(8, 2.5))
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='position drift [m]', xlim=[0,t_es[-1]+4])
ax.plot(t_es, translation_error[:,0], 'r-', label='x')
ax.plot(t_es, translation_error[:,1], 'g-', label='y')
ax.plot(t_es, translation_error[:,2], 'b-', label='z')
fig.tight_layout()
if save:
fig.savefig(results_dir+'/translation_error.pdf')
f = open(translation_error_filename,'w')
for i in range(translation_error.shape[0]):
f.write('%.7f %.5f %.5f %.5f\n'%(t_es[i], translation_error[i,0], translation_error[i,1], translation_error[i,2]))
f.close()
# plot orientation error (drift)
orientation_error = (rpy_gt - rpy_es_aligned)
fig = plt.figure(figsize=(8, 2.5))
ax = fig.add_subplot(111, xlabel='time [s]', ylabel='orientation drift [rad]', xlim=[0,t_es[-1]+4])
ax.plot(t_es, orientation_error[:,0], 'r--', label='yaw')
ax.plot(t_es, orientation_error[:,1], 'g--', label='pitch')
ax.plot(t_es, orientation_error[:,2], 'b--', label='roll')
ax.legend()
fig.tight_layout()
if save:
fig.savefig(results_dir+'/orientation_error.pdf')
f = open(orientation_error_filename,'w')
for i in range(orientation_error.shape[0]):
f.write('%.7f %.5f %.5f %.5f\n'%(t_es[i], orientation_error[i,0], orientation_error[i,1], orientation_error[i,2]))
f.close()
# 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()
if save:
fig.savefig(results_dir+'/scale_drift.pdf')
f = open(scale_drift_filename,'w')
for i in range(orientation_error.shape[0]):
f.write('%.7f %.5f\n'%(t_es[i], scale_drift[i]))
f.close()
svo_analysis/src/svo_analysis/filter_groundtruth_smooth.py
+60
View File
@@ -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()
svo_analysis/src/svo_analysis/hand_eye_calib.py
+287
View File
@@ -0,0 +1,287 @@
# -*- coding: utf-8 -*-
"""
Created on Sun Aug 4 15:47:55 2013
@author: cforster
"""
import yaml
import numpy as np
import associate
import rotation_utils as ru
import matplotlib.pyplot as plt
def alignSim3(model, data, n):
# select the first n datapoints and remove its mean so their center is zero
M = model[0:n,:]
D = data[0:n,:]
# substract mean
mu_M = M.mean(0)
mu_D = D.mean(0)
M_zerocentered = M - mu_M
D_zerocentered = D - mu_D
# correlation
C = 1.0/n*np.dot(M_zerocentered.transpose(), D_zerocentered)
sigma2 = 1.0/n*np.multiply(D_zerocentered,D_zerocentered).sum()
U_svd,D_svd,V_svd = np.linalg.linalg.svd(C)
D_svd = np.diag(D_svd)
V_svd = np.transpose(V_svd)
S = np.eye(3)
if(np.linalg.det(U_svd)*np.linalg.det(V_svd) < 0):
S[2,2] = -1
R = np.dot(U_svd, np.dot(S, np.transpose(V_svd)))
s = 1.0/sigma2*np.trace(np.dot(D_svd, S))
t = mu_M-s*np.dot(R,mu_D)
return s, R, t
def matrixLog(A):
theta = np.arccos((np.trace(A)-1.0)/2.0)
log_theta = 0.5*theta/np.sin(theta) * (A - A.transpose())
x = np.array([log_theta[2,1], log_theta[0,2], log_theta[1,0]])
return x
def handEyeCalib(q_gt, q_es, p_gt, p_es, I, delta=10, verbose=True):
''' Implementation of the least squares solution described in the paper:
Robot Sensor Calibration: Solving AX=XB on the Euclidean Group
by Frank C. Park and Bryan J. Martin
'''
n = np.shape(I)[0]
M = np.zeros([3,3])
C = np.zeros([3*n, 3])
b_A = np.zeros([3*n,1])
b_B = np.zeros([3*n,1])
for ix, i in enumerate(I):
A1 = ru.quat2dcm(q_es[i,:])
A2 = ru.quat2dcm(q_es[i+delta,:])
A = np.dot(A1.transpose(), A2)
B1 = ru.quat2dcm(q_gt[i,:])
B2 = ru.quat2dcm(q_gt[i+delta,:])
B = np.dot(B1.transpose(), B2)
alpha = matrixLog(A)
beta = matrixLog(B)
M = M + np.dot(np.matrix(beta).transpose(), np.matrix(alpha))
C[3*ix:3*ix+3,:] = np.eye(3) - A
b_A[3*ix:3*ix+3,0] = np.dot(np.transpose(A1), p_es[i+delta,:]-p_es[i,:])
b_B[3*ix:3*ix+3,0] = np.dot(np.transpose(B1), p_gt[i+delta,:]-p_gt[i,:])
# compute rotation
D,V = np.linalg.linalg.eig(np.dot(M.transpose(), M))
Lambda = np.diag([np.sqrt(1.0/D[0]), np.sqrt(1.0/D[1]), np.sqrt(1.0/D[2])])
Vinv = np.linalg.linalg.inv(V)
X = np.dot(V, np.dot(Lambda, np.dot(Vinv, M.transpose())))
# compute translation
d = np.zeros([3*n,1])
for i in range(n):
d[3*i:3*i+3,:] = b_A[3*i:3*i+3,:] - np.dot(X, b_B[3*i:3*i+3,:])
b = np.dot(np.linalg.inv(np.dot(np.transpose(C),C)), np.dot(np.transpose(C),d))
return np.array(X),b
# user config
display = True
dataset = '20130906_2149_ptam_i7_asl2'
n_measurements = 1000
delta = 20
# load dataset parameters
dataset_dir = '../results/'+dataset
params_stream = open(dataset_dir+'/params.yaml')
params = yaml.load(params_stream)
# set trajectory groundtruth and estimate file
traj_groundtruth = params['dataset_directory']+'/groundtruth_filtered.txt'
traj_estimate = dataset_dir+'/traj_estimate.txt'
# load dataset params
dataset_param_file_stream = open(params['dataset_directory']+'/dataset_params.yaml','r')
dataset_param = yaml.load(dataset_param_file_stream)
# load data
data_gt = associate.read_file_list(traj_groundtruth)
data_es = associate.read_file_list(traj_estimate)
# select matches
#offset = -dataset_param['cam_delay']
offset = -0.03
matches = associate.associate(data_gt, data_es, offset, 0.02)
#matches = matches[0:470]
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 = alignSim3(p_gt, p_es, np.shape(matches)[0])
p_es = scale*p_es
#p_es = np.transpose(scale*np.dot(rot,np.transpose(p_es)))+trans
print 's='+str(scale)
#print 't='+str(trans)
#print 'R='+str(rot)
# plot sim3 aligned trajectory
if display:
fig = plt.figure()
ax = fig.add_subplot(111, aspect='equal')
ax.plot(p_es[:,0], p_es[:,1], 'r-', label='estimate')
ax.plot(p_gt[:,0], p_gt[:,1], 'b-', label='groundtruth')
ax.legend()
# --------------------------------------------------------------------------------
# hand-eye-calib
# select random measurements
I = np.array(rand(n_measurements,1)*(np.shape(matches)[0]-delta), dtype=int)[:,0]
R,b = handEyeCalib(q_gt, q_es, p_gt, p_es, I, delta, 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])
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))
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))
svo_analysis/src/svo_analysis/rotation_utils.py
+143
View File
@@ -0,0 +1,143 @@
# -*- coding: utf-8 -*-
"""
Created on Fri Jun 28 10:42:37 2013
@author: cforster
"""
import math
import numpy as np
# epsilon for testing whether a number is close to zero
_EPS = np.finfo(float).eps * 4.0
# axis sequences for Euler angles
_NEXT_AXIS = [1, 2, 0, 1]
# map axes strings to/from tuples of inner axis, parity, repetition, frame
_AXES2TUPLE = {
'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
def quat2dcm(quaternion):
"""Convert Quaternion [x y z w] to Direct Cosine Matrix representation """
q = np.array(quaternion[:4], dtype=np.float64, copy=True)
nq = np.dot(q, q)
if nq < _EPS:
return np.identity(4)
q *= math.sqrt(2.0 / nq)
q = np.outer(q, q)
return np.array([ [1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3]],
[ q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3]],
[ q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1]]],
dtype=np.float64)
def dcm2quat(matrix):
"""Convert Direct Cosine Matrix to Quaternion
This algorithm comes from "Quaternion Calculus and Fast Animation",
Ken Shoemake, 1987 SIGGRAPH course notes
"""
q = np.empty((4, ), dtype=np.float64)
M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
t = np.trace(M)
if (t > 0):
t = math.sqrt(t + 1.0);
q[3] = 0.5*t;
t = 0.5/t;
q[0] = (M[2,1] - M[1,2]) * t;
q[1] = (M[0,2] - M[2,0]) * t;
q[2] = (M[1,0] - M[0,1]) * t;
else:
i = 0;
if (M[1,1] > M[0,0]):
i = 1;
if (M[2,2] > M[i,i]):
i = 2;
j = (i+1)%3;
k = (j+1)%3;
t = math.sqrt(M[i,i]-M[j,j]-M[k,k] + 1.0);
q[i] = 0.5 * t;
t = 0.5/t;
q[3] = (M[k,j]-M[j,k])*t;
q[j] = (M[j,i]+M[i,j])*t;
q[k] = (M[k,i]+M[i,k])*t;
return q
def dcm2rpy(matrix, axes='sxyz'):
"""Return Euler angles from rotation matrix for specified axis sequence.
axes : One of 24 axis sequences as string or encoded tuple
Note that many Euler angle triplets can describe one matrix.
>>> R0 = euler_matrix(1, 2, 3, 'syxz')
>>> al, be, ga = euler_from_matrix(R0, 'syxz')
>>> R1 = euler_matrix(al, be, ga, 'syxz')
>>> numpy.allclose(R0, R1)
True
>>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
>>> for axes in _AXES2TUPLE.keys():
... R0 = euler_matrix(axes=axes, *angles)
... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
... if not numpy.allclose(R0, R1): print axes, "failed"
"""
try:
firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
except (AttributeError, KeyError):
_ = _TUPLE2AXES[axes]
firstaxis, parity, repetition, frame = axes
i = firstaxis
j = _NEXT_AXIS[i+parity]
k = _NEXT_AXIS[i-parity+1]
M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
if repetition:
sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
if sy > _EPS:
ax = math.atan2( M[i, j], M[i, k])
ay = math.atan2( sy, M[i, i])
az = math.atan2( M[j, i], -M[k, i])
else:
ax = math.atan2(-M[j, k], M[j, j])
ay = math.atan2( sy, M[i, i])
az = 0.0
else:
cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
if cy > _EPS:
ax = math.atan2( M[k, j], M[k, k])
ay = math.atan2(-M[k, i], cy)
az = math.atan2( M[j, i], M[i, i])
else:
ax = math.atan2(-M[j, k], M[j, j])
ay = math.atan2(-M[k, i], cy)
az = 0.0
if parity:
ax, ay, az = -ax, -ay, -az
if frame:
ax, az = az, ax
return ax, ay, az
def quat2rpy(quaternion, axes='sxyz'):
"""Return Euler angles from quaternion for specified axis sequence.
>>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947])
>>> numpy.allclose(angles, [0.123, 0, 0])
True
"""
return dcm2rpy(quat2dcm(quaternion), axes)
svo_analysis/src/svo_analysis/transformations.py
+1705
View File
File diff suppressed because it is too large Load Diff
svo_analysis/src/svo_analysis/tum_benchmark_tools/__init__.py
+2
View File
@@ -0,0 +1,2 @@
# -*- coding: utf-8 -*-
svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py
Executable
+128
View File
@@ -0,0 +1,128 @@
#!/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='first text file (format: timestamp data)')
parser.add_argument('second_file', help='second text file (format: timestamp data)')
parser.add_argument('--first_only', help='only output associated lines from first file', action='store_true')
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))
if args.first_only:
for a,b in matches:
print("%f %s"%(a," ".join(first_list[a])))
else:
for a,b in matches:
print("%f %s %f %s"%(a," ".join(first_list[a]),b-float(args.offset)," ".join(second_list[b])))
svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_ate.py
Executable
+195
View File
@@ -0,0 +1,195 @@
#!/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
import matplotlib.pylab as pylab
from matplotlib.patches import Ellipse
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)
svo_analysis/src/svo_analysis/tum_benchmark_tools/evaluate_rpe.py
Executable
+382
View File
@@ -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)