def calc_results(self, ape_metric, rpe_metric_trans, rpe_metric_rot, data, segments): """ Create and return a dictionary containing stats and results for ATE, RRE and RTE for a datset. Args: ape_metric: an evo.core.metric object representing the ATE. rpe_metric_trans: an evo.core.metric object representing the RTE. rpe_metric_rot: an evo.core.metric object representing the RRE. data: a 2-tuple with reference and estimated trajectories as PoseTrajectory3D objects in that order. segments: a list of segments for RPE. Returns: a dictionary containing all relevant results. """ # Calculate APE results: results = dict() ape_result = ape_metric.get_result() results["absolute_errors"] = ape_result # Calculate RPE results: # TODO(Toni): Save RPE computation results rather than the statistics # you can compute statistics later... rpe_stats_trans = rpe_metric_trans.get_all_statistics() rpe_stats_rot = rpe_metric_rot.get_all_statistics() # Calculate RPE results of segments and save results["relative_errors"] = dict() 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 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 return results
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 = get_ape_trans(data) ape_result = ape_metric.get_result() evt.print_green("APE translation: %f" % ape_result.stats['mean']) evt.print_purple("Calculating RPE translation part for " + suffix) rpe_metric_trans = get_rpe_trans(data) evt.print_purple("Calculating RPE rotation angle for " + suffix) rpe_metric_rot = get_rpe_rot(data) # Collect results: results = dict() results["absolute_errors"] = ape_result results["relative_errors"] = self.calc_rpe_results( rpe_metric_trans, rpe_metric_rot, data, segments) # Add as well how long hte trajectory was. results["trajectory_length_m"] = traj_est.path_length() return (ape_metric, rpe_metric_trans, rpe_metric_rot, results)
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 run_analysis(self, traj_ref_path, traj_vio_path, traj_pgo_path, segments, dataset_name="", discard_n_start_poses=0, discard_n_end_poses=0): """ Analyze data from a set of trajectory csv files. Args: traj_ref_path: string representing filepath of the reference (ground-truth) trajectory. traj_vio_path: string representing filepath of the vio estimated trajectory. traj_pgo_path: string representing filepath of the pgo estimated trajectory. segments: list of segments for RPE calculation, defined in the experiments yaml file. dataset_name: string representing the dataset's name discard_n_start_poses: int representing number of poses to discard from start of analysis. discard_n_end_poses: int representing the number of poses to discard from end of analysis. """ import copy # Mind that traj_est_pgo might be None traj_ref, traj_est_vio, traj_est_pgo = self.read_traj_files( traj_ref_path, traj_vio_path, traj_pgo_path) # We copy to distinguish from the pgo version that may be created traj_ref_vio = copy.deepcopy(traj_ref) # Register and align trajectories: evt.print_purple("Registering and aligning trajectories") traj_ref_vio, traj_est_vio = sync.associate_trajectories( traj_ref_vio, traj_est_vio) traj_est_vio = trajectory.align_trajectory( traj_est_vio, traj_ref_vio, correct_scale=False, discard_n_start_poses=int(discard_n_start_poses), discard_n_end_poses=int(discard_n_end_poses)) # We do the same for the PGO trajectory if needed: traj_ref_pgo = None if traj_est_pgo is not None: traj_ref_pgo = copy.deepcopy(traj_ref) traj_ref_pgo, traj_est_pgo = sync.associate_trajectories( traj_ref_pgo, traj_est_pgo) traj_est_pgo = trajectory.align_trajectory( traj_est_pgo, traj_ref_pgo, correct_scale=False, discard_n_start_poses=int(discard_n_start_poses), discard_n_end_poses=int(discard_n_end_poses)) # We need to pick the lowest num_poses before doing any computation: num_of_poses = traj_est_vio.num_poses if traj_est_pgo is not None: num_of_poses = min(num_of_poses, traj_est_pgo.num_poses) traj_est_pgo.reduce_to_ids( range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1)) traj_ref_pgo.reduce_to_ids( range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1)) traj_est_vio.reduce_to_ids( range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1)) traj_ref_vio.reduce_to_ids( range(int(discard_n_start_poses), int(num_of_poses - discard_n_end_poses), 1)) # Calculate all metrics: (ape_metric_vio, rpe_metric_trans_vio, rpe_metric_rot_vio, results_vio) = self.process_trajectory_data(traj_ref_vio, traj_est_vio, segments, True) # We do the same for the pgo trajectory if needed: ape_metric_pgo = None rpe_metric_trans_pgo = None rpe_metric_rot_pgo = None results_pgo = None if traj_est_pgo is not None: (ape_metric_pgo, rpe_metric_trans_pgo, rpe_metric_rot_pgo, results_pgo) = self.process_trajectory_data( traj_ref_pgo, traj_est_pgo, segments, False) # Generate plots for return: plot_collection = None if self.display_plots or self.save_plots: evt.print_green("Plotting:") log.info(dataset_name) plot_collection = plot.PlotCollection("Example") if traj_est_pgo is not None: # APE Metric Plot: plot_collection.add_figure( "PGO_APE_translation", plot_metric(ape_metric_pgo, "PGO + VIO APE Translation")) # Trajectory Colormapped with ATE Plot: plot_collection.add_figure( "PGO_APE_translation_trajectory_error", plot_traj_colormap_ape( ape_metric_pgo, traj_ref_pgo, traj_est_vio, traj_est_pgo, "PGO + VIO ATE Mapped Onto Trajectory")) # RPE Translation Metric Plot: plot_collection.add_figure( "PGO_RPE_translation", plot_metric(rpe_metric_trans_pgo, "PGO + VIO RPE Translation")) # Trajectory Colormapped with RTE Plot: plot_collection.add_figure( "PGO_RPE_translation_trajectory_error", plot_traj_colormap_rpe( rpe_metric_trans_pgo, traj_ref_pgo, traj_est_vio, traj_est_pgo, "PGO + VIO RPE Translation Error Mapped Onto Trajectory" )) # RPE Rotation Metric Plot: plot_collection.add_figure( "PGO_RPE_Rotation", plot_metric(rpe_metric_rot_pgo, "PGO + VIO RPE Rotation")) # Trajectory Colormapped with RTE Plot: plot_collection.add_figure( "PGO_RPE_rotation_trajectory_error", plot_traj_colormap_rpe( rpe_metric_rot_pgo, traj_ref_pgo, traj_est_vio, traj_est_pgo, "PGO + VIO RPE Rotation Error Mapped Onto Trajectory")) # Plot VIO results plot_collection.add_figure( "VIO_APE_translation", plot_metric(ape_metric_vio, "VIO APE Translation")) plot_collection.add_figure( "VIO_APE_translation_trajectory_error", plot_traj_colormap_ape(ape_metric_vio, traj_ref_vio, traj_est_vio, None, "VIO ATE Mapped Onto Trajectory")) plot_collection.add_figure( "VIO_RPE_translation", plot_metric(rpe_metric_trans_vio, "VIO RPE Translation")) plot_collection.add_figure( "VIO_RPE_translation_trajectory_error", plot_traj_colormap_rpe( rpe_metric_trans_vio, traj_ref_vio, traj_est_vio, None, "VIO RPE Translation Error Mapped Onto Trajectory")) plot_collection.add_figure( "VIO_RPE_Rotation", plot_metric(rpe_metric_rot_vio, "VIO RPE Rotation")) plot_collection.add_figure( "VIO_RPE_rotation_trajectory_error", plot_traj_colormap_rpe( rpe_metric_rot_vio, traj_ref_vio, traj_est_vio, None, "VIO RPE Rotation Error Mapped Onto Trajectory")) return [plot_collection, results_vio, results_pgo]
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)