def compute_velocity(self): #print("pos diff: " , np.diff(self.p_gt,axis=0)) #print("t diff: " , np.diff(self.t_gt,axis=0)) pos_diff = np.diff(self.p_gt,axis=0) q_diff = np.diff(self.q_es_aligned,axis=0) t_diff = np.diff(self.t_gt,axis=0) lin_vel = np.zeros(np.shape(pos_diff)) lin_vel_norm = np.zeros(np.shape(pos_diff)[0]) angle_ypr = np.zeros(np.shape(self.p_gt)) angle_vel = np.zeros(np.shape(pos_diff)) angle_vel_norm = np.zeros(np.shape(pos_diff)[0]) for i in range(np.shape(pos_diff)[0]): lin_vel[i,:] = pos_diff[i,:]/t_diff[i] lin_vel_norm[i] = np.linalg.norm(lin_vel[i,:]) angle_ypr[i,:] = tf.euler_from_quaternion(self.q_es_aligned[i, :]) angle_ypr_diff = np.diff(angle_ypr,axis=0) for i in range(np.shape(pos_diff)[0]): angle_vel[i,:] = angle_ypr_diff[i,:]/t_diff[i] angle_vel_norm[i] = np.linalg.norm(angle_vel[i,:]) stats_lin_vel = res_writer.compute_statistics(lin_vel_norm) stats_ang_vel = res_writer.compute_statistics(angle_vel_norm) self.velocity['lin_vel'] = stats_lin_vel self.velocity['ang_vel'] = stats_ang_vel
def compute_relative_error_at_subtraj_len(self, subtraj_len, max_dist_diff=-1): if max_dist_diff < 0: max_dist_diff = 0.2 * subtraj_len if self.rel_errors and (subtraj_len in self.rel_errors): print("Relative error at sub-trajectory length {0} is already " "computed or loaded from cache.".format(subtraj_len)) else: print("Computing relative error at sub-trajectory " "length {0}".format(subtraj_len)) Tcm = np.identity(4) _, e_trans, e_trans_perc, e_yaw, e_gravity, e_rot =\ traj_err.compute_relative_error( self.p_es, self.q_es, self.p_gt, self.q_gt, Tcm, subtraj_len, max_dist_diff, self.accum_distances, self.scale) dist_rel_err = {'rel_trans': e_trans, 'rel_trans_stats': res_writer.compute_statistics(e_trans), 'rel_trans_perc': e_trans_perc, 'rel_trans_perc_stats': res_writer.compute_statistics(e_trans_perc), 'rel_rot': e_rot, 'rel_rot_stats': res_writer.compute_statistics(e_rot), 'rel_yaw': e_yaw, 'rel_yaw_stats': res_writer.compute_statistics(e_yaw), 'rel_gravity': e_gravity, 'rel_gravity_stats': res_writer.compute_statistics(e_gravity)} self.rel_errors[subtraj_len] = dist_rel_err
def compute_absolute_error(self): if self.abs_errors: print("Absolute errors already calculated") else: print('Calculating RMSE...') # align trajectory if necessary self.align_trajectory() e_trans, e_trans_vec, e_rot, e_ypr, e_scale_perc =\ traj_err.compute_absolute_error(self.p_es_aligned, self.q_es_aligned, self.p_gt, self.q_gt) stats_trans = res_writer.compute_statistics(e_trans) stats_rot = res_writer.compute_statistics(e_rot) stats_scale = res_writer.compute_statistics(e_scale_perc) self.abs_errors['abs_e_trans'] = e_trans self.abs_errors['abs_e_trans_stats'] = stats_trans self.abs_errors['abs_e_trans_vec'] = e_trans_vec self.abs_errors['abs_e_rot'] = e_rot self.abs_errors['abs_e_rot_stats'] = stats_rot self.abs_errors['abs_e_ypr'] = e_ypr self.abs_errors['abs_e_scale_perc'] = e_scale_perc self.abs_errors['abs_e_scale_stats'] = stats_scale print('...RMSE calculated.') return
def updateStatistics(self): if self.n_traj == 0: return for et in self.kAbsMetrics: self.abs_errors[et + '_stats'] = rw.compute_statistics( np.array(self.abs_errors[et])) self.overall_rel_errors = {} for et in kRelMetrics: values = [] for d in self.rel_errors: self.rel_errors[d][et + '_stats'] = rw.compute_statistics( self.rel_errors[d][et]) values.extend(self.rel_errors[d][et].tolist()) self.overall_rel_errors[et] = rw.compute_statistics(values)
rel_e_distances, plot_settings, output_dir) else: print( "Skip plotting overall odometry error since datasets are evaluated at" " different distances.") rel_err_table = {} rel_err_table['values'] = [] for alg in algorithms: alg_aver_rel = [] for et in rel_err_names: cur_errors = [] for _, v in all_odo_err[et][alg].items(): cur_errors.extend(v) alg_aver_rel.append('{0:.3f}'.format( res_writer.compute_statistics(cur_errors)['mean'])) rel_err_table['values'].append(alg_aver_rel) rel_err_table['cols'] = rel_err_labels rel_err_table['rows'] = algorithms res_writer.write_tex_table( rel_err_table['values'], rel_err_table['rows'], rel_err_table['cols'], os.path.join(output_dir, args.platform + '_rel_err_' + eval_uid + '.txt')) if args.plot_trajectories: print(Fore.MAGENTA + '--- Plotting trajectory top and side view ... ---') plot_trajectories(dataset_trajectories_list, datasets, algorithms,