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

comparison of depth estimates in synthetic scenes

Browse Source
This commit is contained in:
Christian Forster
2014-05-22 19:48:02 +02:00
parent d6927c0bbe
commit e8d6757e9e
5 changed files with 210 additions and 178 deletions
Download Patch File Download Diff File Expand all files Collapse all files
svo_analysis/scripts/comparison.py
+30 -17
View File
@@ -40,6 +40,13 @@ def distances_along_trajectory(traj):
distances.append(sum) distances.append(sum)
return distances return distances
def get_distance_from_start(gt):
distances = np.diff(gt[:,1:4],axis=0)
distances = np.sqrt(np.sum(np.multiply(distances,distances),1))
distances = np.cumsum(distances)
distances = np.concatenate(([0], distances))
return distances
def compare_results(comp_params, results_dir, comparison_dir): def compare_results(comp_params, results_dir, comparison_dir):
print('run comparison: '+comp_params['comparison_name']) print('run comparison: '+comp_params['comparison_name'])
@@ -71,10 +78,7 @@ def compare_results(comp_params, results_dir, comparison_dir):
for i, exp in enumerate(exp_set['experiments']): for i, exp in enumerate(exp_set['experiments']):
gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt')) gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt'))
distances = np.diff(gt[:,1:4],axis=0) distances = get_distance_from_start(gt)
distances = np.sqrt(np.sum(np.multiply(distances,distances),1))
distances = np.cumsum(distances)
distances = np.concatenate(([0], distances))
data = np.loadtxt(os.path.join(results_dir, exp, 'translation_error.txt')) data = np.loadtxt(os.path.join(results_dir, exp, 'translation_error.txt'))
e = np.sqrt(np.sum(np.multiply(data[:,1:4],data[:,1:4]),1)) e = np.sqrt(np.sum(np.multiply(data[:,1:4],data[:,1:4]),1))
@@ -100,21 +104,30 @@ def compare_results(comp_params, results_dir, comparison_dir):
# ------------------------------------------------------------------------- # -------------------------------------------------------------------------
# plot depth estimation error: # plot depth estimation error:
fig = plt.figure(figsize=(6,5)) fig = plt.figure(figsize=(6,5))
ax = fig.add_subplot(111, xlabel='Error [m]', ylabel='Precision [\%]') ax = fig.add_subplot(111, xlabel='Travelled distance [m]', ylabel='Error [m]')
for k, exp_set in enumerate(comp_params['experiment_sets']): for k, exp_set in enumerate(comp_params['experiment_sets']):
print('plot depth error for experiment: ' + exp_set['label']) print('plot depth error for experiment: ' + exp)
exp = exp_set['experiments'][0]
gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt'))
x_axis_data = get_distance_from_start(gt)
analyse_depth.plot_depth_over_time(os.path.join(results_dir, exp), ax,
x_axis_data[1:], line_colors[k], exp_set['label'])
ax.legend(loc='upper left', ncol=3)
save_figure(fig, 'depth_error_textures', comparison_dir)
for i, exp in enumerate(exp_set['experiments']): fig = plt.figure(figsize=(6,5))
ax = fig.add_subplot(111, xlabel='Travelled distance [m]', ylabel='Error [m]')
D = np.loadtxt(os.path.join(results_dir, exp, 'depth_error.txt'), delimiter=' ') exp_set = comp_params['experiment_sets'][0]
D = D[np.logical_and(D[:,0] > 1, D[:,0] < D[-1,0]/4),:] for i, exp in enumerate(exp_set['experiments']):
errors = np.abs(D[:,1]) print('plot depth error for speed: ' + exp)
if i == 0: gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt'))
base_plot = analyse_depth.precision_plot(ax, errors, 0.05, line_colors[k], line_styles[np.mod(i, len(line_styles))]) x_axis_data = get_distance_from_start(gt)
else: params = yaml.load(open(os.path.join(results_dir, exp, 'dataset_params.yaml')))
base_plot = analyse_depth.precision_plot(ax, errors, 0.05, base_plot.get_color(), line_styles[np.mod(i, len(line_styles))]) analyse_depth.plot_depth_over_time(os.path.join(results_dir, exp), ax,
x_axis_data[1:], line_colors[i],
save_figure(fig, 'depth_error', comparison_dir) str(params['trajectory_modifiers']['speed'])+' m/s')
ax.legend(loc='upper left', ncol=3)
save_figure(fig, 'depth_error_speed', comparison_dir)
if __name__ == '__main__': if __name__ == '__main__':
svo_analysis/src/svo_analysis/analyse_depth.py
+19 -17
View File
@@ -32,33 +32,35 @@ def analyse_depth(results_dir):
fig.tight_layout() fig.tight_layout()
fig.savefig(os.path.join(results_dir,'depth_error.pdf'), bbox_inches="tight") fig.savefig(os.path.join(results_dir,'depth_error.pdf'), bbox_inches="tight")
def analyse_depth_over_time(results_dir):
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 = np.loadtxt(os.path.join(results_dir, 'depth_error.txt'), delimiter=' ')
D = D[D[:,0] > 1,:] D = D[D[:,0] > 1,:]
fig = plt.figure(figsize=(6,5))
ax = fig.add_subplot(111, xlabel='Measurement', ylabel='Scale-Drift')
idxs = np.unique(D[:,0]) idxs = np.unique(D[:,0])
percentile_20 = np.zeros(len(idxs)) percentile_10 = np.zeros(len(idxs))
percentile_50 = np.zeros(len(idxs)) percentile_50 = np.zeros(len(idxs))
percentile_80 = np.zeros(len(idxs)) percentile_90 = np.zeros(len(idxs))
for i in range(len(idxs)): for i in range(len(idxs)):
errors = np.abs(D[D[:,0]==idxs[i],1]) errors = np.abs(D[D[:,0]==idxs[i],1])
percentile_20[i] = np.percentile(errors, 20) percentile_10[i] = np.percentile(errors, 10)
percentile_50[i] = np.percentile(errors, 50) percentile_50[i] = np.percentile(errors, 50)
percentile_80[i] = np.percentile(errors, 80) percentile_90[i] = np.percentile(errors, 90)
if len(x_axis_data) == 0:
base_plot, = ax.plot(idxs, percentile_50, linewidth=2) x_axis_data = idxs
ax.fill_between(idxs, percentile_20, percentile_80, alpha=0.25, color=base_plot.get_color()) print np.shape(x_axis_data)
fig.tight_layout() print np.shape(idxs)
fig.savefig(os.path.join(results_dir,'scale_drift.pdf'), bbox_inches="tight") 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__': if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Analyse depth estimate') parser = argparse.ArgumentParser(description='Analyse depth estimate')
parser.add_argument('results_dir', help='folder with the results') parser.add_argument('results_dir', help='folder with the results')
args = parser.parse_args() args = parser.parse_args()
analyse_depth(args.results_dir) analyse_depth(args.results_dir)
analyse_depth_over_time(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)
svo_analysis/src/svo_analysis/analyse_trajectory.py
+105 -103
View File
@@ -32,117 +32,119 @@ def plot_rotation_error(timestamps, rotation_error, results_dir):
fig.tight_layout() fig.tight_layout()
fig.savefig(results_dir+'/orientation_error.pdf') 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): 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 not os.path.isfile(os.path.join(results_dir, 'translation_error.txt')): if params['dataset_is_blender']:
analyse_synthetic_trajectory(results_dir)
T_cm, T_mc = load_hand_eye_calib(results_dir) elif use_hand_eye_calib:
print('loading datasets.') analyse_optitrack_trajectory_with_hand_eye_calib(results_dir, params, n_align_frames)
t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir)
# 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
p_es_aligned = np.zeros(np.shape(p_es))
if use_hand_eye_calib:
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))
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()
else:
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)
# 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')
else: else:
data = np.loadtxt(os.path.join(results_dir, 'translation_error.txt')) t_es, p_es, q_es, t_gt, p_gt, q_gt = load_dataset(results_dir, params['cam_delay'])
timestamps = data[:,0] scale,rot,trans = align_trajectory.align_sim3(p_gt[0:n_align_frames,:], p_es[0:n_align_frames,:])
translation_error = data[:,1:4] p_es_aligned = np.zeros(np.shape(p_es))
plot_translation_error(timestamps, translation_error, results_dir) for i in range(np.shape(p_es)[0]):
p_es_aligned[i,:] = scale*rot.dot(p_es[i,:]) + trans
# plot orientation error # plot position error (drift)
data = np.loadtxt(os.path.join(results_dir, 'orientation_error.txt')) translation_error = (p_gt-p_es_aligned)
timestamps = data[:,0] plot_translation_error(t_es, translation_error, results_dir)
orientation_error = data[:,1:4]
plot_rotation_error(timestamps, orientation_error, results_dir)
def get_rigid_body_trafo(quat,trans): def get_rigid_body_trafo(quat,trans):
T = transformations.quaternion_matrix(quat) T = transformations.quaternion_matrix(quat)
T[0:3,3] = trans T[0:3,3] = trans
return T return T
def load_hand_eye_calib(results_dir): def load_dataset(results_dir, cam_delay):
P = yaml.load(open(os.path.join(results_dir, 'dataset_params.yaml'),'r'))['hand_eye_calib'] print('loading dataset in '+results_dir)
print('cam_delay = '+str(cam_delay))
T_cm_quat = np.array([P['Tcm_qx'], P['Tcm_qy'], P['Tcm_qz'], P['Tcm_qw']]) data_gt = open(os.path.join(results_dir, 'groundtruth.txt')).read()
T_cm_tran = np.array([P['Tcm_tx'], P['Tcm_ty'], P['Tcm_tz']])
T_cm = get_rigid_body_trafo(T_cm_quat, T_cm_tran)
T_mc = transformations.inverse_matrix(T_cm)
return T_cm, T_mc
def load_dataset(results_dir):
"""
Assumes that groundtruth measurements are already matched with the
images.
"""
data_gt = open(os.path.join(results_dir, 'groundtruth_matched.txt')).read()
lines = data_gt.replace(","," ").replace("\t"," ").split("\n") lines = data_gt.replace(","," ").replace("\t"," ").split("\n")
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 = 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 = [(float(l[0]),l[1:]) for l in data_gt]
data_gt = dict(data_gt) data_gt = dict(data_gt)
@@ -152,7 +154,7 @@ def load_dataset(results_dir):
data_es = [(float(l[0]),l[1:]) for l in data_es] data_es = [(float(l[0]),l[1:]) for l in data_es]
data_es = dict(data_es) data_es = dict(data_es)
matches = associate.associate(data_gt, data_es, 0.0, 0.0001) 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]) 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]) 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]) p_es = np.array([[np.float(value) for value in data_es[b][0:3]] for a,b in matches])
@@ -174,7 +176,7 @@ if __name__ == '__main__':
''') ''')
parser.add_argument('results_dir', help='folder with the results') parser.add_argument('results_dir', help='folder with the results')
parser.add_argument('--use_hand_eye_calib', help='', action='store_true') parser.add_argument('--use_hand_eye_calib', help='', action='store_true')
parser.add_argument('--n_align_frames', help='', default=20) parser.add_argument('--n_align_frames', help='', default=200)
args = parser.parse_args() args = parser.parse_args()
print('analyse trajectory for dataset: '+str(args.results_dir)) print('analyse trajectory for dataset: '+str(args.results_dir))
svo_analysis/src/svo_analysis/hand_eye_calib.py
+52 -37
View File
@@ -4,41 +4,37 @@ Created on Sun Aug 4 15:47:55 2013
@author: cforster @author: cforster
""" """
import os
import yaml import yaml
import numpy as np import numpy as np
import associate import svo_analysis.tum_benchmark_tools.associate as associate
import rotation_utils as ru import vikit_py.align_trajectory as align_trajectory
import vikit_py.transformations as transformations
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
# user config # user config
display = True display = True
dataset = '20130906_2149_ptam_i7_asl2' dataset_dir = '/home/cforster/catkin_ws/src/rpg_svo/svo_analysis/results/rpg_circle_1'
n_measurements = 1000 n_measurements = 400
delta = 20 n_align_sim3 = 600
delta = 50
# load dataset parameters # load dataset parameters
dataset_dir = '../results/'+dataset params = yaml.load(open(os.path.join(dataset_dir, 'dataset_params.yaml')))
params_stream = open(dataset_dir+'/params.yaml')
params = yaml.load(params_stream)
# set trajectory groundtruth and estimate file # set trajectory groundtruth and estimate file
traj_groundtruth = params['dataset_directory']+'/groundtruth_filtered.txt' traj_groundtruth = os.path.join(dataset_dir, 'groundtruth.txt')
traj_estimate = dataset_dir+'/traj_estimate.txt' traj_estimate = os.path.join(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 # load data
data_gt = associate.read_file_list(traj_groundtruth) data_gt = associate.read_file_list(traj_groundtruth)
data_es = associate.read_file_list(traj_estimate) data_es = associate.read_file_list(traj_estimate)
# select matches # select matches
#offset = -dataset_param['cam_delay'] offset = -params['cam_delay']
offset = -0.03 print ('offset = '+str(offset))
matches = associate.associate(data_gt, data_es, offset, 0.02) matches = associate.associate(data_gt, data_es, offset, 0.02)
#matches = matches[0:470] #matches = matches[500:]
p_gt = np.array([[float(value) for value in data_gt[a][0:3]] for a,b in matches]) 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]) 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]) p_es = np.array([[float(value) for value in data_es[b][0:3]] for a,b in matches])
@@ -46,30 +42,49 @@ q_es = np.array([[float(value) for value in data_es[b][3:7]] for a,b in matches]
# -------------------------------------------------------------------------------- # --------------------------------------------------------------------------------
# align Sim3 to get scale # align Sim3 to get scale
scale,rot,trans = align_sim3(p_gt, p_es, np.shape(matches)[0]) 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 p_es = scale*p_es
#p_es = np.transpose(scale*np.dot(rot,np.transpose(p_es)))+trans
print 's='+str(scale) print 's='+str(scale)
#print 't='+str(trans) print 't='+str(trans)
#print 'R='+str(rot) print 'R='+str(rot)
# plot sim3 aligned trajectory # plot sim3 aligned trajectory
if display: fig = plt.figure()
fig = plt.figure() ax = fig.add_subplot(111, aspect='equal')
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_es[:,0], p_es[:,1], 'r-', label='estimate') ax.plot(p_gt[:,0], p_gt[:,1], 'b-', label='groundtruth', alpha=0.2)
ax.plot(p_gt[:,0], p_gt[:,1], 'b-', label='groundtruth') ax.plot(p_es_aligned[:n_align_sim3,0], p_es_aligned[:n_align_sim3,1], 'g-', label='aligned', linewidth=2)
ax.legend() 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 # hand-eye-calib
# select random measurements # select random measurements
I = np.array(rand(n_measurements,1)*(np.shape(matches)[0]-delta), dtype=int)[:,0] I = np.array(np.random.rand(n_measurements,1)*(np.shape(matches)[0]-delta), dtype=int)[:,0]
R,b = hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta, True) R,b = align_trajectory.hand_eye_calib(q_gt, q_es, p_gt, p_es, I, delta, True)
print 'quat = ' + str(ru.dcm2quat(R)) print 'quat = ' + str(transformations.quaternion_from_matrix(transformations.convert_3x3_to_4x4(R)))
print 'b = ' + str(b) print 'b = ' + str(b)
rpy_es = np.zeros([q_es.shape[0]-1, 3]) rpy_es = np.zeros([q_es.shape[0]-1, 3])
@@ -78,15 +93,15 @@ t_gt = np.zeros([q_es.shape[0]-1,3])
t_es = 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]): for i in range(delta,np.shape(q_es)[0]):
A1 = ru.quat2dcm(q_es[i-delta,:]) A1 = transformations.quaternion_matrix(q_es[i-delta,:])[:3,:3]
A2 = ru.quat2dcm(q_es[i,:]) A2 = transformations.quaternion_matrix(q_es[i,:])[:3,:3]
A = np.dot(A1.transpose(), A2) A = np.dot(A1.transpose(), A2)
B1 = ru.quat2dcm(q_gt[i-delta,:]) B1 = transformations.quaternion_matrix(q_gt[i-delta,:])[:3,:3]
B2 = ru.quat2dcm(q_gt[i,:]) B2 = transformations.quaternion_matrix(q_gt[i,:])[:3,:3]
B = np.dot(B1.transpose(), B2) B = np.dot(B1.transpose(), B2)
B_es = np.dot(np.transpose(R), np.dot(A, R)) B_es = np.dot(np.transpose(R), np.dot(A, R))
rpy_gt[i-delta,:] = ru.dcm2rpy(B) rpy_gt[i-delta,:] = transformations.euler_from_matrix(B, 'rzyx')
rpy_es[i-delta,:] = ru.dcm2rpy(B_es) 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_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_A = np.dot(np.transpose(A1),(p_es[i,:]-p_es[i-delta,:]))
t_gt[i-delta,:] = t_B t_gt[i-delta,:] = t_B
svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py
+2 -2
View File
@@ -106,8 +106,8 @@ if __name__ == '__main__':
parser = argparse.ArgumentParser(description=''' parser = argparse.ArgumentParser(description='''
This script takes two data files with timestamps and associates them 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('first_file', help='estimated trajectory (format: timestamp data)')
parser.add_argument('second_file', help='second text file (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('--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) parser.add_argument('--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)',default=0.02)
args = parser.parse_args() args = parser.parse_args()