diff --git a/svo_analysis/scripts/comparison.py b/svo_analysis/scripts/comparison.py index f2ac8a7..a502ad1 100755 --- a/svo_analysis/scripts/comparison.py +++ b/svo_analysis/scripts/comparison.py @@ -40,6 +40,13 @@ def distances_along_trajectory(traj): distances.append(sum) return distances +def get_distance_from_start(gt): + distances = np.diff(gt[:,1:4],axis=0) + distances = np.sqrt(np.sum(np.multiply(distances,distances),1)) + distances = np.cumsum(distances) + distances = np.concatenate(([0], distances)) + return distances + def compare_results(comp_params, results_dir, comparison_dir): print('run comparison: '+comp_params['comparison_name']) @@ -71,10 +78,7 @@ def compare_results(comp_params, results_dir, comparison_dir): for i, exp in enumerate(exp_set['experiments']): gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt')) - 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)) + distances = get_distance_from_start(gt) data = np.loadtxt(os.path.join(results_dir, exp, 'translation_error.txt')) e = np.sqrt(np.sum(np.multiply(data[:,1:4],data[:,1:4]),1)) @@ -100,21 +104,30 @@ def compare_results(comp_params, results_dir, comparison_dir): # ------------------------------------------------------------------------- # plot depth estimation error: 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']): - print('plot depth error for experiment: ' + exp_set['label']) - - for i, exp in enumerate(exp_set['experiments']): - - D = np.loadtxt(os.path.join(results_dir, exp, 'depth_error.txt'), delimiter=' ') - D = D[np.logical_and(D[:,0] > 1, D[:,0] < D[-1,0]/4),:] - errors = np.abs(D[:,1]) - if i == 0: - base_plot = analyse_depth.precision_plot(ax, errors, 0.05, line_colors[k], line_styles[np.mod(i, len(line_styles))]) - else: - base_plot = analyse_depth.precision_plot(ax, errors, 0.05, base_plot.get_color(), line_styles[np.mod(i, len(line_styles))]) - - save_figure(fig, 'depth_error', comparison_dir) + print('plot depth error for experiment: ' + exp) + exp = exp_set['experiments'][0] + gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt')) + x_axis_data = get_distance_from_start(gt) + analyse_depth.plot_depth_over_time(os.path.join(results_dir, exp), ax, + x_axis_data[1:], line_colors[k], exp_set['label']) + ax.legend(loc='upper left', ncol=3) + save_figure(fig, 'depth_error_textures', comparison_dir) + + fig = plt.figure(figsize=(6,5)) + ax = fig.add_subplot(111, xlabel='Travelled distance [m]', ylabel='Error [m]') + exp_set = comp_params['experiment_sets'][0] + for i, exp in enumerate(exp_set['experiments']): + print('plot depth error for speed: ' + exp) + gt = np.loadtxt(os.path.join(results_dir, exp, 'groundtruth_matched.txt')) + x_axis_data = get_distance_from_start(gt) + params = yaml.load(open(os.path.join(results_dir, exp, 'dataset_params.yaml'))) + analyse_depth.plot_depth_over_time(os.path.join(results_dir, exp), ax, + x_axis_data[1:], line_colors[i], + str(params['trajectory_modifiers']['speed'])+' m/s') + ax.legend(loc='upper left', ncol=3) + save_figure(fig, 'depth_error_speed', comparison_dir) if __name__ == '__main__': diff --git a/svo_analysis/src/svo_analysis/analyse_depth.py b/svo_analysis/src/svo_analysis/analyse_depth.py index 4ce2b5c..8a21b53 100755 --- a/svo_analysis/src/svo_analysis/analyse_depth.py +++ b/svo_analysis/src/svo_analysis/analyse_depth.py @@ -32,33 +32,35 @@ def analyse_depth(results_dir): fig.tight_layout() 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 = 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]) - percentile_20 = np.zeros(len(idxs)) + percentile_10 = 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)): 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_80[i] = np.percentile(errors, 80) - - base_plot, = ax.plot(idxs, percentile_50, linewidth=2) - ax.fill_between(idxs, percentile_20, percentile_80, alpha=0.25, color=base_plot.get_color()) - fig.tight_layout() - fig.savefig(os.path.join(results_dir,'scale_drift.pdf'), bbox_inches="tight") - - - + 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) - analyse_depth_over_time(args.results_dir) \ No newline at end of file + + fig = plt.figure(figsize=(6,5)) + ax = fig.add_subplot(111, xlabel='Measurement', ylabel='Scale-Drift') + plot_depth_over_time(args.results_dir, [], ax) \ No newline at end of file diff --git a/svo_analysis/src/svo_analysis/analyse_trajectory.py b/svo_analysis/src/svo_analysis/analyse_trajectory.py index 15823d0..c2c93de 100755 --- a/svo_analysis/src/svo_analysis/analyse_trajectory.py +++ b/svo_analysis/src/svo_analysis/analyse_trajectory.py @@ -32,117 +32,119 @@ def plot_rotation_error(timestamps, rotation_error, results_dir): fig.tight_layout() fig.savefig(results_dir+'/orientation_error.pdf') -def analyse_trajectory(results_dir, n_align_frames = 200, use_hand_eye_calib = True): - - if not os.path.isfile(os.path.join(results_dir, 'translation_error.txt')): - - T_cm, T_mc = load_hand_eye_calib(results_dir) - print('loading datasets.') - 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) +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) - # 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 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) - # 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') - +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) - - else: - 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) + 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_hand_eye_calib(results_dir): - P = yaml.load(open(os.path.join(results_dir, 'dataset_params.yaml'),'r'))['hand_eye_calib'] - - T_cm_quat = np.array([P['Tcm_qx'], P['Tcm_qy'], P['Tcm_qz'], P['Tcm_qw']]) - 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. - """ + +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_matched.txt')).read() + 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 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 = 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 = 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]) 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]) @@ -174,7 +176,7 @@ if __name__ == '__main__': ''') 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=20) + parser.add_argument('--n_align_frames', help='', default=200) args = parser.parse_args() print('analyse trajectory for dataset: '+str(args.results_dir)) diff --git a/svo_analysis/src/svo_analysis/hand_eye_calib.py b/svo_analysis/src/svo_analysis/hand_eye_calib.py index f0eae14..a54b4f8 100644 --- a/svo_analysis/src/svo_analysis/hand_eye_calib.py +++ b/svo_analysis/src/svo_analysis/hand_eye_calib.py @@ -4,41 +4,37 @@ Created on Sun Aug 4 15:47:55 2013 @author: cforster """ - +import os import yaml import numpy as np -import associate -import rotation_utils as ru +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 = '20130906_2149_ptam_i7_asl2' -n_measurements = 1000 -delta = 20 +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 -dataset_dir = '../results/'+dataset -params_stream = open(dataset_dir+'/params.yaml') -params = yaml.load(params_stream) +params = yaml.load(open(os.path.join(dataset_dir, 'dataset_params.yaml'))) # 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) +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 = -dataset_param['cam_delay'] -offset = -0.03 +offset = -params['cam_delay'] +print ('offset = '+str(offset)) 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]) 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]) @@ -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 -scale,rot,trans = align_sim3(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 +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) +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() +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(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) +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(ru.dcm2quat(R)) +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]) @@ -78,15 +93,15 @@ 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,:]) + 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 = ru.quat2dcm(q_gt[i-delta,:]) - B2 = ru.quat2dcm(q_gt[i,:]) + 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,:] = ru.dcm2rpy(B) - rpy_es[i-delta,:] = ru.dcm2rpy(B_es) + 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 diff --git a/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py b/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py index 01bea59..361cdd5 100755 --- a/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py +++ b/svo_analysis/src/svo_analysis/tum_benchmark_tools/associate.py @@ -106,8 +106,8 @@ if __name__ == '__main__': 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_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()