diff --git a/.gitignore b/.gitignore new file mode 100644 index 000000000..2f78cf5b6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +*.pyc + diff --git a/scripts/analyze_trajectories.py b/scripts/analyze_trajectories.py index b040254bf..5d3f4608d 100755 --- a/scripts/analyze_trajectories.py +++ b/scripts/analyze_trajectories.py @@ -440,11 +440,9 @@ def parse_config_file(config_fn, sort_names): os.makedirs(cur_res_dir) datasets_res_dir[d] = cur_res_dir same_subtraj = True if rel_e_distances else False - assert len(PALLETE) > len(algorithms),\ - "Not enough colors for all configurations" algo_colors = {} for i in range(len(algorithms)): - algo_colors[algorithms[i]] = PALLETE[i] + algo_colors[algorithms[i]] = PALLETE[i % len(PALLETE)] print(Fore.YELLOW+"=== Evaluation Configuration Summary ===") print(Fore.YELLOW+"Datasests to evaluate: ") diff --git a/src/rpg_trajectory_evaluation/compute_trajectory_errors.py b/src/rpg_trajectory_evaluation/compute_trajectory_errors.py index b10e42d4a..22c9b6afe 100644 --- a/src/rpg_trajectory_evaluation/compute_trajectory_errors.py +++ b/src/rpg_trajectory_evaluation/compute_trajectory_errors.py @@ -5,13 +5,14 @@ import os import numpy as np +import align_utils import trajectory_utils as tu import transformations as tf def compute_relative_error(p_es, q_es, p_gt, q_gt, T_cm, dist, max_dist_diff, accum_distances=[], - scale=1.0): + scale=1.0, method='pos_yaw_align', debug=False): if len(accum_distances) == 0: accum_distances = tu.get_distance_from_start(p_gt) @@ -30,48 +31,82 @@ def compute_relative_error(p_es, q_es, p_gt, q_gt, T_cm, dist, max_dist_diff, # IPython.embed() for idx, c in enumerate(comparisons): if not c == -1: - T_c1 = tu.get_rigid_body_trafo(q_es[idx, :], p_es[idx, :]) - T_c2 = tu.get_rigid_body_trafo(q_es[c, :], p_es[c, :]) - T_c1_c2 = np.dot(np.linalg.inv(T_c1), T_c2) - T_c1_c2[:3, 3] *= scale - - T_m1 = tu.get_rigid_body_trafo(q_gt[idx, :], p_gt[idx, :]) - T_m2 = tu.get_rigid_body_trafo(q_gt[c, :], p_gt[c, :]) - T_m1_m2 = np.dot(np.linalg.inv(T_m1), T_m2) - - T_m1_m2_in_c1 = np.dot(T_cm, np.dot(T_m1_m2, T_mc)) - T_error_in_c2 = np.dot(np.linalg.inv(T_m1_m2_in_c1), T_c1_c2) - T_c2_rot = np.eye(4) - T_c2_rot[0:3, 0:3] = T_c2[0:3, 0:3] - T_error_in_w = np.dot(T_c2_rot, np.dot( - T_error_in_c2, np.linalg.inv(T_c2_rot))) - errors.append(T_error_in_w) + p_gt_i = p_gt[idx:c, :] + q_gt_i = q_gt[idx:c, :] + p_es_i = p_es[idx:c, :] + q_es_i = q_es[idx:c, :] - plt.figure(0, figsize=(10, 5)) - plt.clf() - ax1 = plt.subplot(1, 2, 1) - ax1.plot(p_es[:, 0], p_es[:, 1]) - ax1.plot(p_gt[:, 0], p_gt[:, 1]) - ax1.plot(p_es[idx, 0], p_es[idx, 1], 'rx') - ax1.plot(p_es[c, 0], p_es[c, 1], 'rx') - ax1.plot(p_gt[idx, 0], p_gt[idx, 1], 'rx') - ax1.plot(p_gt[c, 0], p_gt[c, 1], 'rx') - ax1.set_aspect('equal') - - - ax2 = plt.subplot(1, 2, 2) - gt_portion = p_gt[idx:c, :] - gt_al0 = np.dot(np.linalg.inv(T_m1)[:3, :3], gt_portion.T) + np.linalg.inv(T_m1)[:3, 3].reshape([3, 1]) - es_portion = p_es[idx:c, :] - es_al0 = np.dot(np.linalg.inv(T_c1)[:3, :3], es_portion.T) + np.linalg.inv(T_c1)[:3, 3].reshape([3, 1]) - ax2.plot(es_al0[0, :], es_al0[1, :], label='es') - ax2.plot(gt_al0[0, :], gt_al0[1, :], label='gt') - ax2.axis([-10, 10, -10, 10]) - ax2.set_aspect('equal') - plt.legend() - plt.savefig('debug/%04d.png' % idx) - - #IPython.embed() + if method == 'initial_pose': + T_c1 = tu.get_rigid_body_trafo(q_es[idx, :], p_es[idx, :]) + T_c2 = tu.get_rigid_body_trafo(q_es[c, :], p_es[c, :]) + T_c1_c2 = np.dot(np.linalg.inv(T_c1), T_c2) + T_c1_c2[:3, 3] *= scale + + T_m1 = tu.get_rigid_body_trafo(q_gt[idx, :], p_gt[idx, :]) + T_m2 = tu.get_rigid_body_trafo(q_gt[c, :], p_gt[c, :]) + T_m1_m2 = np.dot(np.linalg.inv(T_m1), T_m2) + + T_m1_m2_in_c1 = np.dot(T_cm, np.dot(T_m1_m2, T_mc)) + T_error_in_c2 = np.dot(np.linalg.inv(T_m1_m2_in_c1), T_c1_c2) + T_c2_rot = np.eye(4) + T_c2_rot[0:3, 0:3] = T_c2[0:3, 0:3] + T_error_in_w = np.dot(T_c2_rot, np.dot( + T_error_in_c2, np.linalg.inv(T_c2_rot))) + errors.append(T_error_in_w) + + if debug: + gt_al0 = np.dot(np.linalg.inv(T_m1)[:3, :3], p_gt_i.T) + np.linalg.inv(T_m1)[:3, 3].reshape([3, 1]) + es_al0 = np.dot(np.linalg.inv(T_c1)[:3, :3], p_es_i.T) + np.linalg.inv(T_c1)[:3, 3].reshape([3, 1]) + + else: + assert method == 'pos_yaw_align' + # IPython.embed() + s, R_gt_es, t_gt_es = align_utils.alignTrajectory(p_es_i, p_gt_i, q_es_i, q_gt_i, method='posyaw') + assert s == 1. + p_es_i_aligned = (np.dot(R_gt_es, p_es_i.T) + t_gt_es.reshape((3, 1))).T + T_error_dummy = np.eye(4) + e_trans_vec = p_gt_i - p_es_i_aligned + T_error_dummy[0, 3] = np.sqrt(np.mean(np.sum(e_trans_vec**2, 1))) + errors.append(T_error_dummy) + + if debug: + T_m1 = tu.get_rigid_body_trafo(q_gt[idx, :], p_gt[idx, :]) + gt_al0 = np.dot(np.linalg.inv(T_m1)[:3, :3], p_gt_i.T) + np.linalg.inv(T_m1)[:3, 3].reshape([3, 1]) + es_al0 = np.dot(np.linalg.inv(T_m1)[:3, :3], p_es_i_aligned.T) + np.linalg.inv(T_m1)[:3, 3].reshape([3, 1]) + + + + if debug: + plt.figure(0, figsize=(15, 5)) + plt.clf() + ax1 = plt.subplot(1, 3, 1) + ax1.plot(p_es[:, 0], p_es[:, 1]) + ax1.plot(p_gt[:, 0], p_gt[:, 1]) + ax1.plot(p_es[idx, 0], p_es[idx, 1], 'rx') + ax1.plot(p_es[c, 0], p_es[c, 1], 'rx') + ax1.plot(p_gt[idx, 0], p_gt[idx, 1], 'rx') + ax1.plot(p_gt[c, 0], p_gt[c, 1], 'rx') + ax1.set_title('Full traj., sample source') + ax1.set_aspect('equal') + + + ax2 = plt.subplot(1, 3, 2) + ax2.plot(es_al0[0, :], es_al0[1, :], label='es') + ax2.plot(gt_al0[0, :], gt_al0[1, :], label='gt') + # ax2.axis([-dist, dist, -dist, dist]) + ax2.set_aspect('equal') + ax2.set_title('Error is %f m' % np.linalg.norm(errors[-1][:3, 3])) + plt.legend() + + ax3 = plt.subplot(1, 3, 3) + x = range(len(comparisons)) + y = [np.linalg.norm(e[0:3, 3]) for e in errors] + (len(comparisons) - len(errors)) * [None] + ax3.plot(x, y) + ax3.set_title('All errors') + + if not os.path.exists('debug'): + os.makedirs('debug') + plt.savefig('debug/%04d.png' % idx) error_trans_norm = [] error_trans_perc = [] @@ -89,7 +124,6 @@ def compute_relative_error(p_es, q_es, p_gt, q_gt, T_cm, dist, max_dist_diff, error_gravity.append( np.sqrt(ypr_angles[1]**2+ypr_angles[2]**2)*180.0/np.pi) e_rot_deg_per_m.append(e_rot[-1] / dist) - IPython.embed() return errors, np.array(error_trans_norm), np.array(error_trans_perc),\ np.array(error_yaw), np.array(error_gravity), np.array(e_rot),\ np.array(e_rot_deg_per_m) diff --git a/src/rpg_trajectory_evaluation/trajectory_utils.py b/src/rpg_trajectory_evaluation/trajectory_utils.py index 420b8aa33..965a0bc62 100644 --- a/src/rpg_trajectory_evaluation/trajectory_utils.py +++ b/src/rpg_trajectory_evaluation/trajectory_utils.py @@ -34,9 +34,6 @@ def compute_comparison_indices_length(distances, dist, max_dist_diff): error = np.abs(distances[i] - (d+dist)) if best_idx != -1: comparisons.append(best_idx) - print("%d to %d" % (idx, best_idx)) - else: - print("No best match for index %d!" % idx) return comparisons