def load_data(self, nm_gt, nm_est, nm_matches): """ Loads the trajectory data. The resuls {p_es, q_es, p_gt, q_gt} is synchronized and has the same length. """ if not os.path.exists(os.path.join(self.data_dir, nm_gt)) or \ not os.path.exists(os.path.join(self.data_dir, nm_est)): print(Fore.RED + "Either groundtruth or estimate does not exist") return False print(Fore.RED + 'Loading trajectory data...') # only timestamped pose series is supported self.t_es, self.p_es, self.q_es, self.t_gt, self.p_gt, self.q_gt =\ traj_loading.load_stamped_dataset( self.data_dir, nm_gt, nm_est, os.path.join(Trajectory.saved_res_dir_nm, self.est_type, nm_matches), start_t_sec=self.start_time_sec, end_t_sec=self.end_time_sec) self.t_gt_raw, self.p_gt_raw, self.q_gt_raw =\ traj_loading.load_raw_groundtruth(self.data_dir, nm_gt, start_t_sec=self.start_time_sec, end_t_sec=self.end_time_sec) if self.p_es.size == 0: print(Fore.RED + "Empty estimate file.") return False self.accum_distances = traj_utils.get_distance_from_start( self.p_gt_raw) self.traj_length = self.accum_distances[-1] self.accum_distances = traj_utils.get_distance_from_start(self.p_gt) if os.path.isfile(self.cached_rel_err_fn): print('Loading cached relative (odometry) errors from ' + self.cached_rel_err_fn) with open(self.cached_rel_err_fn, "rb") as f: self.rel_errors = pickle.load(f) print("Loaded odometry error calcualted at {0}".format( self.rel_errors.keys())) print(Fore.GREEN + '...done.') return True
def compute_accumulate_errors(self): self.accum_distances_est = traj_utils.get_distance_from_start( self.p_es) assert len(self.accum_distances_est) == len( self.accum_distances ), "estimation and groundtruth should have the same length" accum_distance_error = np.abs(self.accum_distances - self.accum_distances_est) accum_distances_error_ratio = accum_distance_error[ -1] / self.traj_length self.accum_errors['accum_distance_error'] = accum_distance_error self.accum_errors['accum_distance'] = self.accum_distances self.accum_errors[ 'accum_distance_error_ratio'] = accum_distances_error_ratio
def compute_relative_error(p_es, q_es, p_gt, q_gt, T_cm, dist, max_dist_diff, accum_distances=[], scale=1.0): if len(accum_distances) == 0: accum_distances = tu.get_distance_from_start(p_gt) comparisons = tu.compute_comparison_indices_length( accum_distances, dist, max_dist_diff) print('number of samples = '+str(len(comparisons))) T_mc = np.linalg.inv(T_cm) errors = [] 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) error_trans_norm = [] error_trans_perc = [] error_yaw = [] error_gravity = [] e_rot = [] for e in errors: tn = np.linalg.norm(e[0:3, 3]) error_trans_norm.append(tn) error_trans_perc.append(tn / dist * 100) ypr_angles = tf.euler_from_matrix(e, 'rzyx') e_rot.append(tu.compute_angle(e)) error_yaw.append(abs(ypr_angles[0])*180.0/np.pi) error_gravity.append( np.sqrt(ypr_angles[1]**2+ypr_angles[2]**2)*180.0/np.pi) return errors, np.array(error_trans_norm), np.array(error_trans_perc),\ np.array(error_yaw), np.array(error_gravity), np.array(e_rot)
def load_data(self): """ Loads the trajectory data. The resuls {p_es, q_es, p_gt, q_gt} is synchronized and has the same length. """ if self.data_loaded == True: print("Data already loaded") return print('Loading trajectory data...') # associate the estimation and groundtruth matches = associate_timestamps.read_files_and_associate( self.est_traj_file, self.gt_traj_file, self.time_offset, self.max_diff) dict_matches = dict(matches) data_es = np.loadtxt(self.est_traj_file) data_gt = np.loadtxt(self.gt_traj_file) self.p_es = [] self.p_gt = [] self.q_es = [] self.q_gt = [] self.t_gt = [] for es_id, es in enumerate(data_es): if es_id in dict_matches: gt = data_gt[dict_matches[es_id]] self.p_es.append(es[1:4]) self.p_gt.append(gt[1:4]) self.q_es.append(es[4:8]) self.q_gt.append(gt[4:8]) self.t_gt.append(gt[0]) self.p_es = np.array(self.p_es) self.p_gt = np.array(self.p_gt) self.q_es = np.array(self.q_es) self.q_gt = np.array(self.q_gt) self.t_gt = np.array(self.t_gt) self.accum_distances = traj_utils.get_distance_from_start(self.p_gt) self.traj_length = self.accum_distances[-1] self.data_loaded = True print('...done.')
def load_data(self): """ Loads the trajectory data. The resuls {p_es, q_es, p_gt, q_gt} is synchronized and has the same length. """ print('Loading trajectory data...') # only timestamped pose series is supported self.t_es, self.p_es, self.q_es, self.t_gt, self.p_gt, self.q_gt =\ traj_loading.load_stamped_dataset(self.data_dir) self.accum_distances = traj_utils.get_distance_from_start(self.p_gt) self.traj_length = self.accum_distances[-1] if os.path.isfile(self.cached_rel_err_fn): print('Loading cached relative (odometry) errors from ' + self.cached_rel_err_fn) with open(self.cached_rel_err_fn) as f: self.rel_errors = pickle.load(f) print("Loaded odometry error calcualted at {0}".format( self.rel_errors.keys())) self.data_loaded = True print('...done.')