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