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)
