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 run_dataset(self, dataset):
""" Run a single dataset from an experiments file and save all output. This is done
for every pipeline requested for the dataset.
Args:
dataset: a dataset to run as defined in the experiments yaml file.
Returns: True if all pipelines for the dataset succeed, False otherwise.
"""
dataset_name = dataset['name']
has_a_pipeline_failed = False
pipelines_to_run_list = dataset['pipelines']
if len(pipelines_to_run_list) == 0:
log.warning("Not running pipeline...")
for pipeline_type in pipelines_to_run_list:
# TODO shouldn't this break when a pipeline has failed? Not necessarily
# if we want to plot all pipelines except the failing ones.
evt.print_green("Run pipeline: %s" % pipeline_type)
pipeline_success = self.__run_vio(dataset, pipeline_type)
if pipeline_success:
evt.print_green("Successful pipeline run.")
else:
log.error("Failed pipeline run!")
has_a_pipeline_failed = True
if not has_a_pipeline_failed:
evt.print_green("All pipeline runs were successful.")
evt.print_green("Finished evaluation for dataset: " + dataset_name)
return not has_a_pipeline_failed
def save_results_to_file(self, results, title,
dataset_pipeline_result_dir):
""" Writes a result dictionary to file as a yaml file.
Args:
results: a dictionary containing ape, rpe rotation and rpe translation results and
statistics.
title: a string representing the filename without the '.yaml' extension.
dataset_pipeline_result_dir: a string representing the filepath for the location to
save the results file.
"""
results_file = os.path.join(dataset_pipeline_result_dir,
title + '.yaml')
evt.print_green("Saving analysis results to: %s" % results_file)
evt.create_full_path_if_not_exists(results_file)
with open(results_file, 'w') as outfile:
outfile.write(yaml.dump(results, default_flow_style=False))
def save_plots_to_file(self,
plot_collection,
dataset_pipeline_result_dir,
save_pdf=True):
""" Wrie plot collection to disk as both eps and pdf.
Args:
- plot_collection: a PlotCollection containing all the plots to save to file.
- dataset_pipeline_result_dir: a string representing the filepath for the location to
which the plot files are saved.
- save_pdf: whether to save figures to pdf or eps format
"""
# Config output format (pdf, eps, ...) using evo_config...
if save_pdf:
pdf_output_file_path = os.path.join(dataset_pipeline_result_dir,
"plots.pdf")
evt.print_green("Saving plots to: %s" % pdf_output_file_path)
plot_collection.export(pdf_output_file_path, False)
else:
eps_output_file_path = os.path.join(dataset_pipeline_result_dir,
"plots.eps")
evt.print_green("Saving plots to: %s" % eps_output_file_path)
plot_collection.export(eps_output_file_path, False)
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 __evaluate_run(self, pipeline_type, dataset):
""" Evaluate performance of one pipeline of one dataset, as defined in the experiments
yaml file.
Assumes that the files traj_gt.csv traj_vio.csv and traj_pgo.csv are present.
Args:
dataset: a dataset to evaluate as defined in the experiments yaml file.
pipeline_type: a pipeline representing a set of parameters to use, as
defined in the experiments yaml file for the dataset in question.
Returns: True if there are no exceptions during evaluation, False otherwise.
"""
dataset_name = dataset["name"]
dataset_results_dir = os.path.join(self.results_dir, dataset_name)
dataset_pipeline_result_dir = os.path.join(dataset_results_dir,
pipeline_type)
traj_gt_path = os.path.join(dataset_pipeline_result_dir,
self.traj_gt_csv_name)
traj_vio_path = os.path.join(dataset_pipeline_result_dir,
self.traj_vio_csv_name)
traj_pgo_path = os.path.join(dataset_pipeline_result_dir,
self.traj_pgo_csv_name)
# Analyze dataset:
log.debug(
"\033[1mAnalysing dataset:\033[0m \n %s \n \033[1m for pipeline \033[0m %s."
% (dataset_results_dir, pipeline_type))
evt.print_green("Starting analysis of pipeline: %s" % pipeline_type)
discard_n_start_poses = dataset["discard_n_start_poses"]
discard_n_end_poses = dataset["discard_n_end_poses"]
segments = dataset["segments"]
[plot_collection, results_vio,
results_pgo] = self.run_analysis(traj_gt_path, traj_vio_path,
traj_pgo_path, segments,
dataset_name, discard_n_start_poses,
discard_n_end_poses)
if self.save_results:
if results_vio is not None:
self.save_results_to_file(results_vio, "results_vio",
dataset_pipeline_result_dir)
if results_pgo is not None:
self.save_results_to_file(results_pgo, "results_pgo",
dataset_pipeline_result_dir)
if self.display_plots and plot_collection is not None:
evt.print_green("Displaying plots.")
plot_collection.show()
if self.save_plots and plot_collection is not None:
self.save_plots_to_file(plot_collection,
dataset_pipeline_result_dir)
if self.write_website:
log.info("Writing performance website for dataset: %s" %
dataset_name)
self.website_builder.add_dataset_to_website(
dataset_name, pipeline_type, dataset_pipeline_result_dir)
self.website_builder.write_datasets_website()
return True
def run_dataset(results_dir,
params_dir,
dataset_dir,
dataset_properties,
executable_path,
run_pipeline,
analyse_vio,
plot,
save_results,
save_plots,
save_boxplots,
pipelines_to_run_list,
initial_k,
final_k,
discard_n_start_poses=0,
discard_n_end_poses=0,
extra_flagfile_path='',
verbose_sparkvio=False):
""" Evaluates pipeline using Structureless(S), Structureless(S) + Projection(P), \
and Structureless(S) + Projection(P) + Regular(R) factors \
and then compiles a list of results """
dataset_name = dataset_properties['name']
dataset_segments = dataset_properties['segments']
################### RUN PIPELINE ################################
pipeline_output_dir = os.path.join(results_dir, "tmp_output/output/")
evt.create_full_path_if_not_exists(pipeline_output_dir)
output_file = os.path.join(pipeline_output_dir, "output_posesVIO.csv")
has_a_pipeline_failed = False
if len(pipelines_to_run_list) == 0:
log.warning("Not running pipeline...")
for pipeline_type in pipelines_to_run_list:
has_a_pipeline_failed = not process_vio(
executable_path, dataset_dir, dataset_name, results_dir,
params_dir, pipeline_output_dir, pipeline_type, dataset_segments,
save_results, plot, save_plots, output_file, run_pipeline,
analyse_vio, discard_n_start_poses, discard_n_end_poses, initial_k,
final_k, extra_flagfile_path, verbose_sparkvio)
# Save boxplots
if save_boxplots:
# TODO(Toni) is this really saving the boxplots?
if not has_a_pipeline_failed:
stats = dict()
for pipeline_type in pipelines_to_run_list:
results_dataset_dir = os.path.join(results_dir, dataset_name)
results = os.path.join(results_dataset_dir, pipeline_type,
"results.yaml")
if not os.path.exists(results):
raise Exception(
"\033[91mCannot plot boxplots: missing results for %s pipeline \
and dataset: %s" %
(pipeline_type, dataset_name) + "\033[99m \n \
Expected results here: %s" % results)
try:
stats[pipeline_type] = yaml.load(open(results, 'r'),
Loader=yaml.Loader)
except yaml.YAMLError as e:
raise Exception("Error in results file: ", e)
log.info("Check stats %s in %s" % (pipeline_type, results))
check_stats(stats[pipeline_type])
log.info("Drawing boxplots.")
evt.draw_rpe_boxplots(results_dataset_dir, stats,
len(dataset_segments))
else:
log.warning(
"A pipeline run has failed... skipping boxplot drawing.")
if not has_a_pipeline_failed:
evt.print_green("All pipeline runs were successful.")
else:
log.error("A pipeline has failed!")
evt.print_green("Finished evaluation for dataset: " + dataset_name)
return not has_a_pipeline_failed
def process_vio(executable_path,
dataset_dir,
dataset_name,
results_dir,
params_dir,
pipeline_output_dir,
pipeline_type,
SEGMENTS,
save_results,
plot,
save_plots,
output_file,
run_pipeline,
analyse_vio,
discard_n_start_poses,
discard_n_end_poses,
initial_k,
final_k,
extra_flagfile_path='',
verbose_sparkvio=False):
"""
Args:
- executable_path: path to the pipeline executable (i.e. `./build/spark_vio`).
- dataset_dir: directory of the dataset, must contain traj_gt.csv (the ground truth trajectory for analysis to work).
- dataset_name: specific dataset to run.
- results_dir: directory where the results of the run will reside:
- used as results_dir/dataset_name/S, results_dir/dataset_name/SP, results_dir/dataset_name/SPR
- where each directory have traj_est.csv (the estimated trajectory), and plots if requested.
- params_dir: directory where the parameters for each pipeline reside:
- used as params_dir/S, params_dir/SP, params_dir/SPR.
- pipeline_output_dir: where to store all output_* files produced by the pipeline.
- pipeline_type: type of pipeline to process (1: S, 2: SP, 3: SPR).
- SEGMENTS: segments for RPE boxplots.
- save_results: saves APE, and RPE per segment results of the run.
- plot: whether to plot the APE/RPE results or not.
- save_plots: saves plots of APE/RPE.
- output_file: the name of the trajectory estimate output of the vio which will then be copied as traj_est.csv.
- run_pipeline: whether to run the VIO to generate a new traj_est.csv.
- analyse_vio: whether to analyse traj_est.csv or not.
- extra_flagfile_path: to be used in order to override other flags or add new ones.
Useful for regression tests when the param to be regressed is a gflag.
- verbose_sparkvio: whether to print the SparkVIO messages or not.
This is useful for debugging, but too verbose when you want to see APE/RPE results.
"""
dataset_results_dir = os.path.join(results_dir, dataset_name)
dataset_pipeline_result_dir = os.path.join(dataset_results_dir,
pipeline_type)
traj_ref_path = os.path.join(dataset_dir, dataset_name,
"mav0/state_groundtruth_estimate0/data.csv"
) # TODO make it not specific to EUROC
traj_es = os.path.join(dataset_results_dir, pipeline_type, "traj_es.csv")
evt.create_full_path_if_not_exists(traj_es)
if run_pipeline:
evt.print_green("Run pipeline: %s" % pipeline_type)
# The override flags are used by the regression tests.
if run_vio(executable_path, dataset_dir, dataset_name, params_dir,
pipeline_output_dir, pipeline_type, initial_k, final_k,
extra_flagfile_path, verbose_sparkvio) == 0:
evt.print_green("Successful pipeline run.")
log.debug(
"\033[1mCopying output file: \033[0m \n %s \n \033[1m to results file:\033[0m\n %s"
% (output_file, traj_es))
copyfile(output_file, traj_es)
output_destination_dir = os.path.join(dataset_pipeline_result_dir,
"output")
log.debug(
"\033[1mMoving output dir:\033[0m \n %s \n \033[1m to destination:\033[0m \n %s"
% (pipeline_output_dir, output_destination_dir))
try:
evt.move_output_from_to(pipeline_output_dir,
output_destination_dir)
except:
log.fatal(
"\033[1mFailed copying output dir: \033[0m\n %s \n \033[1m to destination: %s \033[0m\n"
% (pipeline_output_dir, output_destination_dir))
else:
log.error("Pipeline failed on dataset: " + dataset_name)
# Avoid writting results.yaml with analysis if the pipeline failed.
log.info("Not writting results.yaml")
return False
if analyse_vio:
log.debug(
"\033[1mAnalysing dataset:\033[0m \n %s \n \033[1m for pipeline \033[0m %s."
% (dataset_results_dir, pipeline_type))
evt.print_green("Starting analysis of pipeline: %s" % pipeline_type)
run_analysis(traj_ref_path, traj_es, SEGMENTS, save_results, plot,
save_plots, dataset_pipeline_result_dir, False,
dataset_name, discard_n_start_poses, discard_n_end_poses)
return True
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)