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)
