def test_poses_5_stamps_5_radians(self):
target_speed = math.pi/3 # rad/s
tol = 0.01
id_pairs = filters.filter_pairs_by_angular_speed(poses_5, stamps_5, target_speed, tol)
self.assertEqual(id_pairs, [(2, 3)])
def test_poses_5_stamps_5_degrees(self):
target_speed = math.pi/3 * 180/math.pi # rad/s
tol = 0.01
id_pairs = filters.filter_pairs_by_angular_speed(poses_5, stamps_5, target_speed, tol, degrees=True)
self.assertEqual(id_pairs, [(2, 3)])
def main_rpe_for_each(traj_ref,
traj_est,
pose_relation,
mode,
bins,
rel_tols,
align=False,
correct_scale=False,
ref_name="",
est_name="",
show_plot=False,
save_plot=None,
save_results=None,
no_warnings=False,
serialize_plot=None):
from evo.algorithms import metrics
from evo.algorithms import filters
from evo.algorithms import trajectory
from evo.tools import file_interface
from evo.tools.settings import SETTINGS
if not bins or not rel_tols:
raise RuntimeError(
"bins and tolerances must have more than one element")
if len(bins) != len(rel_tols):
raise RuntimeError(
"bins and tolerances must have the same number of elements")
if mode in {"speed", "angular_speed"
} and traj_est is trajectory.PosePath3D:
raise RuntimeError("timestamps are required for mode: " + mode)
bin_unit = None
if mode == "speed":
bin_unit = metrics.VelUnit.meters_per_sec
elif mode == "path":
bin_unit = metrics.Unit.meters
elif mode == "angle":
bin_unit = metrics.Unit.degrees
elif mode == "angular_speed":
bin_unit = metrics.VelUnit.degrees_per_sec
rpe_unit = None
if pose_relation is metrics.PoseRelation.translation_part:
rpe_unit = metrics.Unit.meters
elif pose_relation is metrics.PoseRelation.rotation_angle_deg:
rpe_unit = metrics.Unit.degrees
elif pose_relation is metrics.PoseRelation.rotation_angle_rad:
rpe_unit = metrics.Unit.radians
correct_only_scale = correct_scale and not align
if align or correct_scale:
logging.debug(SEP)
if correct_only_scale:
logging.debug("correcting scale...")
else:
logging.debug("aligning using Umeyama's method..." + (
" (with scale correction)" if correct_scale else ""))
traj_est = trajectory.align_trajectory(traj_est, traj_ref,
correct_scale,
correct_only_scale)
results = []
for bin, rel_tol, in zip(bins, rel_tols):
logging.debug(SEP)
logging.info("calculating RPE for each sub-sequence of " + str(bin) +
" (" + bin_unit.value + ")")
tol = bin * rel_tol
id_pairs = []
if mode == "path":
id_pairs = filters.filter_pairs_by_path(traj_ref.poses_se3,
bin,
tol,
all_pairs=True)
elif mode == "angle":
id_pairs = filters.filter_pairs_by_angle(traj_ref.poses_se3,
bin,
tol,
degrees=True)
elif mode == "speed":
id_pairs = filters.filter_pairs_by_speed(traj_ref.poses_se3,
traj_ref.timestamps, bin,
tol)
elif mode == "angular_speed":
id_pairs = filters.filter_pairs_by_angular_speed(
traj_ref.poses_se3, traj_ref.timestamps, bin, tol, True)
if len(id_pairs) == 0:
raise RuntimeError("bin " + str(bin) + " (" + str(bin_unit.value) +
") " +
"produced empty index list - try other values")
# calculate RPE with all IDs (delta 1 frames)
data = (traj_ref, traj_est)
# the delta here has nothing to do with the bin - 1f delta just to use all poses of the bin
rpe_metric = metrics.RPE(pose_relation,
delta=1,
delta_unit=metrics.Unit.frames,
all_pairs=True)
rpe_metric.process_data(data, id_pairs)
mean = rpe_metric.get_statistic(metrics.StatisticsType.mean)
results.append(mean)
if SETTINGS.plot_usetex:
mode.replace("_", "\_")
title = "mean RPE w.r.t. " + pose_relation.value + "\nfor different " + mode + " sub-sequences"
if align and not correct_scale:
title += "\n(with SE(3) Umeyama alignment)"
elif align and correct_scale:
title += "\n(with Sim(3) Umeyama alignment)"
elif correct_only_scale:
title += "\n(scale corrected)"
else:
title += "\n(not aligned)"
logging.debug(SEP)
logging.info("\n" + title + "\n")
res_str = ""
for bin, result in zip(bins, results):
res_str += "{:>10}".format(str(bin) + "(" + bin_unit.value + ")")
res_str += "\t" + "{0:.6f}".format(result) + "\n"
logging.info(res_str)
if show_plot or save_plot or serialize_plot:
from evo.tools import plot
import matplotlib.pyplot as plt
plot_collection = plot.PlotCollection(title)
fig = plt.figure(figsize=(SETTINGS.plot_figsize[0],
SETTINGS.plot_figsize[1]))
plot.error_array(fig,
results,
x_array=bins,
name="mean RPE" +
(" (" + rpe_unit.value + ")") if rpe_unit else "",
marker="o",
title=title,
xlabel=mode + " sub-sequences " + " (" +
bin_unit.value + ")")
# info text
if SETTINGS.plot_info_text and est_name and ref_name:
ax = fig.gca()
ax.text(0,
-0.12,
"estimate: " + est_name + "\nreference: " + ref_name,
transform=ax.transAxes,
fontsize=8,
color="gray")
plt.title(title)
plot_collection.add_figure("raw", fig)
if show_plot:
plot_collection.show()
if save_plot:
plot_collection.export(save_plot,
confirm_overwrite=not no_warnings)
if serialize_plot:
logging.debug(SEP)
plot_collection.serialize(serialize_plot,
confirm_overwrite=not no_warnings)
rpe_statistics = {bin: result for bin, result in zip(bins, results)}
if save_results:
logging.debug(SEP)
# utility class to trick save_res_file
class Metric:
unit = rpe_unit
error = results
file_interface.save_res_file(save_results,
Metric,
rpe_statistics,
title,
ref_name,
est_name,
bins,
traj_ref,
traj_est,
xlabel=mode + " sub-sequences " + " (" +
bin_unit.value + ")",
confirm_overwrite=not no_warnings)
return rpe_statistics, results