def ape(traj_ref,
traj_est,
pose_relation,
align=False,
correct_scale=False,
align_origin=False,
ref_name="reference",
est_name="estimate"):
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)
elif align_origin:
logger.debug(SEP)
traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref)
# Calculate APE.
logger.debug(SEP)
data = (traj_ref, traj_est)
ape_metric = metrics.APE(pose_relation)
ape_metric.process_data(data)
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)"
elif align_origin:
title += "\n(with origin alignment)"
else:
title += "\n(not aligned)"
ape_result = ape_metric.get_result(ref_name, est_name)
ape_result.info["title"] = title
logger.debug(SEP)
logger.info(ape_result.pretty_str())
ape_result.add_trajectory(ref_name, traj_ref)
ape_result.add_trajectory(est_name, traj_est)
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)
ape_result.add_np_array("timestamps", traj_est.timestamps)
return ape_result
def original_ape(traj_ref,
traj_est,
pose_relation,
align=False,
correct_scale=False,
align_origin=False,
ref_name="reference",
est_name="estimate"):
''' Copied from main_ape.py
'''
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
# Align the trajectories.
only_scale = correct_scale and not align
if align or correct_scale:
traj_est = trajectory.align_trajectory(traj_est, traj_ref,
correct_scale, only_scale)
elif align_origin:
traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref)
# Calculate APE.
data = (traj_ref, traj_est)
ape_metric = metrics.APE(pose_relation)
ape_metric.process_data(data)
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)"
elif align_origin:
title += "\n(with origin alignment)"
else:
title += "\n(not aligned)"
ape_result = ape_metric.get_result(ref_name, est_name)
ape_result.info["title"] = title
ape_result.add_trajectory(ref_name, traj_ref)
ape_result.add_trajectory(est_name, traj_est)
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)
ape_result.add_np_array("timestamps", traj_est.timestamps)
return ape_result
def ape(traj_ref,
traj_est,
pose_relation,
align=False,
correct_scale=False,
align_origin=False,
ref_name="reference",
est_name="estimate"):
''' Based on main_ape.py
'''
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
# Align the trajectories.
only_scale = correct_scale and not align
if align or correct_scale:
traj_est = trajectory.align_trajectory(traj_est, traj_ref,
correct_scale, only_scale)
elif align_origin:
traj_est = trajectory.align_trajectory_origin(traj_est, traj_ref)
# Calculate APE.
data = (traj_ref, traj_est)
ape_metric = APE(pose_relation)
ape_metric.process_data(data)
ape_result = ape_metric.get_result(ref_name, est_name)
ape_result.add_trajectory(ref_name, traj_ref)
ape_result.add_trajectory(est_name, traj_est)
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)
ape_result.add_np_array("timestamps", traj_est.timestamps)
return ape_result
def run(args):
import os
import sys
import numpy as np
import evo.core.lie_algebra as lie
from evo.core import trajectory
from evo.core.trajectory import PoseTrajectory3D
from evo.tools import file_interface, log
from evo.tools.settings import SETTINGS
log.configure_logging(verbose=args.verbose,
silent=args.silent,
debug=args.debug,
local_logfile=args.logfile)
if args.debug:
import pprint
logger.debug(
"main_parser config:\n" +
pprint.pformat({arg: getattr(args, arg)
for arg in vars(args)}) + "\n")
logger.debug(SEP)
trajectories, ref_traj = load_trajectories(args)
if args.merge:
if args.subcommand == "kitti":
die("Can't merge KITTI files.")
if len(trajectories) == 0:
die("No trajectories to merge (excluding --ref).")
trajectories = {
"merged_trajectory": trajectory.merge(trajectories.values())
}
if args.transform_left or args.transform_right:
tf_type = "left" if args.transform_left else "right"
tf_path = args.transform_left \
if args.transform_left else args.transform_right
transform = file_interface.load_transform_json(tf_path)
logger.debug(SEP)
if not lie.is_se3(transform):
logger.warning("Not a valid SE(3) transformation!")
if args.invert_transform:
transform = lie.se3_inverse(transform)
logger.debug("Applying a {}-multiplicative transformation:\n{}".format(
tf_type, transform))
for traj in trajectories.values():
traj.transform(transform,
right_mul=args.transform_right,
propagate=args.propagate_transform)
if args.t_offset:
logger.debug(SEP)
for name, traj in trajectories.items():
if type(traj) is trajectory.PosePath3D:
die("{} doesn't have timestamps - can't add time offset.".
format(name))
logger.info("Adding time offset to {}: {} (s)".format(
name, args.t_offset))
traj.timestamps += args.t_offset
if args.n_to_align != -1 and not (args.align or args.correct_scale):
die("--n_to_align is useless without --align or/and --correct_scale")
if args.sync or args.align or args.correct_scale or args.align_origin:
from evo.core import sync
if not args.ref:
logger.debug(SEP)
die("Can't align or sync without a reference! (--ref) *grunt*")
for name, traj in trajectories.items():
if args.subcommand == "kitti":
ref_traj_tmp = ref_traj
else:
logger.debug(SEP)
ref_traj_tmp, trajectories[name] = sync.associate_trajectories(
ref_traj,
traj,
max_diff=args.t_max_diff,
first_name="reference",
snd_name=name)
if args.align or args.correct_scale:
logger.debug(SEP)
logger.debug("Aligning {} to reference.".format(name))
trajectories[name] = trajectory.align_trajectory(
trajectories[name],
ref_traj_tmp,
correct_scale=args.correct_scale,
correct_only_scale=args.correct_scale and not args.align,
n=args.n_to_align)
if args.align_origin:
logger.debug(SEP)
logger.debug("Aligning {}'s origin to reference.".format(name))
trajectories[name] = trajectory.align_trajectory_origin(
trajectories[name], ref_traj_tmp)
print_compact_name = not args.subcommand == "bag"
for name, traj in trajectories.items():
print_traj_info(name, traj, args.verbose, args.full_check,
print_compact_name)
if args.ref:
print_traj_info(args.ref, ref_traj, args.verbose, args.full_check,
print_compact_name)
if args.plot or args.save_plot or args.serialize_plot:
import numpy as np
from evo.tools import plot
import matplotlib.pyplot as plt
import matplotlib.cm as cm
plot_collection = plot.PlotCollection("evo_traj - trajectory plot")
fig_xyz, axarr_xyz = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_rpy, axarr_rpy = plt.subplots(3,
sharex="col",
figsize=tuple(SETTINGS.plot_figsize))
fig_traj = plt.figure(figsize=tuple(SETTINGS.plot_figsize))
plot_mode = plot.PlotMode[args.plot_mode]
ax_traj = plot.prepare_axis(fig_traj, plot_mode)
if args.ref:
short_traj_name = os.path.splitext(os.path.basename(args.ref))[0]
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha)
plot.draw_coordinate_axes(ax_traj, ref_traj, plot_mode,
SETTINGS.plot_axis_marker_scale)
plot.traj_xyz(axarr_xyz,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha)
plot.traj_rpy(axarr_rpy,
ref_traj,
style=SETTINGS.plot_reference_linestyle,
color=SETTINGS.plot_reference_color,
label=short_traj_name,
alpha=SETTINGS.plot_reference_alpha)
if args.ros_map_yaml:
plot.ros_map(ax_traj, args.ros_map_yaml, plot_mode)
cmap_colors = None
if SETTINGS.plot_multi_cmap.lower() != "none":
cmap = getattr(cm, SETTINGS.plot_multi_cmap)
cmap_colors = iter(cmap(np.linspace(0, 1, len(trajectories))))
for name, traj in trajectories.items():
if cmap_colors is None:
color = next(ax_traj._get_lines.prop_cycler)['color']
else:
color = next(cmap_colors)
if print_compact_name:
short_traj_name = os.path.splitext(os.path.basename(name))[0]
else:
short_traj_name = name
if SETTINGS.plot_usetex:
short_traj_name = short_traj_name.replace("_", "\\_")
plot.traj(ax_traj,
plot_mode,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha)
plot.draw_coordinate_axes(ax_traj, traj, plot_mode,
SETTINGS.plot_axis_marker_scale)
if args.ref and isinstance(ref_traj, trajectory.PoseTrajectory3D):
start_time = ref_traj.timestamps[0]
else:
start_time = None
plot.traj_xyz(axarr_xyz,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
plot.traj_rpy(axarr_rpy,
traj,
SETTINGS.plot_trajectory_linestyle,
color,
short_traj_name,
alpha=SETTINGS.plot_trajectory_alpha,
start_timestamp=start_time)
if not SETTINGS.plot_usetex:
fig_rpy.text(0.,
0.005,
"euler_angle_sequence: {}".format(
SETTINGS.euler_angle_sequence),
fontsize=6)
plot_collection.add_figure("trajectories", fig_traj)
plot_collection.add_figure("xyz_view", fig_xyz)
plot_collection.add_figure("rpy_view", fig_rpy)
if args.plot:
plot_collection.show()
if args.save_plot:
logger.info(SEP)
plot_collection.export(args.save_plot,
confirm_overwrite=not args.no_warnings)
if args.serialize_plot:
logger.info(SEP)
plot_collection.serialize(args.serialize_plot,
confirm_overwrite=not args.no_warnings)
if args.save_as_tum:
logger.info(SEP)
for name, traj in trajectories.items():
dest = os.path.splitext(os.path.basename(name))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".tum"
file_interface.write_tum_trajectory_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_kitti:
logger.info(SEP)
for name, traj in trajectories.items():
dest = os.path.splitext(os.path.basename(name))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, traj, confirm_overwrite=not args.no_warnings)
if args.ref:
dest = os.path.splitext(os.path.basename(args.ref))[0] + ".kitti"
file_interface.write_kitti_poses_file(
dest, ref_traj, confirm_overwrite=not args.no_warnings)
if args.save_as_bag:
import datetime
import rosbag
dest_bag_path = str(
datetime.datetime.now().strftime("%Y-%m-%d-%H-%M-%S")) + ".bag"
logger.info(SEP)
logger.info("Saving trajectories to " + dest_bag_path + "...")
bag = rosbag.Bag(dest_bag_path, 'w')
try:
for name, traj in trajectories.items():
dest_topic = os.path.splitext(os.path.basename(name))[0]
frame_id = traj.meta[
"frame_id"] if "frame_id" in traj.meta else ""
file_interface.write_bag_trajectory(bag, traj, dest_topic,
frame_id)
if args.ref:
dest_topic = os.path.splitext(os.path.basename(args.ref))[0]
frame_id = ref_traj.meta[
"frame_id"] if "frame_id" in ref_traj.meta else ""
file_interface.write_bag_trajectory(bag, ref_traj, dest_topic,
frame_id)
finally:
bag.close()
def rpe(traj_ref, traj_est, pose_relation, delta, delta_unit,
rel_delta_tol=0.1, all_pairs=False, align=False, correct_scale=False,
align_origin=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)
elif align_origin:
logger.debug(SEP)
traj_est = trajectory.align_trajectory_origin(traj_est, 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)"
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 test_origin_alignment(self):
traj_1 = helpers.fake_trajectory(1000, 1)
traj_2 = helpers.fake_trajectory(1000, 1)
self.assertFalse(np.allclose(traj_1.poses_se3[0], traj_2.poses_se3[0]))
traj_2 = trajectory.align_trajectory_origin(traj_2, traj_1)
self.assertTrue(np.allclose(traj_1.poses_se3[0], traj_2.poses_se3[0]))
def get_and_save_results_from_folder(folder_with_predicted_poses,category):
global args
global kitti_eval_tool
global folder_with_gt_poses
global output_folder
global t
global results
values_for_excel = []
columns_for_excel = []
type_of_statistics = 'mean'
for filename in sorted(os.listdir(folder_with_predicted_poses)):
if not(os.path.exists(os.path.join(folder_with_gt_poses, filename))):
print("file with gt poses doesn't exist for "+filename)
continue
if filename.find('.txt') == -1:
continue
seq_results = {}
seq_results['name_seq'] = filename[:filename.rfind('.')]
seq_results['category'] = category
folder_name = seq_results['category']
seq_results['metrics'] = {}
seq_results['lost'] = False
os.makedirs(os.path.join(output_folder, folder_name), exist_ok=True)
output_folder_seq = os.path.join(output_folder, folder_name, filename[:filename.rfind('.')])
os.makedirs(output_folder_seq, exist_ok=True)
if os.path.isfile(os.path.join(output_folder, folder_name,"results.txt")):
file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "a")
else:
file_results_txt = open(os.path.join(output_folder, folder_name,"results.txt"), "w")
file_results_txt.write("translation_error(%) rotation_error(deg/m) ATE(m) APE_translation_error_median(m) APE_rotation_error_median(deg) dst_to_trgt\n")
# -------------------------------------Getting results---------------------------------------------------
if args.gt_format == 'kitti':
traj_ref = file_interface.read_kitti_poses_file(os.path.join(folder_with_gt_poses, filename))
if args.gt_format == 'tum':
traj_ref = file_interface.read_tum_trajectory_file(os.path.join(folder_with_gt_poses, filename))
seq_results["length_of_ref_traj"] = traj_ref.path_length
end_time_gt = traj_ref.get_infos()["t_end (s)"]
if args.gt_format == 'euroc':
traj_ref = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_gt_poses, filename))
if args.result_format == 'kitti':
traj_est = file_interface.read_kitti_poses_file(os.path.join(folder_with_predicted_poses, filename))
if args.result_format == 'tum':
traj_est = file_interface.read_tum_trajectory_file(os.path.join(folder_with_predicted_poses, filename))
seq_results["length_of_estimated_traj"] = traj_est.path_length
if args.result_format == 'euroc':
traj_est = file_interface.read_euroc_csv_trajectory(os.path.join(folder_with_predicted_poses, filename))
if args.result_format == 'tum' and args.gt_format == 'tum':
seq_results["num_gt_poses"] = traj_ref.num_poses
seq_results["num_predicted_poses"] = traj_est.num_poses
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, args.max_diff)
end_time_est = traj_est.get_infos()["t_end (s)"]
if (abs(end_time_est - end_time_gt) > 0.2) or (traj_est.get_infos()["t_start (s)"] > 0.2):
print('LOST in track '+filename[:filename.rfind('.')])
seq_results['lost'] = True
results.append(seq_results)
t.update(1)
continue
if args.alignment != None:
traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=args.alignment.find("scale") != -1, correct_only_scale=args.alignment=="scale")
trajectory.align_trajectory_origin(traj_est, traj_ref)
data = (traj_ref, traj_est)
ape_metric_translation = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric_rotation = metrics.APE(metrics.PoseRelation.rotation_angle_deg)
ape_metric_translation.process_data(data)
ape_metric_rotation.process_data(data)
ape_translation_statistics = ape_metric_translation.get_all_statistics()
ape_rotation_statistics = ape_metric_rotation.get_all_statistics()
ape_translation_statistics_plot = copy.deepcopy(ape_translation_statistics)
ape_rotation_statistics_plot = copy.deepcopy(ape_rotation_statistics)
ape_translation_statistics_plot.pop('sse')
ape_translation_statistics_plot.pop('std')
ape_translation_statistics_plot.pop('min')
ape_translation_statistics_plot.pop('max')
ape_rotation_statistics_plot.pop('sse')
ape_rotation_statistics_plot.pop('std')
ape_rotation_statistics_plot.pop('min')
ape_rotation_statistics_plot.pop('max')
kitti_trans_err, kitti_rot_err, ate = kitti_eval_tool.eval(traj_ref.poses_se3,
traj_est.poses_se3,
alignment=None)
#---------------------------------adding results to variable seq_results for excel -----------------------------
seq_results['metrics']['dist_to_trgt'] = traj_est.get_infos()['pos_end (m)'] - traj_ref.get_infos()['pos_end (m)']
seq_results['metrics']['dist_to_trgt'] = np.sum(np.array(seq_results['metrics']['dist_to_trgt'])**2)**0.5
seq_results['metrics']["Kitti trans err (%)"] = kitti_trans_err
seq_results['metrics']["Kitti rot err (deg/m)"] = kitti_rot_err
seq_results['metrics']["ATE (m)"] = ate
seq_results['metrics']["APE(trans err) median (m)"] = ape_translation_statistics["median"]
seq_results['metrics']["APE(rot err) median (deg)"] = ape_rotation_statistics["median"]
#--------------------------------------------------------------------------------------------------------
#-------------------------------------------------------------------------------------------------------
# --------------------------------printing results into console----------------------------------------------
print('Results for "'+filename+'":')
print('Kitti average translational error (%): {:.7f}'.format(kitti_trans_err))
print('Kitti average rotational error (deg/m): {:.7f}'.format(kitti_rot_err))
print('ATE (m): {:.7f}'.format(ate))
print('APE(translation error) median (m): {:.7f}'.format(ape_translation_statistics["median"]))
print('APE(rotation error) median (deg): {:.7f}'.format(ape_rotation_statistics["median"]))
print('distance to target on the last frame: {:.7f}'.format(seq_results['metrics']['dist_to_trgt']))
#------------------------------------------------------------------------------------------------------------
#---------------------------------Saving results into overall results text file------------------------------
file_results_txt.write('{:<24} '.format(filename[:filename.rfind('.')]))
file_results_txt.write('{:>7.4f} '.format(kitti_trans_err))
file_results_txt.write('{:>7.4f} '.format(kitti_rot_err))
file_results_txt.write('{:>7.4f} '.format(ate))
file_results_txt.write('{:>7.4f} '.format(ape_translation_statistics["median"]))
file_results_txt.write('{:>7.4f} '.format(ape_rotation_statistics["median"]))
file_results_txt.write('{:>7.4f}\n'.format(seq_results['metrics']['dist_to_trgt']))
#------------------------------------------------------------------------------------------------------------
# --------------------------------Saving metrics to text file for one track----------------------------------
txt_filename = filename[:filename.rfind('.')]+"_metrics.txt"
with open(os.path.join(output_folder_seq, txt_filename), "w") as txt_file:
txt_file.write('Kitti average translational error (%): {:.7f}\n'.format(kitti_trans_err))
txt_file.write('Kitti average rotational error (deg/m): {:.7f}\n'.format(kitti_rot_err))
txt_file.write('ATE (m): {:.7f}\n'.format(ate))
txt_file.write('APE(translation error) median (m): {:.7f}\n'.format(ape_translation_statistics["median"]))
txt_file.write('APE(rotation error) median (deg): {:.7f}\n'.format(ape_rotation_statistics["median"]))
txt_file.write('Distance to target on the last frame: {:.7f}\n'.format(seq_results['metrics']['dist_to_trgt']))
#---------------------------------------------------------------------------------------------------------
# ---------------------------------Saving values of errors for each frame to text file------------------------
# ------------------------------------------for translation errors----------------------------------------
txt_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.txt"
output_folder_seq_translation = os.path.join(output_folder_seq,"translation")
output_folder_seq_rotation = os.path.join(output_folder_seq,"rotation")
os.makedirs(output_folder_seq_translation, exist_ok=True)
os.makedirs(output_folder_seq_rotation, exist_ok=True)
with open(os.path.join(output_folder_seq_translation, txt_filename), "w") as txt_file:
for error in ape_metric_translation.error:
txt_file.write('{:.10f}\n'.format(error))
# -----------------------------------------for rotation degree errors--------------------------------------
txt_filename = filename[:filename.rfind('.')]+"_APE(rotation_deg)_errors.txt"
with open(os.path.join(output_folder_seq_rotation, txt_filename), "w") as txt_file:
for error in ape_metric_rotation.error:
txt_file.write('{:.10f}\n'.format(error))
#----------------------------------------------------------------------------------------------------------
# ---------------------------------------Saving plot of errors of each frame------------------------------
# ------------------------------------------for translation errors----------------------------------------
plot_collection = plot.PlotCollection("Example")
fig_1 = plt.figure(figsize=(8, 8))
plot.error_array(fig_1, ape_metric_translation.error,
name="APE", title=str(ape_metric_translation), xlabel="Index of frame", ylabel='Error')
plot_collection.add_figure("raw", fig_1)
plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_errors.png"
plt.savefig(os.path.join(output_folder_seq_translation, plot_filename))
plt.close(fig_1)
# -----------------------------------------for rotation degree errors--------------------------------------
plot_collection = plot.PlotCollection("Example")
fig_1 = plt.figure(figsize=(8, 8))
plot.error_array(fig_1, ape_metric_rotation.error,
name="APE", title=str(ape_metric_rotation), xlabel="Index of frame", ylabel='Error')
plot_collection.add_figure("raw", fig_1)
plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_errors.png"
plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename))
plt.close(fig_1)
#-----------------------------------------------------------------------------------------------------------
# -----------------------------------------Saving trajectory plot-------------------------------------------
# ------------------------------------------for translation errors----------------------------------------
fig_2 = plt.figure(figsize=(8, 8))
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_translation.error, plot_mode,
min_map=ape_translation_statistics["min"],
max_map=ape_translation_statistics["max"], title="APE translation mapped onto trajectory")
plot_collection.add_figure("traj (error)", fig_2)
plot_filename = filename[:filename.rfind('.')]+"_APE(translation)_map.png"
plt.savefig(os.path.join(output_folder_seq_translation,plot_filename))
plt.close(fig_2)
# -----------------------------------------for rotation degree errors--------------------------------------
fig_2 = plt.figure(figsize=(8, 8))
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_rotation.error, plot_mode,
min_map=ape_rotation_statistics["min"],
max_map=ape_rotation_statistics["max"], title="APE rotation mapped onto trajectory")
plot_collection.add_figure("traj (error)", fig_2)
plot_filename = filename[:filename.rfind('.')]+"_APE(rotation)_map.png"
plt.savefig(os.path.join(output_folder_seq_rotation,plot_filename))
plt.close(fig_2)
#-----------------------------------------------------------------------------------------------------------
print()
active_worksheet = wb['sheet1']
thin = Side(border_style="thin", color="000000")
thick = Side(border_style="thick", color="000000")
medium = Side(border_style="medium", color="000000")
font_header = Font(name='Arial',
size=10,
bold=True,
italic=False,
vertAlign=None,
underline='none',
strike=False,
color='FF000000')
font_values = Font(name='Arial',
size=10,
bold=False,
italic=False,
vertAlign=None,
underline='none',
strike=False,
color='FF000000')
active_worksheet.row_dimensions[2].height = 35
file_results_txt.close()
results.append(seq_results)
t.update(1)
def ape(traj_ref_list,
traj_est_list,
pose_relation,
align=False,
correct_scale=False,
align_origin=False,
ref_name=["reference"],
est_name=["estimate"]):
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)
for i in range(len(traj_ref_list)):
print(traj_est_list[i], traj_ref_list[i])
traj_est_list[i] = trajectory.align_trajectory(
traj_est_list[i], traj_ref_list[i], correct_scale, only_scale)
elif align_origin:
logger.debug(SEP)
for i in range(len(traj_ref_list)):
traj_est_list[i] = trajectory.align_trajectory_origin(
traj_est_list[i], traj_ref_list[i])
# Calculate APE.
logger.debug(SEP)
data = [(traj_ref_list[i], traj_est_list[i])
for i in range(len(traj_ref_list))]
ape_metric = [
metrics.APE(pose_relation) for i in range(len(traj_ref_list))
]
for i in range(len(traj_ref_list)):
ape_metric[i].process_data(data[i])
ape_result = [
ape_metric[i].get_result(ref_name[i], est_name[i])
for i in range(len(traj_ref_list))
]
for i in range(len(traj_ref_list)):
title = str(ape_metric[i])
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)"
ape_result[i].info["title"] = title
logger.debug(SEP)
logger.info(ape_result[i].pretty_str())
ape_result[i].add_trajectory(ref_name[i], traj_ref_list[i])
ape_result[i].add_trajectory(est_name[i], traj_est_list[i])
if isinstance(traj_est_list[i], trajectory.PoseTrajectory3D):
seconds_from_start = [
t - traj_est_list[i].timestamps[0]
for t in traj_est_list[i].timestamps
]
ape_result[i].add_np_array("seconds_from_start",
seconds_from_start)
ape_result[i].add_np_array("timestamps",
traj_est_list[i].timestamps)
return ape_result
def four_plots(ref, est, table, name):
"""Generates plots and statistics table into Report
:ref: PoseTrajectory3D object that is used as reference
:est: PoseTrajectory3D object that is plotted against reference
:table: Tabular object that is generated by Tabular('c c')
:name: String that is used as name for file and table entry
:returns: translation of reference against estimation
"""
ref, est = sync.associate_trajectories(ref, est)
# est, rot, tra, s = trajectory.align_trajectory(est,
# ref, correct_scale=False, return_parameters=True)
est = trajectory.align_trajectory_origin(est, ref)
data = (ref, est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
# Plot x, y, xy,yaw
style = '-'
if name == 'slam':
style = 'o'
plt.style.use(['seaborn-whitegrid', 'stylerc'])
mpl.use('pgf')
mpl.rcParams.update({
"text.usetex": True,
"pgf.texsystem": "pdflatex",
})
fig, axarr = plt.subplots(2, 2, figsize=(6.125, 4.8))
plot.traj_fourplots(axarr, est, style, sns.xkcd_rgb["pale red"],
'Estimation', 1, ref.timestamps[0])
plot.traj_fourplots(axarr, ref, '-', 'gray', 'MoCap Ground Truth')
handles, labels = axarr[0, 0].get_legend_handles_labels()
fig.legend(handles,
labels,
loc='lower center',
ncol=2,
bbox_to_anchor=(0.5, 0))
plt.tight_layout()
fig.tight_layout()
fig.subplots_adjust(bottom=0.18)
fig.savefig("/home/kostas/report/figures/localization/" + name + ".pgf")
if name == 'slam':
name = name.upper()
elif name == 'odometry':
name = 'Odometry+IMU'
else:
name = name.capitalize()
table.add_row((
name,
round(ape_statistics["rmse"], 3),
round(ape_statistics["mean"], 3),
round(ape_statistics["median"], 3),
round(ape_statistics["std"], 3),
round(ape_statistics["min"], 3),
round(ape_statistics["max"], 3),
round(ape_statistics["sse"], 3),
))
table.add_hline
