def ape(traj_ref: PosePath3D, traj_est: PosePath3D, pose_relation: metrics.PoseRelation, align: bool = False, correct_scale: bool = False, n_to_align: int = -1, align_origin: bool = False, ref_name: str = "reference", est_name: str = "estimate") -> Result: # Align the trajectories. only_scale = correct_scale and not align alignment_transformation = None if align or correct_scale: logger.debug(SEP) alignment_transformation = lie_algebra.sim3( *traj_est.align(traj_ref, correct_scale, only_scale, n=n_to_align)) elif align_origin: logger.debug(SEP) alignment_transformation = traj_est.align_origin(traj_ref) # Calculate APE. logger.debug(SEP) data = (traj_ref, traj_est) ape_metric = metrics.APE(pose_relation) ape_metric.process_data(data) title = str(ape_metric) if align and not correct_scale: title += "\n(with SE(3) Umeyama alignment)" elif align and correct_scale: title += "\n(with Sim(3) Umeyama alignment)" elif only_scale: title += "\n(scale corrected)" elif align_origin: title += "\n(with origin alignment)" else: title += "\n(not aligned)" if (align or correct_scale) and n_to_align != -1: title += " (aligned poses: {})".format(n_to_align) ape_result = ape_metric.get_result(ref_name, est_name) ape_result.info["title"] = title logger.debug(SEP) logger.info(ape_result.pretty_str()) ape_result.add_trajectory(ref_name, traj_ref) ape_result.add_trajectory(est_name, traj_est) if isinstance(traj_est, PoseTrajectory3D): seconds_from_start = np.array( [t - traj_est.timestamps[0] for t in traj_est.timestamps]) ape_result.add_np_array("seconds_from_start", seconds_from_start) ape_result.add_np_array("timestamps", traj_est.timestamps) if alignment_transformation is not None: ape_result.add_np_array("alignment_transformation_sim3", alignment_transformation) return ape_result
def get_ape_static_raw(data, mode): ape_metric = metrics.APE(mode) ape_metric.process_data(data) ape_stat = np.asarray(list( ape_metric.get_all_statistics().values())) #总体误差统计值 error_full = ape_metric.error #单个误差 return ape_stat, error_full
def stats_to_latex_table(traj_ref, segments, idx, table): """Associate segments of an object trajectory as given by a DATMO system with the object's reference trajectory :traj_ref: Reference trajectory :segments: All the segments of the robot trajectory :table: Latex table that the statistics get added to """ whole = trajectory.merge(segments) traj_ref, traj_est = sync.associate_trajectories(traj_ref, whole, max_diff=0.01) data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() # print(traj_est.get_infos()) table.add_row(( idx + 1, round(ape_statistics["rmse"], 3), round(ape_statistics["mean"], 3), round(ape_statistics["median"], 3), round(ape_statistics["std"], 3), round(ape_statistics["min"], 3), round(ape_statistics["max"], 3), round(ape_statistics["sse"], 3), )) table.add_hline
def calc_euroc_seq_errors(est_traj, gt_traj): gt_traj_synced, est_traj_synced = sync.associate_trajectories( gt_traj, est_traj, max_diff=0.01) est_traj_aligned = trajectory.align_trajectory(est_traj_synced, gt_traj_synced, correct_scale=False, correct_only_scale=False) pose_relation = metrics.PoseRelation.translation_part ape_metric = metrics.APE(pose_relation) ape_metric.process_data(( gt_traj_synced, est_traj_aligned, )) ape_stat = ape_metric.get_statistic(metrics.StatisticsType.rmse) # ape_metric = metrics.RPE(pose_relation) # ape_metric.process_data((gt_traj_synced, est_traj_aligned,)) # ape_stat = ape_metric.get_statistic(metrics.StatisticsType.rmse) # fig = plt.figure() # traj_by_label = { # "estimate (not aligned)": est_traj, # "estimate (aligned)": est_traj_aligned, # "reference": gt_traj # } # plot.trajectories(fig, traj_by_label, plot.PlotMode.xyz) # plt.show() return ape_metric, ape_stat
def process_trajectory_data(self, traj_ref, traj_est, segments, is_vio_traj=True): """ """ suffix = "VIO" if is_vio_traj else "PGO" data = (traj_ref, traj_est) evt.print_purple("Calculating APE translation part for " + suffix) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) evt.print_purple("Calculating RPE translation part for " + suffix) rpe_metric_trans = metrics.RPE(metrics.PoseRelation.translation_part, 1.0, metrics.Unit.frames, 0.0, False) rpe_metric_trans.process_data(data) evt.print_purple("Calculating RPE rotation angle for " + suffix) rpe_metric_rot = metrics.RPE(metrics.PoseRelation.rotation_angle_deg, 1.0, metrics.Unit.frames, 1.0, False) rpe_metric_rot.process_data(data) results = self.calc_results(ape_metric, rpe_metric_trans, rpe_metric_rot, data, segments) return (ape_metric, rpe_metric_trans, rpe_metric_rot, results)
def ape(traj_ref, traj_est, pose_relation, align=False, correct_scale=False, align_origin=False, ref_name="reference", est_name="estimate"): from evo.core import metrics from evo.core import trajectory # Align the trajectories. only_scale = correct_scale and not align if align or correct_scale: logger.debug(SEP) traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale, only_scale) elif align_origin: logger.debug(SEP) traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref) # Calculate APE. logger.debug(SEP) data = (traj_ref, traj_est) ape_metric = metrics.APE(pose_relation) ape_metric.process_data(data) title = str(ape_metric) if align and not correct_scale: title += "\n(with SE(3) Umeyama alignment)" elif align and correct_scale: title += "\n(with Sim(3) Umeyama alignment)" elif only_scale: title += "\n(scale corrected)" elif align_origin: title += "\n(with origin alignment)" else: title += "\n(not aligned)" ape_result = ape_metric.get_result(ref_name, est_name) ape_result.info["title"] = title logger.debug(SEP) logger.info(ape_result.pretty_str()) ape_result.add_trajectory(ref_name, traj_ref) ape_result.add_trajectory(est_name, traj_est) if isinstance(traj_est, trajectory.PoseTrajectory3D): seconds_from_start = [ t - traj_est.timestamps[0] for t in traj_est.timestamps ] ape_result.add_np_array("seconds_from_start", seconds_from_start) ape_result.add_np_array("timestamps", traj_est.timestamps) return ape_result
def associate_segments_common_frame(traj, tracks, distance): """Associate segments of an object trajectory as given by a DATMO system with the object's reference trajectory :traj: Reference trajectory :tracks: All the tracks that got produced by the DATMO system :localization: The trajectory of the self-localization :returns: segments: The tracks that match to the reference trajectory :returns: traj_ref: The part of the reference trajectory that matches with tracks """ matches = [] for tr in tracks: # Find the best matching tracks to the object trajectory traj_ref, traj_est = sync.associate_trajectories(traj, tr, max_diff=0.1) # print("calculating APE for track of length", len(tr.timestamps)) data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() # print(ape_statistics) mismatch = ape_statistics['mean'] # print(mismatch) tuple = [ traj_est, mismatch, traj_est.get_infos()['t_start (s)'], traj_ref ] matches.append(tuple) matches.sort(key=lambda x: x[2]) segments_track = [] #The parts of the trajectory are added to this list segments_refer = [ ] #The parts of the reference trajectory are added to this list for m in matches: if m[1] < distance: # if the mismatch is smaller than 1 # print(m[1],distance) # print(m[0].get_statistics()['v_avg (m/s)']) segments_track.append(m[0]) segments_refer.append(m[3]) # print(m[0].get_infos()['t_start (s)'],m[0].get_infos()["path length (m)"]) # print(m[0].get_statistics()['v_avg (m/s)']) if len(segments_track) == 0: print("No matching segments") traj_ref = trajectory.merge(segments_refer) # print(traj_ref.length) return segments_track, traj_ref
def get_ape_rot(data): """ Return APE rotation metric for input data. Args: data: A 2-tuple containing the reference trajectory and the estimated trajectory as PoseTrajectory3D objects. Returns: A metrics object containing the desired results. """ ape_rot = metrics.APE(metrics.PoseRelation.rotation_angle_deg) ape_rot.process_data(data) return ape_rot
def get_ape_trans(data): """ Return APE translation metric for input data. Args: data: A 2-tuple containing the reference trajectory and the estimated trajectory as PoseTrajectory3D objects. Returns: A metrics object containing the desired results. """ ape_trans = metrics.APE(metrics.PoseRelation.translation_part) ape_trans.process_data(data) return ape_trans
def original_ape(traj_ref, traj_est, pose_relation, align=False, correct_scale=False, align_origin=False, ref_name="reference", est_name="estimate"): ''' Copied from main_ape.py ''' traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est) # Align the trajectories. only_scale = correct_scale and not align if align or correct_scale: traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale, only_scale) elif align_origin: traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref) # Calculate APE. data = (traj_ref, traj_est) ape_metric = metrics.APE(pose_relation) ape_metric.process_data(data) title = str(ape_metric) if align and not correct_scale: title += "\n(with SE(3) Umeyama alignment)" elif align and correct_scale: title += "\n(with Sim(3) Umeyama alignment)" elif only_scale: title += "\n(scale corrected)" elif align_origin: title += "\n(with origin alignment)" else: title += "\n(not aligned)" ape_result = ape_metric.get_result(ref_name, est_name) ape_result.info["title"] = title ape_result.add_trajectory(ref_name, traj_ref) ape_result.add_trajectory(est_name, traj_est) if isinstance(traj_est, trajectory.PoseTrajectory3D): seconds_from_start = [ t - traj_est.timestamps[0] for t in traj_est.timestamps ] ape_result.add_np_array("seconds_from_start", seconds_from_start) ape_result.add_np_array("timestamps", traj_est.timestamps) return ape_result
def three_plots(ref, est, table, name): """Generates plots and statistics table into Report :ref: PoseTrajectory3D object that is used as reference :est: PoseTrajectory3D object that is plotted against reference :table: Tabular object that is generated by Tabular('c c') :name: String that is used as name for file and table entry :returns: translation of reference against estimation """ ref, est = sync.associate_trajectories(ref, est) est, rot, tra, s = trajectory.align_trajectory(est, ref, correct_scale=False, return_parameters=True) data = (ref, est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() # [ Localization ] fig, axarr = plt.subplots(3) #sharex=True) fig.suptitle('Localization', fontsize=30) fig.tight_layout() plot.traj_xyyaw(axarr, est, '-', 'red', 'estimation', 1, ref.timestamps[0]) plot.traj_xyyaw(axarr, ref, '-', 'gray', 'original') fig.subplots_adjust(hspace=0.2) plt.waitforbuttonpress(0) plt.savefig("/home/kostas/results/latest/" + name + ".png", format='png', bbox_inches='tight') plt.close(fig) table.add_row(( name, round(ape_statistics["rmse"], 3), round(ape_statistics["mean"], 3), round(ape_statistics["median"], 3), round(ape_statistics["std"], 3), round(ape_statistics["min"], 3), round(ape_statistics["max"], 3), round(ape_statistics["sse"], 3), )) table.add_hline
def compare_using_APE(ref_file, est_file, use_aligned_trajectories=False): """ Compare two files using EVO API. Using the APE metric. :param ref_file: :param est_file: :param use_aligned_trajectories: True to align before comparing. False to leave original data as it is. :return: """ # Load trajectories traj_ref = file_interface.read_tum_trajectory_file(ref_file) traj_est = file_interface.read_tum_trajectory_file(est_file) # Sinchronize trajectories by timestamps max_diff = 0.01 traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, max_diff) # -------------EVO_APE------------- # Settings pose_relation = metrics.PoseRelation.translation_part use_aligned_trajectories = False # OPTION -va on the scripts. Is related to Uleyamas alignment # The aligned trajectories can be used if we want it to if use_aligned_trajectories: # Align trajectories with Uleyamas algorithm try: traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False, correct_only_scale=False) data = (traj_ref, traj_est_aligned) except GeometryException: print("Couldnt align with Uleyamas algorithm...") data = (traj_ref, traj_est) else: data = (traj_ref, traj_est) ape_metric = metrics.APE(pose_relation) # APE with only pose is in reality ATE (Absolute trajectory error) instead of APE (abs. pose error) ape_metric.process_data(data) # Get all stadistics in a dict ape_stats = ape_metric.get_all_statistics() return ape_stats
def four_plots(ref, est, table, name): """Generates plots and statistics table into Report :ref: PoseTrajectory3D object that is used as reference :est: PoseTrajectory3D object that is plotted against reference :table: Tabular object that is generated by Tabular('c c') :name: String that is used as name for file and table entry :returns: translation of reference against estimation """ ref, est = sync.associate_trajectories(ref, est) # est, rot, tra, s = trajectory.align_trajectory(est, # ref, correct_scale=False, return_parameters=True) est = trajectory.align_trajectory_origin(est, ref) data = (ref, est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() # Plot x, y, xy,yaw style = '-' if name == 'slam': style = 'o' plt.style.use(['seaborn-whitegrid', 'stylerc']) mpl.use('pgf') mpl.rcParams.update({ "text.usetex": True, "pgf.texsystem": "pdflatex", }) fig, axarr = plt.subplots(2, 2, figsize=(6.125, 4.8)) plot.traj_fourplots(axarr, est, style, sns.xkcd_rgb["pale red"], 'Estimation', 1, ref.timestamps[0]) plot.traj_fourplots(axarr, ref, '-', 'gray', 'MoCap Ground Truth') handles, labels = axarr[0, 0].get_legend_handles_labels() fig.legend(handles, labels, loc='lower center', ncol=2, bbox_to_anchor=(0.5, 0)) plt.tight_layout() fig.tight_layout() fig.subplots_adjust(bottom=0.18) fig.savefig("/home/kostas/report/figures/localization/" + name + ".pgf") if name == 'slam': name = name.upper() elif name == 'odometry': name = 'Odometry+IMU' else: name = name.capitalize() table.add_row(( name, round(ape_statistics["rmse"], 3), round(ape_statistics["mean"], 3), round(ape_statistics["median"], 3), round(ape_statistics["std"], 3), round(ape_statistics["min"], 3), round(ape_statistics["max"], 3), round(ape_statistics["sse"], 3), )) table.add_hline
gt_file = f'{gt_path}/{seq}.csv' failure_count = 0 if not len(traj_files) == 0: mean, rmse = [], [] for traj_file in traj_files: traj_gt = file_interface.read_euroc_csv_trajectory(gt_file) traj_est = file_interface.read_tum_trajectory_file(traj_file) kf_traj_est = file_interface.read_tum_trajectory_file(traj_file) traj_gt, traj_est = sync.associate_trajectories(traj_gt, traj_est) traj_est_aligned, rot, trans, scale = trajectory.align_trajectory( traj_est, traj_gt, correct_scale=True, return_parameters=True) data = (traj_gt, traj_est_aligned) ape_metric = metrics.APE(pose_relation=pose_relation) ape_metric.process_data(data) ape_stat = ape_metric.get_all_statistics() mean_curr = ape_stat['mean'] rmse_curr = ape_stat['rmse'] if mean_curr > 1.0 or rmse_curr > 1.0: failure_count += 1 continue mean.append(mean_curr) rmse.append(rmse_curr) print( f'{seq}: mean: {np.mean(mean)}, rmse: {np.mean(rmse)}, #failure {failure_count}' ) else:
def run_analysis(traj_ref_path, traj_est_path, segments, save_results, display_plot, save_plots, save_folder, confirm_overwrite=False, dataset_name="", discard_n_start_poses=0, discard_n_end_poses=0): """ Run analysis on given trajectories, saves plots on given path: :param traj_ref_path: path to the reference (ground truth) trajectory. :param traj_est_path: path to the estimated trajectory. :param save_results: saves APE, and RPE per segment results. :param save_plots: whether to save the plots. :param save_folder: where to save the plots. :param confirm_overwrite: whether to confirm overwriting plots or not. :param dataset_name: optional param, to allow setting the same scale on different plots. """ # Load trajectories. from evo.tools import file_interface traj_ref = None try: traj_ref = file_interface.read_euroc_csv_trajectory( traj_ref_path) # TODO make it non-euroc specific. except file_interface.FileInterfaceException as e: raise Exception( "\033[91mMissing ground truth csv! \033[93m {}.".format(e)) traj_est = None try: traj_est = file_interface.read_swe_csv_trajectory(traj_est_path) except file_interface.FileInterfaceException as e: log.info(e) raise Exception("\033[91mMissing vio output csv.\033[99m") evt.print_purple("Registering trajectories") traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est) evt.print_purple("Aligning trajectories") traj_est = trajectory.align_trajectory( traj_est, traj_ref, correct_scale=False, discard_n_start_poses=int(discard_n_start_poses), discard_n_end_poses=int(discard_n_end_poses)) num_of_poses = traj_est.num_poses traj_est.reduce_to_ids( range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1)) traj_ref.reduce_to_ids( range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1)) results = dict() evt.print_purple("Calculating APE translation part") data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_result = ape_metric.get_result() results["absolute_errors"] = ape_result log.info(ape_result.pretty_str(info=True)) # TODO(Toni): Save RPE computation results rather than the statistics # you can compute statistics later... evt.print_purple("Calculating RPE translation part for plotting") rpe_metric_trans = metrics.RPE(metrics.PoseRelation.translation_part, 1.0, metrics.Unit.frames, 0.0, False) rpe_metric_trans.process_data(data) rpe_stats_trans = rpe_metric_trans.get_all_statistics() log.info("mean: %f" % rpe_stats_trans["mean"]) evt.print_purple("Calculating RPE rotation angle for plotting") rpe_metric_rot = metrics.RPE(metrics.PoseRelation.rotation_angle_deg, 1.0, metrics.Unit.frames, 1.0, False) rpe_metric_rot.process_data(data) rpe_stats_rot = rpe_metric_rot.get_all_statistics() log.info("mean: %f" % rpe_stats_rot["mean"]) results["relative_errors"] = dict() # Read segments file for segment in segments: results["relative_errors"][segment] = dict() evt.print_purple("RPE analysis of segment: %d" % segment) evt.print_lightpurple("Calculating RPE segment translation part") rpe_segment_metric_trans = metrics.RPE( metrics.PoseRelation.translation_part, float(segment), metrics.Unit.meters, 0.01, True) rpe_segment_metric_trans.process_data(data) rpe_segment_stats_trans = rpe_segment_metric_trans.get_all_statistics() results["relative_errors"][segment][ "rpe_trans"] = rpe_segment_stats_trans # print(rpe_segment_stats_trans) # print("mean:", rpe_segment_stats_trans["mean"]) evt.print_lightpurple("Calculating RPE segment rotation angle") rpe_segment_metric_rot = metrics.RPE( metrics.PoseRelation.rotation_angle_deg, float(segment), metrics.Unit.meters, 0.01, True) rpe_segment_metric_rot.process_data(data) rpe_segment_stats_rot = rpe_segment_metric_rot.get_all_statistics() results["relative_errors"][segment]["rpe_rot"] = rpe_segment_stats_rot # print(rpe_segment_stats_rot) # print("mean:", rpe_segment_stats_rot["mean"]) if save_results: # Save results file results_file = os.path.join(save_folder, 'results.yaml') evt.print_green("Saving analysis results to: %s" % results_file) with open(results_file, 'w') as outfile: if confirm_overwrite: if evt.user.check_and_confirm_overwrite(results_file): outfile.write(yaml.dump(results, default_flow_style=False)) else: log.info("Not overwritting results.") else: outfile.write(yaml.dump(results, default_flow_style=False)) # For each segment in segments file # Calculate rpe with delta = segment in meters with all-pairs set to True # Calculate max, min, rmse, mean, median etc # Plot boxplot, or those cumulative figures you see in evo (like demographic plots) if display_plot or save_plots: evt.print_green("Plotting:") log.info(dataset_name) plot_collection = plot.PlotCollection("Example") # metric values fig_1 = plt.figure(figsize=(8, 8)) ymax = -1 if dataset_name is not "" and FIX_MAX_Y: ymax = Y_MAX_APE_TRANS[dataset_name] ape_statistics = ape_metric.get_all_statistics() plot.error_array( fig_1, ape_metric.error, statistics=ape_statistics, name="APE translation", title="" #str(ape_metric) , xlabel="Keyframe index [-]", ylabel="APE translation [m]", y_min=0.0, y_max=ymax) plot_collection.add_figure("APE_translation", fig_1) # trajectory colormapped with error fig_2 = plt.figure(figsize=(8, 8)) plot_mode = plot.PlotMode.xy ax = plot.prepare_axis(fig_2, plot_mode) plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference') plot.traj_colormap(ax, traj_est, ape_metric.error, plot_mode, min_map=0.0, max_map=math.ceil(ape_statistics['max'] * 10) / 10, title="ATE mapped onto trajectory [m]") plot_collection.add_figure("APE_translation_trajectory_error", fig_2) # RPE ## Trans ### metric values fig_3 = plt.figure(figsize=(8, 8)) if dataset_name is not "" and FIX_MAX_Y: ymax = Y_MAX_RPE_TRANS[dataset_name] plot.error_array( fig_3, rpe_metric_trans.error, statistics=rpe_stats_trans, name="RPE translation", title="" #str(rpe_metric_trans) , xlabel="Keyframe index [-]", ylabel="RPE translation [m]", y_max=ymax) plot_collection.add_figure("RPE_translation", fig_3) ### trajectory colormapped with error fig_4 = plt.figure(figsize=(8, 8)) plot_mode = plot.PlotMode.xy ax = plot.prepare_axis(fig_4, plot_mode) traj_ref_trans = copy.deepcopy(traj_ref) traj_ref_trans.reduce_to_ids(rpe_metric_trans.delta_ids) traj_est_trans = copy.deepcopy(traj_est) traj_est_trans.reduce_to_ids(rpe_metric_trans.delta_ids) plot.traj(ax, plot_mode, traj_ref_trans, '--', 'gray', 'Reference') plot.traj_colormap( ax, traj_est_trans, rpe_metric_trans.error, plot_mode, min_map=0.0, max_map=math.ceil(rpe_stats_trans['max'] * 10) / 10, title="RPE translation error mapped onto trajectory [m]") plot_collection.add_figure("RPE_translation_trajectory_error", fig_4) ## Rot ### metric values fig_5 = plt.figure(figsize=(8, 8)) if dataset_name is not "" and FIX_MAX_Y: ymax = Y_MAX_RPE_ROT[dataset_name] plot.error_array( fig_5, rpe_metric_rot.error, statistics=rpe_stats_rot, name="RPE rotation error", title="" #str(rpe_metric_rot) , xlabel="Keyframe index [-]", ylabel="RPE rotation [deg]", y_max=ymax) plot_collection.add_figure("RPE_rotation", fig_5) ### trajectory colormapped with error fig_6 = plt.figure(figsize=(8, 8)) plot_mode = plot.PlotMode.xy ax = plot.prepare_axis(fig_6, plot_mode) traj_ref_rot = copy.deepcopy(traj_ref) traj_ref_rot.reduce_to_ids(rpe_metric_rot.delta_ids) traj_est_rot = copy.deepcopy(traj_est) traj_est_rot.reduce_to_ids(rpe_metric_rot.delta_ids) plot.traj(ax, plot_mode, traj_ref_rot, '--', 'gray', 'Reference') plot.traj_colormap( ax, traj_est_rot, rpe_metric_rot.error, plot_mode, min_map=0.0, max_map=math.ceil(rpe_stats_rot['max'] * 10) / 10, title="RPE rotation error mapped onto trajectory [deg]") plot_collection.add_figure("RPE_rotation_trajectory_error", fig_6) if display_plot: evt.print_green("Displaying plots.") plot_collection.show() if save_plots: evt.print_green("Saving plots to: ") log.info(save_folder) # Config output format (pdf, eps, ...) using evo_config... plot_collection.export(os.path.join(save_folder, "plots.eps"), False) plot_collection.export(os.path.join(save_folder, "plots.pdf"), False)
def associate_segments(traj, tracks): """Associate segments of an object trajectory as given by a DATMO system with the object's reference trajectory :traj: Reference trajectory :tracks: All the tracks that got produced by the DATMO system :localization: The trajectory of the self-localization :returns: segments: The tracks that match to the reference trajectory :returns: traj_ref: The part of the reference trajectory that matches with tracks """ matches = [] for tr in tracks: # Find the best matching tracks to the object trajectory traj_ref, traj_est = sync.associate_trajectories(traj, tr, max_diff=0.01) traj_est, rot, tra, _ = trajectory.align_trajectory( traj_est, traj_ref, correct_scale=False, return_parameters=True) # print("calculating APE for track of length", len(tr.timestamps)) data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() tra_dif = (tra - loc_tra) # print(tra) abs_tra_dif = abs((tra - loc_tra)[0]) + abs((tra - loc_tra)[1]) translation = abs(tra[0]) + abs(tra[1]) rot_dif = (rot - loc_rot) abs_rot_dif = 0 for i in range(0, len(rot_dif)): abs_rot_dif += abs(rot_dif[i][0])+ abs(rot_dif[i][1]) +\ abs(rot_dif[i][2]) # print(abs_tra_dif,abs_rot_dif) mismatch = abs_tra_dif + abs_rot_dif tuple = [traj_est, mismatch, traj_est.get_infos()['t_start (s)']] matches.append(tuple) matches.sort(key=lambda x: x[2]) segments = [] #The parts of the trajectory are added to this list for m in matches: # print(m[1]) if m[1] < 0.1: # if the mismatch is smaller than 1 # print(m[0].get_statistics()['v_avg (m/s)']) segments.append(m[0]) # print(m[0].get_infos()['t_start (s)'],m[0].get_infos()["path length (m)"]) # print(m[0].get_statistics()['v_avg (m/s)']) if len(segments) == 0: print("No matching segments") whole = trajectory.merge(segments) traj_ref, traj_est = sync.associate_trajectories(traj, whole, max_diff=0.01) traj_est, rot, tra, _ = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False, return_parameters=True) # print(traj_est.get_infos()) return segments, traj_ref, translation
def get_and_save_results_from_folder(folder_with_predicted_poses,category): global args global kitti_eval_tool global folder_with_gt_poses global output_folder global t global results values_for_excel = [] columns_for_excel = [] type_of_statistics = 'mean' for filename in sorted(os.listdir(folder_with_predicted_poses)): if not(os.path.exists(os.path.join(folder_with_gt_poses, filename))): print("file with gt poses doesn't exist for "+filename) continue if filename.find('.txt') == -1: continue seq_results = {} seq_results['name_seq'] = filename[:filename.rfind('.')] seq_results['category'] = category folder_name = seq_results['category'] seq_results['metrics'] = {} seq_results['lost'] = False os.makedirs(os.path.join(output_folder, folder_name), exist_ok=True) output_folder_seq = os.path.join(output_folder, folder_name, filename[:filename.rfind('.')]) os.makedirs(output_folder_seq, exist_ok=True) if os.path.isfile(os.path.join(output_folder, folder_name,"results.txt")): file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "a") else: file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "w") file_results_txt.write("translation_error(%) rotation_error(deg/m) ATE(m) APE_translation_error_median(m) APE_rotation_error_median(deg) dst_to_trgt\n") # -------------------------------------Getting results--------------------------------------------------- if args.gt_format == 'kitti': traj_ref = file_interface.read_kitti_poses_file(os.path.join(folder_with_gt_poses, filename)) if args.gt_format == 'tum': traj_ref = file_interface.read_tum_trajectory_file(os.path.join(folder_with_gt_poses, filename)) seq_results["length_of_ref_traj"] = traj_ref.path_length end_time_gt = traj_ref.get_infos()["t_end (s)"] if args.gt_format == 'euroc': traj_ref = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_gt_poses, filename)) if args.result_format == 'kitti': traj_est = file_interface.read_kitti_poses_file(os.path.join(folder_with_predicted_poses, filename)) if args.result_format == 'tum': traj_est = file_interface.read_tum_trajectory_file(os.path.join(folder_with_predicted_poses, filename)) seq_results["length_of_estimated_traj"] = traj_est.path_length if args.result_format == 'euroc': traj_est = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_predicted_poses, filename)) if args.result_format == 'tum' and args.gt_format == 'tum': seq_results["num_gt_poses"] = traj_ref.num_poses seq_results["num_predicted_poses"] = traj_est.num_poses traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, args.max_diff) end_time_est = traj_est.get_infos()["t_end (s)"] if (abs(end_time_est - end_time_gt) > 0.2) or (traj_est.get_infos()["t_start (s)"] > 0.2): print('LOST in track '+filename[:filename.rfind('.')]) seq_results['lost'] = True results.append(seq_results) t.update(1) continue if args.alignment != None: traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=args.alignment.find("scale") != -1, correct_only_scale=args.alignment=="scale") trajectory.align_trajectory_origin(traj_est, traj_ref) data = (traj_ref, traj_est) ape_metric_translation = metrics.APE(metrics.PoseRelation.translation_part) ape_metric_rotation = metrics.APE(metrics.PoseRelation.rotation_angle_deg) ape_metric_translation.process_data(data) ape_metric_rotation.process_data(data) ape_translation_statistics = ape_metric_translation.get_all_statistics() ape_rotation_statistics = ape_metric_rotation.get_all_statistics() ape_translation_statistics_plot = copy.deepcopy(ape_translation_statistics) ape_rotation_statistics_plot = copy.deepcopy(ape_rotation_statistics) ape_translation_statistics_plot.pop('sse') ape_translation_statistics_plot.pop('std') ape_translation_statistics_plot.pop('min') ape_translation_statistics_plot.pop('max') ape_rotation_statistics_plot.pop('sse') ape_rotation_statistics_plot.pop('std') ape_rotation_statistics_plot.pop('min') ape_rotation_statistics_plot.pop('max') kitti_trans_err, kitti_rot_err, ate = kitti_eval_tool.eval(traj_ref.poses_se3, traj_est.poses_se3, alignment=None) #---------------------------------adding results to variable seq_results for excel ----------------------------- seq_results['metrics']['dist_to_trgt'] = traj_est.get_infos()['pos_end (m)'] - traj_ref.get_infos()['pos_end (m)'] seq_results['metrics']['dist_to_trgt'] = np.sum(np.array(seq_results['metrics']['dist_to_trgt'])**2)**0.5 seq_results['metrics']["Kitti trans err (%)"] = kitti_trans_err seq_results['metrics']["Kitti rot err (deg/m)"] = kitti_rot_err seq_results['metrics']["ATE (m)"] = ate seq_results['metrics']["APE(trans err) median (m)"] = ape_translation_statistics["median"] seq_results['metrics']["APE(rot err) median (deg)"] = ape_rotation_statistics["median"] #-------------------------------------------------------------------------------------------------------- #------------------------------------------------------------------------------------------------------- # --------------------------------printing results into console---------------------------------------------- print('Results for "'+filename+'":') print('Kitti average translational error (%): {:.7f}'.format(kitti_trans_err)) print('Kitti average rotational error (deg/m): {:.7f}'.format(kitti_rot_err)) print('ATE (m): {:.7f}'.format(ate)) print('APE(translation error) median (m): {:.7f}'.format(ape_translation_statistics["median"])) print('APE(rotation error) median (deg): {:.7f}'.format(ape_rotation_statistics["median"])) print('distance to target on the last frame: {:.7f}'.format(seq_results['metrics']['dist_to_trgt'])) #------------------------------------------------------------------------------------------------------------ #---------------------------------Saving results into overall results text file------------------------------ file_results_txt.write('{:<24} '.format(filename[:filename.rfind('.')])) file_results_txt.write('{:>7.4f} '.format(kitti_trans_err)) file_results_txt.write('{:>7.4f} '.format(kitti_rot_err)) file_results_txt.write('{:>7.4f} '.format(ate)) file_results_txt.write('{:>7.4f} '.format(ape_translation_statistics["median"])) file_results_txt.write('{:>7.4f} '.format(ape_rotation_statistics["median"])) file_results_txt.write('{:>7.4f}\n'.format(seq_results['metrics']['dist_to_trgt'])) #------------------------------------------------------------------------------------------------------------ # --------------------------------Saving metrics to text file for one track---------------------------------- txt_filename = filename[:filename.rfind('.')]+"_metrics.txt" with open(os.path.join(output_folder_seq, txt_filename), "w") as txt_file: txt_file.write('Kitti average translational error (%): {:.7f}\n'.format(kitti_trans_err)) txt_file.write('Kitti average rotational error (deg/m): {:.7f}\n'.format(kitti_rot_err)) txt_file.write('ATE (m): {:.7f}\n'.format(ate)) txt_file.write('APE(translation error) median (m): {:.7f}\n'.format(ape_translation_statistics["median"])) txt_file.write('APE(rotation error) median (deg): {:.7f}\n'.format(ape_rotation_statistics["median"])) txt_file.write('Distance to target on the last frame: {:.7f}\n'.format(seq_results['metrics']['dist_to_trgt'])) #--------------------------------------------------------------------------------------------------------- # ---------------------------------Saving values of errors for each frame to text file------------------------ # ------------------------------------------for translation errors---------------------------------------- txt_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.txt" output_folder_seq_translation = os.path.join(output_folder_seq,"translation") output_folder_seq_rotation = os.path.join(output_folder_seq,"rotation") os.makedirs(output_folder_seq_translation, exist_ok=True) os.makedirs(output_folder_seq_rotation, exist_ok=True) with open(os.path.join(output_folder_seq_translation, txt_filename), "w") as txt_file: for error in ape_metric_translation.error: txt_file.write('{:.10f}\n'.format(error)) # -----------------------------------------for rotation degree errors-------------------------------------- txt_filename = filename[:filename.rfind('.')]+"_APE(rotation_deg)_errors.txt" with open(os.path.join(output_folder_seq_rotation, txt_filename), "w") as txt_file: for error in ape_metric_rotation.error: txt_file.write('{:.10f}\n'.format(error)) #---------------------------------------------------------------------------------------------------------- # ---------------------------------------Saving plot of errors of each frame------------------------------ # ------------------------------------------for translation errors---------------------------------------- plot_collection = plot.PlotCollection("Example") fig_1 = plt.figure(figsize=(8, 8)) plot.error_array(fig_1, ape_metric_translation.error, name="APE", title=str(ape_metric_translation), xlabel="Index of frame", ylabel='Error') plot_collection.add_figure("raw", fig_1) plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.png" plt.savefig(os.path.join(output_folder_seq_translation, plot_filename)) plt.close(fig_1) # -----------------------------------------for rotation degree errors-------------------------------------- plot_collection = plot.PlotCollection("Example") fig_1 = plt.figure(figsize=(8, 8)) plot.error_array(fig_1, ape_metric_rotation.error, name="APE", title=str(ape_metric_rotation), xlabel="Index of frame", ylabel='Error') plot_collection.add_figure("raw", fig_1) plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_errors.png" plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename)) plt.close(fig_1) #----------------------------------------------------------------------------------------------------------- # -----------------------------------------Saving trajectory plot------------------------------------------- # ------------------------------------------for translation errors---------------------------------------- fig_2 = plt.figure(figsize=(8, 8)) ax = plot.prepare_axis(fig_2, plot_mode) plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference') plot.traj_colormap( ax, traj_est, ape_metric_translation.error, plot_mode, min_map=ape_translation_statistics["min"], max_map=ape_translation_statistics["max"], title="APE translation mapped onto trajectory") plot_collection.add_figure("traj (error)", fig_2) plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_map.png" plt.savefig(os.path.join(output_folder_seq_translation,plot_filename)) plt.close(fig_2) # -----------------------------------------for rotation degree errors-------------------------------------- fig_2 = plt.figure(figsize=(8, 8)) ax = plot.prepare_axis(fig_2, plot_mode) plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference') plot.traj_colormap( ax, traj_est, ape_metric_rotation.error, plot_mode, min_map=ape_rotation_statistics["min"], max_map=ape_rotation_statistics["max"], title="APE rotation mapped onto trajectory") plot_collection.add_figure("traj (error)", fig_2) plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_map.png" plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename)) plt.close(fig_2) #----------------------------------------------------------------------------------------------------------- print() active_worksheet = wb['sheet1'] thin = Side(border_style="thin", color="000000") thick = Side(border_style="thick", color="000000") medium = Side(border_style="medium", color="000000") font_header = Font(name='Arial', size=10, bold=True, italic=False, vertAlign=None, underline='none', strike=False, color='FF000000') font_values = Font(name='Arial', size=10, bold=False, italic=False, vertAlign=None, underline='none', strike=False, color='FF000000') active_worksheet.row_dimensions[2].height = 35 file_results_txt.close() results.append(seq_results) t.update(1)
from evo.core import trajectory, sync, metrics from evo.tools import file_interface print("loading trajectories") traj_ref = file_interface.read_tum_trajectory_file( "../../test/data/fr2_desk_groundtruth.txt") traj_est = file_interface.read_tum_trajectory_file( "../../test/data/fr2_desk_ORB.txt") print("registering and aligning trajectories") traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est) traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False) print("calculating APE") data = (traj_ref, traj_est) ape_metric = metrics.APE(metrics.PoseRelation.translation_part) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() print("mean:", ape_statistics["mean"]) print("loading plot modules") from evo.tools import plot import matplotlib.pyplot as plt print("plotting") plot_collection = plot.PlotCollection("Example") # metric values fig_1 = plt.figure(figsize=(8, 8)) plot.error_array(fig_1, ape_metric.error, statistics=ape_statistics,
def ape(traj_ref_list, traj_est_list, pose_relation, align=False, correct_scale=False, align_origin=False, ref_name=["reference"], est_name=["estimate"]): from evo.core import metrics from evo.core import trajectory # Align the trajectories. only_scale = correct_scale and not align if align or correct_scale: logger.debug(SEP) for i in range(len(traj_ref_list)): print(traj_est_list[i], traj_ref_list[i]) traj_est_list[i] = trajectory.align_trajectory( traj_est_list[i], traj_ref_list[i], correct_scale, only_scale) elif align_origin: logger.debug(SEP) for i in range(len(traj_ref_list)): traj_est_list[i] = trajectory.align_trajectory_origin( traj_est_list[i], traj_ref_list[i]) # Calculate APE. logger.debug(SEP) data = [(traj_ref_list[i], traj_est_list[i]) for i in range(len(traj_ref_list))] ape_metric = [ metrics.APE(pose_relation) for i in range(len(traj_ref_list)) ] for i in range(len(traj_ref_list)): ape_metric[i].process_data(data[i]) ape_result = [ ape_metric[i].get_result(ref_name[i], est_name[i]) for i in range(len(traj_ref_list)) ] for i in range(len(traj_ref_list)): title = str(ape_metric[i]) if align and not correct_scale: title += "\n(with SE(3) Umeyama alignment)" elif align and correct_scale: title += "\n(with Sim(3) Umeyama alignment)" elif only_scale: title += "\n(scale corrected)" elif align_origin: title += "\n(with origin alignment)" else: title += "\n(not aligned)" ape_result[i].info["title"] = title logger.debug(SEP) logger.info(ape_result[i].pretty_str()) ape_result[i].add_trajectory(ref_name[i], traj_ref_list[i]) ape_result[i].add_trajectory(est_name[i], traj_est_list[i]) if isinstance(traj_est_list[i], trajectory.PoseTrajectory3D): seconds_from_start = [ t - traj_est_list[i].timestamps[0] for t in traj_est_list[i].timestamps ] ape_result[i].add_np_array("seconds_from_start", seconds_from_start) ape_result[i].add_np_array("timestamps", traj_est_list[i].timestamps) return ape_result
def main_ape(traj_ref, traj_est, pose_relation, align=True, correct_scale=False, ref_name="", est_name="", show_plot=False, save_plot=None, plot_mode=None, save_results=None, no_warnings=False, serialize_plot=None, plot_colormap_max=None, plot_colormap_min=None, plot_colormap_max_percentile=None): from evo.core import metrics, result from evo.core import trajectory from evo.tools import file_interface from evo.tools.settings import SETTINGS import numpy as np only_scale = correct_scale and not align if align or correct_scale: logger.debug(SEP) if only_scale: logger.debug("correcting scale...") else: logger.debug("aligning using Umeyama's method..." + (" (with scale correction)" if correct_scale else "")) traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale, only_scale) logger.debug(SEP) # calculate APE data = (traj_ref, traj_est) ape_metric = metrics.APE(pose_relation) ape_metric.process_data(data) ape_statistics = ape_metric.get_all_statistics() title = str(ape_metric) if align and not correct_scale: title += "\n(with SE(3) Umeyama alignment)" elif align and correct_scale: title += "\n(with Sim(3) Umeyama alignment)" elif only_scale: title += "\n(scale corrected)" else: title += "\n(not aligned)" ape_result = ape_metric.get_result(ref_name, est_name) logger.debug(SEP) logger.info(ape_result.pretty_str()) if isinstance(traj_est, trajectory.PoseTrajectory3D): seconds_from_start = [ t - traj_est.timestamps[0] for t in traj_est.timestamps ] ape_result.add_np_array("seconds_from_start", seconds_from_start) ape_result.add_np_array("timestamps", traj_est.timestamps) else: seconds_from_start = None if show_plot or save_plot or save_results or serialize_plot: if show_plot or save_plot or serialize_plot: from evo.tools import plot import matplotlib.pyplot as plt logger.debug(SEP) logger.debug("plotting results... ") fig1 = plt.figure(figsize=(SETTINGS.plot_figsize[0], SETTINGS.plot_figsize[1])) # metric values plot.error_array( fig1, ape_metric.error, x_array=seconds_from_start, statistics=ape_statistics, name="APE" + (" (" + ape_metric.unit.value + ")") if ape_metric.unit else "", title=title, xlabel="$t$ (s)" if seconds_from_start else "index") # info text if SETTINGS.plot_info_text and est_name and ref_name: ax = fig1.gca() ax.text(0, -0.12, "estimate: " + est_name + "\nreference: " + ref_name, transform=ax.transAxes, fontsize=8, color="gray") # trajectory colormapped fig2 = plt.figure(figsize=(SETTINGS.plot_figsize[0], SETTINGS.plot_figsize[1])) plot_mode = plot_mode if plot_mode is not None else plot.PlotMode.xyz ax = plot.prepare_axis(fig2, plot_mode) plot.traj(ax, plot_mode, traj_ref, '--', 'gray', 'reference', alpha=0.0 if SETTINGS.plot_hideref else 1.0) min_map = ape_statistics[ "min"] if plot_colormap_min is None else plot_colormap_min if plot_colormap_max_percentile is not None: max_map = np.percentile(ape_result.np_arrays["error_array"], plot_colormap_max_percentile) else: max_map = ape_statistics[ "max"] if plot_colormap_max is None else plot_colormap_max plot.traj_colormap(ax, traj_est, ape_metric.error, plot_mode, min_map=min_map, max_map=max_map, title="APE mapped onto trajectory") fig2.axes.append(ax) plot_collection = plot.PlotCollection(title) plot_collection.add_figure("raw", fig1) plot_collection.add_figure("map", fig2) if show_plot: plot_collection.show() if save_plot: plot_collection.export(save_plot, confirm_overwrite=not no_warnings) if serialize_plot: logger.debug(SEP) plot_collection.serialize(serialize_plot, confirm_overwrite=not no_warnings) if save_results: logger.debug(SEP) if SETTINGS.save_traj_in_zip: ape_result.add_trajectory("traj_ref", traj_ref) ape_result.add_trajectory("traj_est", traj_est) file_interface.save_res_file(save_results, ape_result, confirm_overwrite=not no_warnings) return ape_result