From b49e43635671575e7fc8a0ae68df0766718d40d3 Mon Sep 17 00:00:00 2001 From: Christian Forster Date: Tue, 22 Apr 2014 18:35:45 +0200 Subject: [PATCH] plotting trajectory error --- svo_analysis/scripts/benchmark.py | 14 +- .../src/svo_analysis/analyse_trajectory.py | 317 +++++++++--------- 2 files changed, 171 insertions(+), 160 deletions(-) mode change 100644 => 100755 svo_analysis/src/svo_analysis/analyse_trajectory.py diff --git a/svo_analysis/scripts/benchmark.py b/svo_analysis/scripts/benchmark.py index 272cbce..c66f115 100755 --- a/svo_analysis/scripts/benchmark.py +++ b/svo_analysis/scripts/benchmark.py @@ -29,18 +29,28 @@ def run_experiment(dataset, params): algo_params = yaml.load(open(algo_params_file,'r')) # combine all parameters - if dataset_params['rig_size'] == 1: + if 'rig_size' not in dataset_params or dataset_params['rig_size'] == 1: params = dict(params.items() + algo_params.items() + dataset_params['cam0'].items()) else: params = dict(params.items() + algo_params.items() + dataset_params.items()) if 'dataset_is_blender' in dataset_params: params['dataset_is_blender'] = dataset_params['dataset_is_blender'] + params['dataset_first_frame'] = dataset_params['dataset_first_frame'] # dump experiment params to file and copy the other parameter files: params_dump_file = os.path.join(params['trace_dir'],'params.yaml') with open(params_dump_file,'w') as outfile: outfile.write(yaml.dump(params, default_flow_style=False)) - shutil.copyfile(dataset_params_file, os.path.join(params['trace_dir'], 'dataset_params.yaml')) + shutil.copyfile(dataset_params_file, + os.path.join(params['trace_dir'], 'dataset_params.yaml')) + + # copy the groundtruth trajectory to the trace dir for later evaluation + if params['dataset_is_blender']: + shutil.copyfile(os.path.join(params['dataset_directory'], 'trajectory.txt'), + os.path.join(params['trace_dir'], 'groundtruth_matched.txt')) + else: + shutil.copyfile(os.path.join(params['dataset_directory'], 'groundtruth_matched.txt'), + os.path.join(params['trace_dir'], 'groundtruth_matched.txt')) # execute ros node node = ros_node.RosNode(args.version, args.executable) diff --git a/svo_analysis/src/svo_analysis/analyse_trajectory.py b/svo_analysis/src/svo_analysis/analyse_trajectory.py old mode 100644 new mode 100755 index f2a800e..a024b79 --- a/svo_analysis/src/svo_analysis/analyse_trajectory.py +++ b/svo_analysis/src/svo_analysis/analyse_trajectory.py @@ -1,179 +1,180 @@ -# -*- coding: utf-8 -*- +#!/usr/bin/python import os import yaml -from .tum_benchmark_tools import associate +import argparse +import numpy as np import matplotlib.pyplot as plt +import svo_analysis.tum_benchmark_tools.associate as associate import vikit_py.transformations as transformations import vikit_py.align_trajectory as align_trajectory -import numpy as np - -# plot options from matplotlib import rc rc('font',**{'family':'serif','serif':['Cardo']}) rc('text', usetex=True) def plot_translation_error(timestamps, translation_error, results_dir): - fig = plt.figure(figsize=(8, 2.5)) - ax = fig.add_subplot(111, xlabel='time [s]', ylabel='position drift [mm]', xlim=[0,timestamps[-1]-timestamps[0]+4]) - ax.plot(timestamps-timestamps[0], translation_error[:,0]*1000, 'r-', label='x') - ax.plot(timestamps-timestamps[0], translation_error[:,1]*1000, 'g-', label='y') - ax.plot(timestamps-timestamps[0], translation_error[:,2]*1000, 'b-', label='z') - ax.legend() - fig.tight_layout() - fig.savefig(results_dir+'/translation_error.pdf') + fig = plt.figure(figsize=(8, 2.5)) + ax = fig.add_subplot(111, xlabel='time [s]', ylabel='position drift [mm]', xlim=[0,timestamps[-1]-timestamps[0]+4]) + ax.plot(timestamps-timestamps[0], translation_error[:,0]*1000, 'r-', label='x') + ax.plot(timestamps-timestamps[0], translation_error[:,1]*1000, 'g-', label='y') + ax.plot(timestamps-timestamps[0], translation_error[:,2]*1000, 'b-', label='z') + ax.legend() + fig.tight_layout() + fig.savefig(results_dir+'/translation_error.pdf') def plot_rotation_error(timestamps, rotation_error, results_dir): - fig = plt.figure(figsize=(8, 2.5)) - ax = fig.add_subplot(111, xlabel='time [s]', ylabel='orientation drift [rad]', xlim=[0,timestamps[-1]-timestamps[0]+4]) - ax.plot(timestamps-timestamps[0], rotation_error[:,0], 'r-', label='yaw') - ax.plot(timestamps-timestamps[0], rotation_error[:,1], 'g-', label='pitch') - ax.plot(timestamps-timestamps[0], rotation_error[:,2], 'b-', label='roll') - ax.legend() - fig.tight_layout() - fig.savefig(results_dir+'/orientation_error.pdf') + fig = plt.figure(figsize=(8, 2.5)) + ax = fig.add_subplot(111, xlabel='time [s]', ylabel='orientation drift [rad]', xlim=[0,timestamps[-1]-timestamps[0]+4]) + ax.plot(timestamps-timestamps[0], rotation_error[:,0], 'r-', label='yaw') + ax.plot(timestamps-timestamps[0], rotation_error[:,1], 'g-', label='pitch') + ax.plot(timestamps-timestamps[0], rotation_error[:,2], 'b-', label='roll') + ax.legend() + fig.tight_layout() + fig.savefig(results_dir+'/orientation_error.pdf') -def analyse_trajectory(results_dir): - # plot translation error - 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) +def analyse_trajectory(results_dir, n_align_frames = 20, use_hand_eye_calib = True): - # 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) + 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) + + # 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 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: + 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 get_rigid_body_trafo(quat,trans): - T = transformations.quaternion_matrix(quat) - T[0:3,3] = trans - return T + 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. + """ + data_gt = open(os.path.join(results_dir, 'groundtruth_matched.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 = [(float(l[0]),l[1:]) for l in data_gt] + data_gt = dict(data_gt) + + data_es = open(os.path.join(results_dir, 'traj_estimate.txt')).read() + lines = data_es.replace(","," ").replace("\t"," ").split("\n") + data_es = np.array([[np.float(v.strip()) for v in line.split(" ") if v.strip()!=""] for line in lines if len(line)>0 and line[0]!="#"]) + data_es = [(float(l[0]),l[1:]) for l in data_es] + data_es = dict(data_es) + + matches = associate.associate(data_gt, data_es, 0.0, 0.0001) + p_gt = np.array([[np.float(value) for value in data_gt[a][0:3]] for a,b in matches]) + q_gt = np.array([[np.float(value) for value in data_gt[a][3:7]] for a,b in matches]) + p_es = np.array([[np.float(value) for value in data_es[b][0:3]] for a,b in matches]) + q_es = np.array([[np.float(value) for value in data_es[b][3:7]] for a,b in matches]) + t_gt = np.array([np.float(a) for a,b in matches]) + t_es = np.array([np.float(b) for a,b in matches]) + + # set start time to zero + start_time = min(t_es[0], t_gt[0]) + t_es -= start_time + t_gt -= start_time + return t_es, p_es, q_es, t_gt, p_gt, q_gt + if __name__ == '__main__': - # config - dataset='20131003_1937_nslam_i7_asl2_fast' - n_align_frames = 100 - save = True - results_dir = '../results/'+dataset - - # 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 = get_rigid_body_trafo(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 = align_trajectory.align_sim3(p_gt, p_es, n_align_frames) - print 'scale = '+str(scale) - - # get trafo between (v)ision and (o)ptitrack frame - 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) - - 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 = 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') - - 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) - plot_translation_error(t_es, translation_error, results_dir) - - if save: - 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) - plot_rotation_error(t_es, orientation_error, results_dir) - - if save: - 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() + # parse command line + parser = argparse.ArgumentParser(description=''' + Analyse trajectory + ''') + parser.add_argument('results_dir', help='folder with the results') + parser.add_argument('--use_hand_eye_calib', help='', action='store_true') + parser.add_argument('--n_align_frames', help='', default=20) + args = parser.parse_args() + print('analyse trajectory for dataset: '+str(args.results_dir)) + analyse_trajectory(args.results_dir, + n_align_frames = int(args.n_align_frames), + use_hand_eye_calib = args.use_hand_eye_calib) \ No newline at end of file