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
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): from evo.core import metrics, result from evo.core import trajectory from evo.tools import file_interface from evo.tools.settings import SETTINGS 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 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 = result.from_metric(ape_metric, title, ref_name, est_name) logging.debug(SEP) logging.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) 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 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, 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) plot.traj_colormap(ax, traj_est, ape_metric.error, plot_mode, min_map=ape_statistics["min"], max_map=ape_statistics["max"], 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: 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: 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