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 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 rpe(traj_ref, traj_est, pose_relation, delta, delta_unit, rel_delta_tol=0.1, all_pairs=False, align=False, correct_scale=False, ref_name="reference", est_name="estimate", support_loop=False): 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) # Calculate RPE. logger.debug(SEP) data = (traj_ref, traj_est) rpe_metric = metrics.RPE(pose_relation, delta, delta_unit, rel_delta_tol, all_pairs) rpe_metric.process_data(data) title = str(rpe_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)" rpe_result = rpe_metric.get_result(ref_name, est_name) rpe_result.info["title"] = title logger.debug(SEP) logger.info(rpe_result.pretty_str()) # Restrict trajectories to delta ids for further processing steps. if support_loop: # Avoid overwriting if called repeatedly e.g. in Jupyter notebook. import copy traj_ref = copy.deepcopy(traj_ref) traj_est = copy.deepcopy(traj_est) traj_ref.reduce_to_ids(rpe_metric.delta_ids) traj_est.reduce_to_ids(rpe_metric.delta_ids) rpe_result.add_trajectory(ref_name, traj_ref) rpe_result.add_trajectory(est_name, traj_est) if isinstance(traj_est, trajectory.PoseTrajectory3D) and not all_pairs: seconds_from_start = [ t - traj_est.timestamps[0] for t in traj_est.timestamps ] rpe_result.add_np_array("seconds_from_start", seconds_from_start) rpe_result.add_np_array("timestamps", traj_est.timestamps) return rpe_result
def get_rpe_trans(data): """ Return RPE 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. """ rpe_trans = metrics.RPE(metrics.PoseRelation.translation_part, 1.0, metrics.Unit.frames, 0.0, False) rpe_trans.process_data(data) return rpe_trans
def rpe(traj_ref: PosePath3D, traj_est: PosePath3D, pose_relation: metrics.PoseRelation, delta: float, delta_unit: metrics.Unit, rel_delta_tol: float = 0.1, all_pairs: bool = False, align: bool = False, correct_scale: bool = False, n_to_align: int = -1, align_origin: bool = False, ref_name: str = "reference", est_name: str = "estimate", support_loop: bool = False) -> Result: # Align the trajectories. only_scale = correct_scale and not align if align or correct_scale: logger.debug(SEP) traj_est.align(traj_ref, correct_scale, only_scale, n=n_to_align) elif align_origin: logger.debug(SEP) traj_est.align_origin(traj_ref) # Calculate RPE. logger.debug(SEP) data = (traj_ref, traj_est) rpe_metric = metrics.RPE(pose_relation, delta, delta_unit, rel_delta_tol, all_pairs) rpe_metric.process_data(data) title = str(rpe_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) rpe_result = rpe_metric.get_result(ref_name, est_name) rpe_result.info["title"] = title logger.debug(SEP) logger.info(rpe_result.pretty_str()) # Restrict trajectories to delta ids for further processing steps. if support_loop: # Avoid overwriting if called repeatedly e.g. in Jupyter notebook. import copy traj_ref = copy.deepcopy(traj_ref) traj_est = copy.deepcopy(traj_est) traj_ref.reduce_to_ids(rpe_metric.delta_ids) traj_est.reduce_to_ids(rpe_metric.delta_ids) rpe_result.add_trajectory(ref_name, traj_ref) rpe_result.add_trajectory(est_name, traj_est) if isinstance(traj_est, PoseTrajectory3D): seconds_from_start = [ t - traj_est.timestamps[0] for t in traj_est.timestamps ] rpe_result.add_np_array("seconds_from_start", seconds_from_start) rpe_result.add_np_array("timestamps", traj_est.timestamps) return rpe_result
def main_rpe(traj_ref, traj_est, pose_relation, delta, delta_unit, rel_delta_tol=0.1, all_pairs=False, align=False, correct_scale=False, ref_name="", est_name="", show_plot=False, save_plot=None, plot_mode=None, save_results=None, no_warnings=False, support_loop=False, serialize_plot=None): from evo.core import metrics, result from evo.core import trajectory from evo.tools import file_interface from evo.tools.settings import SETTINGS if (show_plot or save_plot or serialize_plot) and all_pairs: raise metrics.MetricsException( "all_pairs mode cannot be used with plotting functions") only_scale = correct_scale and not align if align or correct_scale: logging.debug(SEP) if only_scale: logging.debug("correcting scale...") else: logging.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) logging.debug(SEP) # calculate RPE data = (traj_ref, traj_est) rpe_metric = metrics.RPE(pose_relation, delta, delta_unit, rel_delta_tol, all_pairs) rpe_metric.process_data(data) rpe_statistics = rpe_metric.get_all_statistics() title = str(rpe_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)" rpe_result = result.from_metric(rpe_metric, title, ref_name, est_name) logging.debug(SEP) logging.info(rpe_result.pretty_str()) # restrict trajectories to delta ids if support_loop: # avoid overwriting if called repeatedly e.g. in Jupyter notebook import copy traj_ref = copy.deepcopy(traj_ref) traj_est = copy.deepcopy(traj_est) traj_ref.reduce_to_ids(rpe_metric.delta_ids) traj_est.reduce_to_ids(rpe_metric.delta_ids) if isinstance(traj_est, trajectory.PoseTrajectory3D) and not all_pairs: seconds_from_start = [ t - traj_est.timestamps[0] for t in traj_est.timestamps ] rpe_result.add_np_array("seconds_from_start", seconds_from_start) else: seconds_from_start = None if show_plot or save_plot or serialize_plot and not all_pairs: from evo.tools import plot import matplotlib.pyplot as plt logging.debug(SEP) logging.debug("plotting results... ") fig1 = plt.figure(figsize=(SETTINGS.plot_figsize[0], SETTINGS.plot_figsize[1])) # metric values plot.error_array( fig1, rpe_metric.error, x_array=seconds_from_start, statistics=rpe_statistics, name="RPE" + (" (" + rpe_metric.unit.value + ")") if rpe_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 if SETTINGS.plot_hideref else 1) plot.traj_colormap(ax, traj_est, rpe_metric.error, plot_mode, min_map=rpe_statistics["min"], max_map=rpe_statistics["max"], title="RPE 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: logging.debug(SEP) plot_collection.serialize(serialize_plot, confirm_overwrite=not no_warnings) if save_results: logging.debug(SEP) if SETTINGS.save_traj_in_zip: rpe_result.add_trajectory("traj_ref", traj_ref) rpe_result.add_trajectory("traj_est", traj_est) file_interface.save_res_file(save_results, rpe_result, confirm_overwrite=not no_warnings) return rpe_result
print("elapsed time for trajectory loading (seconds): \t", "{0:.6f}".format(load_time)) """ test relative pose error algorithms for different types of pose relations """ delta = 1 delta_unit = metrics.Unit.frames for pose_relation in metrics.PoseRelation: data = (traj_ref, traj_est) print("\n------------------------------------------------------------------\n") start = time.clock() rpe_metric = metrics.RPE(pose_relation, delta, delta_unit) rpe_metric.process_data(data) rpe_statistics = rpe_metric.get_all_statistics() stop = time.clock() rpe_time = stop-start print("RPE statistics w.r.t. " + rpe_metric.pose_relation.value + ": ") pretty_printer.pprint(rpe_statistics) print("\nelapsed time for running the RPE algorithm (seconds):\t", "{0:.6f}".format(rpe_time)) print("elapsed time for trajectory loading and RPE (seconds):\t", "{0:.6f}".format(load_time+rpe_time)) print("\n------------------------------------------------------------------") print("------------------------------------------------------------------\n") print("calling offical TUM RPE script for comparison...") cmd = ["tum_benchmark_tools/evaluate_rpe.py", ref_file, est_file,
def main_rpe_for_each(traj_ref, traj_est, pose_relation, mode, bins, rel_tols, align=False, correct_scale=False, ref_name="", est_name="", show_plot=False, save_plot=None, save_results=None, no_warnings=False, serialize_plot=None): from evo.core import metrics, result from evo.core import filters from evo.core import trajectory from evo.tools import file_interface from evo.tools.settings import SETTINGS if not bins or not rel_tols: raise RuntimeError("bins and tolerances must have more than one element") if len(bins) != len(rel_tols): raise RuntimeError("bins and tolerances must have the same number of elements") if mode in {"speed", "angular_speed"} and traj_est is trajectory.PosePath3D: raise RuntimeError("timestamps are required for mode: " + mode) bin_unit = None if mode == "speed": bin_unit = metrics.VelUnit.meters_per_sec elif mode == "path": bin_unit = metrics.Unit.meters elif mode == "angle": bin_unit = metrics.Unit.degrees elif mode == "angular_speed": bin_unit = metrics.VelUnit.degrees_per_sec rpe_unit = None if pose_relation is metrics.PoseRelation.translation_part: rpe_unit = metrics.Unit.meters elif pose_relation is metrics.PoseRelation.rotation_angle_deg: rpe_unit = metrics.Unit.degrees elif pose_relation is metrics.PoseRelation.rotation_angle_rad: rpe_unit = metrics.Unit.radians correct_only_scale = correct_scale and not align if align or correct_scale: logger.debug(SEP) if correct_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, correct_only_scale) results = [] for bin, rel_tol, in zip(bins, rel_tols): logger.debug(SEP) logger.info( "Calculating RPE for each sub-sequence of " + str(bin) + " (" + bin_unit.value + ")") tol = bin * rel_tol id_pairs = [] if mode == "path": id_pairs = filters.filter_pairs_by_path(traj_ref.poses_se3, bin, tol, all_pairs=True) elif mode == "angle": id_pairs = filters.filter_pairs_by_angle(traj_ref.poses_se3, bin, tol, degrees=True) elif mode == "speed": id_pairs = filters.filter_pairs_by_speed(traj_ref.poses_se3, traj_ref.timestamps, bin, tol) elif mode == "angular_speed": id_pairs = filters.filter_pairs_by_angular_speed(traj_ref.poses_se3, traj_ref.timestamps, bin, tol, True) if len(id_pairs) == 0: raise RuntimeError("bin " + str(bin) + " (" + str(bin_unit.value) + ") " + "produced empty index list - try other values") # calculate RPE with all IDs (delta 1 frames) data = (traj_ref, traj_est) # the delta here has nothing to do with the bin - 1f delta just to use all poses of the bin rpe_metric = metrics.RPE(pose_relation, delta=1, delta_unit=metrics.Unit.frames, all_pairs=True) rpe_metric.process_data(data, id_pairs) mean = rpe_metric.get_statistic(metrics.StatisticsType.mean) results.append(mean) if SETTINGS.plot_usetex: mode.replace("_", "\_") title = "mean RPE w.r.t. " + pose_relation.value + "\nfor different " + mode + " sub-sequences" 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 correct_only_scale: title += "\n(scale corrected)" else: title += "\n(not aligned)" rpe_for_each_result = result.Result() rpe_for_each_result.add_info({ "label": "RPE ({})".format(rpe_unit.value), "est_name": est_name, "ref_name": ref_name, "title": title, "xlabel": "{} sub-sequences ({})".format(mode, bin_unit.value) }) rpe_for_each_result.add_stats({bin: result for bin, result in zip(bins, results)}) # TODO use a more suitable name than seconds rpe_for_each_result.add_np_array("seconds_from_start", bins) rpe_for_each_result.add_np_array("error_array", results) logger.debug(SEP) logger.info(rpe_for_each_result.pretty_str()) if show_plot or save_plot or serialize_plot: from evo.tools import plot import matplotlib.pyplot as plt plot_collection = plot.PlotCollection(title) fig = plt.figure(figsize=(SETTINGS.plot_figsize[0], SETTINGS.plot_figsize[1])) plot.error_array(fig, results, x_array=bins, name="mean RPE" + (" (" + rpe_unit.value + ")") if rpe_unit else "", marker="o", title=title, xlabel=mode + " sub-sequences " + " (" + bin_unit.value + ")") # info text if SETTINGS.plot_info_text and est_name and ref_name: ax = fig.gca() ax.text(0, -0.12, "estimate: " + est_name + "\nreference: " + ref_name, transform=ax.transAxes, fontsize=8, color="gray") plt.title(title) plot_collection.add_figure("raw", fig) 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: rpe_for_each_result.add_trajectory("traj_ref", traj_ref) rpe_for_each_result.add_trajectory("traj_est", traj_est) file_interface.save_res_file(save_results, rpe_for_each_result, confirm_overwrite=not no_warnings) return rpe_for_each_result
def get_rpe_static_raw(data, mode, delta, unit, allpair): rpe_metric = metrics.RPE(mode, delta, unit, allpair) # rpe_metric.process_data(data) rpe_stat = np.asarray(list(rpe_metric.get_all_statistics().values())) error_full = rpe_metric.error return rpe_stat, error_full
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)