def ape(traj_ref: PosePath3D,
traj_est: PosePath3D,
pose_relation: metrics.PoseRelation,
align: bool = False,
correct_scale: bool = False,
n_to_align: int = -1,
align_origin: bool = False,
ref_name: str = "reference",
est_name: str = "estimate") -> Result:
# Align the trajectories.
only_scale = correct_scale and not align
alignment_transformation = None
if align or correct_scale:
logger.debug(SEP)
alignment_transformation = lie_algebra.sim3(
*traj_est.align(traj_ref, correct_scale, only_scale, n=n_to_align))
elif align_origin:
logger.debug(SEP)
alignment_transformation = traj_est.align_origin(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)"
if (align or correct_scale) and n_to_align != -1:
title += " (aligned poses: {})".format(n_to_align)
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, PoseTrajectory3D):
seconds_from_start = np.array(
[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)
if alignment_transformation is not None:
ape_result.add_np_array("alignment_transformation_sim3",
alignment_transformation)
return ape_result
def get_ape_static_raw(data, mode):
ape_metric = metrics.APE(mode)
ape_metric.process_data(data)
ape_stat = np.asarray(list(
ape_metric.get_all_statistics().values())) #总体误差统计值
error_full = ape_metric.error #单个误差
return ape_stat, error_full
def stats_to_latex_table(traj_ref, segments, idx, table):
"""Associate segments of an object trajectory as given by a DATMO system
with the object's reference trajectory
:traj_ref: Reference trajectory
:segments: All the segments of the robot trajectory
:table: Latex table that the statistics get added to
"""
whole = trajectory.merge(segments)
traj_ref, traj_est = sync.associate_trajectories(traj_ref,
whole,
max_diff=0.01)
data = (traj_ref, traj_est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
# print(traj_est.get_infos())
table.add_row((
idx + 1,
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
def calc_euroc_seq_errors(est_traj, gt_traj):
gt_traj_synced, est_traj_synced = sync.associate_trajectories(
gt_traj, est_traj, max_diff=0.01)
est_traj_aligned = trajectory.align_trajectory(est_traj_synced,
gt_traj_synced,
correct_scale=False,
correct_only_scale=False)
pose_relation = metrics.PoseRelation.translation_part
ape_metric = metrics.APE(pose_relation)
ape_metric.process_data((
gt_traj_synced,
est_traj_aligned,
))
ape_stat = ape_metric.get_statistic(metrics.StatisticsType.rmse)
# ape_metric = metrics.RPE(pose_relation)
# ape_metric.process_data((gt_traj_synced, est_traj_aligned,))
# ape_stat = ape_metric.get_statistic(metrics.StatisticsType.rmse)
# fig = plt.figure()
# traj_by_label = {
# "estimate (not aligned)": est_traj,
# "estimate (aligned)": est_traj_aligned,
# "reference": gt_traj
# }
# plot.trajectories(fig, traj_by_label, plot.PlotMode.xyz)
# plt.show()
return ape_metric, ape_stat
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 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 associate_segments_common_frame(traj, tracks, distance):
"""Associate segments of an object trajectory as given by a DATMO system
with the object's reference trajectory
:traj: Reference trajectory
:tracks: All the tracks that got produced by the DATMO system
:localization: The trajectory of the self-localization
:returns: segments: The tracks that match to the reference trajectory
:returns: traj_ref: The part of the reference trajectory that matches with
tracks
"""
matches = []
for tr in tracks: # Find the best matching tracks to the object trajectory
traj_ref, traj_est = sync.associate_trajectories(traj,
tr,
max_diff=0.1)
# print("calculating APE for track of length", len(tr.timestamps))
data = (traj_ref, traj_est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
# print(ape_statistics)
mismatch = ape_statistics['mean']
# print(mismatch)
tuple = [
traj_est, mismatch,
traj_est.get_infos()['t_start (s)'], traj_ref
]
matches.append(tuple)
matches.sort(key=lambda x: x[2])
segments_track = [] #The parts of the trajectory are added to this list
segments_refer = [
] #The parts of the reference trajectory are added to this list
for m in matches:
if m[1] < distance: # if the mismatch is smaller than 1
# print(m[1],distance)
# print(m[0].get_statistics()['v_avg (m/s)'])
segments_track.append(m[0])
segments_refer.append(m[3])
# print(m[0].get_infos()['t_start (s)'],m[0].get_infos()["path length (m)"])
# print(m[0].get_statistics()['v_avg (m/s)'])
if len(segments_track) == 0:
print("No matching segments")
traj_ref = trajectory.merge(segments_refer)
# print(traj_ref.length)
return segments_track, traj_ref
def get_ape_rot(data):
""" Return APE rotation 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.
"""
ape_rot = metrics.APE(metrics.PoseRelation.rotation_angle_deg)
ape_rot.process_data(data)
return ape_rot
def get_ape_trans(data):
""" Return APE 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.
"""
ape_trans = metrics.APE(metrics.PoseRelation.translation_part)
ape_trans.process_data(data)
return ape_trans
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 three_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)
data = (ref, est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
# [ Localization ]
fig, axarr = plt.subplots(3) #sharex=True)
fig.suptitle('Localization', fontsize=30)
fig.tight_layout()
plot.traj_xyyaw(axarr, est, '-', 'red', 'estimation', 1, ref.timestamps[0])
plot.traj_xyyaw(axarr, ref, '-', 'gray', 'original')
fig.subplots_adjust(hspace=0.2)
plt.waitforbuttonpress(0)
plt.savefig("/home/kostas/results/latest/" + name + ".png",
format='png',
bbox_inches='tight')
plt.close(fig)
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
def compare_using_APE(ref_file, est_file, use_aligned_trajectories=False):
"""
Compare two files using EVO API. Using the APE metric.
:param ref_file:
:param est_file:
:param use_aligned_trajectories: True to align before comparing. False to leave original data as it is.
:return:
"""
# Load trajectories
traj_ref = file_interface.read_tum_trajectory_file(ref_file)
traj_est = file_interface.read_tum_trajectory_file(est_file)
# Sinchronize trajectories by timestamps
max_diff = 0.01
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est, max_diff)
# -------------EVO_APE-------------
# Settings
pose_relation = metrics.PoseRelation.translation_part
use_aligned_trajectories = False # OPTION -va on the scripts. Is related to Uleyamas alignment
# The aligned trajectories can be used if we want it to
if use_aligned_trajectories:
# Align trajectories with Uleyamas algorithm
try:
traj_est_aligned = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False,
correct_only_scale=False)
data = (traj_ref, traj_est_aligned)
except GeometryException:
print("Couldnt align with Uleyamas algorithm...")
data = (traj_ref, traj_est)
else:
data = (traj_ref, traj_est)
ape_metric = metrics.APE(pose_relation) # APE with only pose is in reality ATE (Absolute trajectory error) instead of APE (abs. pose error)
ape_metric.process_data(data)
# Get all stadistics in a dict
ape_stats = ape_metric.get_all_statistics()
return ape_stats
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
gt_file = f'{gt_path}/{seq}.csv'
failure_count = 0
if not len(traj_files) == 0:
mean, rmse = [], []
for traj_file in traj_files:
traj_gt = file_interface.read_euroc_csv_trajectory(gt_file)
traj_est = file_interface.read_tum_trajectory_file(traj_file)
kf_traj_est = file_interface.read_tum_trajectory_file(traj_file)
traj_gt, traj_est = sync.associate_trajectories(traj_gt, traj_est)
traj_est_aligned, rot, trans, scale = trajectory.align_trajectory(
traj_est, traj_gt, correct_scale=True, return_parameters=True)
data = (traj_gt, traj_est_aligned)
ape_metric = metrics.APE(pose_relation=pose_relation)
ape_metric.process_data(data)
ape_stat = ape_metric.get_all_statistics()
mean_curr = ape_stat['mean']
rmse_curr = ape_stat['rmse']
if mean_curr > 1.0 or rmse_curr > 1.0:
failure_count += 1
continue
mean.append(mean_curr)
rmse.append(rmse_curr)
print(
f'{seq}: mean: {np.mean(mean)}, rmse: {np.mean(rmse)}, #failure {failure_count}'
)
else:
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)
def associate_segments(traj, tracks):
"""Associate segments of an object trajectory as given by a DATMO system
with the object's reference trajectory
:traj: Reference trajectory
:tracks: All the tracks that got produced by the DATMO system
:localization: The trajectory of the self-localization
:returns: segments: The tracks that match to the reference trajectory
:returns: traj_ref: The part of the reference trajectory that matches with
tracks
"""
matches = []
for tr in tracks: # Find the best matching tracks to the object trajectory
traj_ref, traj_est = sync.associate_trajectories(traj,
tr,
max_diff=0.01)
traj_est, rot, tra, _ = trajectory.align_trajectory(
traj_est, traj_ref, correct_scale=False, return_parameters=True)
# print("calculating APE for track of length", len(tr.timestamps))
data = (traj_ref, traj_est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
tra_dif = (tra - loc_tra)
# print(tra)
abs_tra_dif = abs((tra - loc_tra)[0]) + abs((tra - loc_tra)[1])
translation = abs(tra[0]) + abs(tra[1])
rot_dif = (rot - loc_rot)
abs_rot_dif = 0
for i in range(0, len(rot_dif)): abs_rot_dif += abs(rot_dif[i][0])+ abs(rot_dif[i][1]) +\
abs(rot_dif[i][2])
# print(abs_tra_dif,abs_rot_dif)
mismatch = abs_tra_dif + abs_rot_dif
tuple = [traj_est, mismatch, traj_est.get_infos()['t_start (s)']]
matches.append(tuple)
matches.sort(key=lambda x: x[2])
segments = [] #The parts of the trajectory are added to this list
for m in matches:
# print(m[1])
if m[1] < 0.1: # if the mismatch is smaller than 1
# print(m[0].get_statistics()['v_avg (m/s)'])
segments.append(m[0])
# print(m[0].get_infos()['t_start (s)'],m[0].get_infos()["path length (m)"])
# print(m[0].get_statistics()['v_avg (m/s)'])
if len(segments) == 0:
print("No matching segments")
whole = trajectory.merge(segments)
traj_ref, traj_est = sync.associate_trajectories(traj,
whole,
max_diff=0.01)
traj_est, rot, tra, _ = trajectory.align_trajectory(traj_est,
traj_ref,
correct_scale=False,
return_parameters=True)
# print(traj_est.get_infos())
return segments, traj_ref, translation
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)
from evo.core import trajectory, sync, metrics
from evo.tools import file_interface
print("loading trajectories")
traj_ref = file_interface.read_tum_trajectory_file(
"../../test/data/fr2_desk_groundtruth.txt")
traj_est = file_interface.read_tum_trajectory_file(
"../../test/data/fr2_desk_ORB.txt")
print("registering and aligning trajectories")
traj_ref, traj_est = sync.associate_trajectories(traj_ref, traj_est)
traj_est = trajectory.align_trajectory(traj_est, traj_ref, correct_scale=False)
print("calculating APE")
data = (traj_ref, traj_est)
ape_metric = metrics.APE(metrics.PoseRelation.translation_part)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
print("mean:", ape_statistics["mean"])
print("loading plot modules")
from evo.tools import plot
import matplotlib.pyplot as plt
print("plotting")
plot_collection = plot.PlotCollection("Example")
# metric values
fig_1 = plt.figure(figsize=(8, 8))
plot.error_array(fig_1,
ape_metric.error,
statistics=ape_statistics,
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 main_ape(traj_ref,
traj_est,
pose_relation,
align=True,
correct_scale=False,
ref_name="",
est_name="",
show_plot=False,
save_plot=None,
plot_mode=None,
save_results=None,
no_warnings=False,
serialize_plot=None,
plot_colormap_max=None,
plot_colormap_min=None,
plot_colormap_max_percentile=None):
from evo.core import metrics, result
from evo.core import trajectory
from evo.tools import file_interface
from evo.tools.settings import SETTINGS
import numpy as np
only_scale = correct_scale and not align
if align or correct_scale:
logger.debug(SEP)
if 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, only_scale)
logger.debug(SEP)
# calculate APE
data = (traj_ref, traj_est)
ape_metric = metrics.APE(pose_relation)
ape_metric.process_data(data)
ape_statistics = ape_metric.get_all_statistics()
title = str(ape_metric)
if align and not correct_scale:
title += "\n(with SE(3) Umeyama alignment)"
elif align and correct_scale:
title += "\n(with Sim(3) Umeyama alignment)"
elif only_scale:
title += "\n(scale corrected)"
else:
title += "\n(not aligned)"
ape_result = ape_metric.get_result(ref_name, est_name)
logger.debug(SEP)
logger.info(ape_result.pretty_str())
if isinstance(traj_est, trajectory.PoseTrajectory3D):
seconds_from_start = [
t - traj_est.timestamps[0] for t in traj_est.timestamps
]
ape_result.add_np_array("seconds_from_start", seconds_from_start)
ape_result.add_np_array("timestamps", traj_est.timestamps)
else:
seconds_from_start = None
if show_plot or save_plot or save_results or serialize_plot:
if show_plot or save_plot or serialize_plot:
from evo.tools import plot
import matplotlib.pyplot as plt
logger.debug(SEP)
logger.debug("plotting results... ")
fig1 = plt.figure(figsize=(SETTINGS.plot_figsize[0],
SETTINGS.plot_figsize[1]))
# metric values
plot.error_array(
fig1,
ape_metric.error,
x_array=seconds_from_start,
statistics=ape_statistics,
name="APE" + (" (" + ape_metric.unit.value + ")")
if ape_metric.unit else "",
title=title,
xlabel="$t$ (s)" if seconds_from_start else "index")
# info text
if SETTINGS.plot_info_text and est_name and ref_name:
ax = fig1.gca()
ax.text(0,
-0.12,
"estimate: " + est_name + "\nreference: " + ref_name,
transform=ax.transAxes,
fontsize=8,
color="gray")
# trajectory colormapped
fig2 = plt.figure(figsize=(SETTINGS.plot_figsize[0],
SETTINGS.plot_figsize[1]))
plot_mode = plot_mode if plot_mode is not None else plot.PlotMode.xyz
ax = plot.prepare_axis(fig2, plot_mode)
plot.traj(ax,
plot_mode,
traj_ref,
'--',
'gray',
'reference',
alpha=0.0 if SETTINGS.plot_hideref else 1.0)
min_map = ape_statistics[
"min"] if plot_colormap_min is None else plot_colormap_min
if plot_colormap_max_percentile is not None:
max_map = np.percentile(ape_result.np_arrays["error_array"],
plot_colormap_max_percentile)
else:
max_map = ape_statistics[
"max"] if plot_colormap_max is None else plot_colormap_max
plot.traj_colormap(ax,
traj_est,
ape_metric.error,
plot_mode,
min_map=min_map,
max_map=max_map,
title="APE mapped onto trajectory")
fig2.axes.append(ax)
plot_collection = plot.PlotCollection(title)
plot_collection.add_figure("raw", fig1)
plot_collection.add_figure("map", fig2)
if show_plot:
plot_collection.show()
if save_plot:
plot_collection.export(save_plot,
confirm_overwrite=not no_warnings)
if serialize_plot:
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:
ape_result.add_trajectory("traj_ref", traj_ref)
ape_result.add_trajectory("traj_est", traj_est)
file_interface.save_res_file(save_results,
ape_result,
confirm_overwrite=not no_warnings)
return ape_result
