def test_poses6_all_pairs(self):
target_angle = math.pi
tol = 0.001
id_pairs = filters.filter_pairs_by_angle(poses_6,
target_angle,
tol,
all_pairs=True)
self.assertEqual(id_pairs, [(0, 3), (0, 4)])
def test_poses5_all_pairs(self):
target_angle = math.pi
tol = 0.01
id_pairs = filters.filter_pairs_by_angle(poses_5,
target_angle,
tol,
all_pairs=True)
self.assertEqual(id_pairs, [(0, 1), (0, 4), (1, 2), (2, 4)])
def test_poses6(self):
tol = 0.001
target_angle = math.pi - tol
id_pairs = filters.filter_pairs_by_angle(poses_6,
target_angle,
tol,
all_pairs=False)
self.assertEqual(id_pairs, [(0, 3)])
def test_poses5_all_pairs(self):
tol = 0.01
expected_result = [(0, 1), (0, 4), (1, 2), (2, 4)]
# Result should be unaffected by global transformation.
for poses in (POSES_5, POSES_5_TRANSFORMED):
target_angle = math.pi
id_pairs = filters.filter_pairs_by_angle(poses,
target_angle,
tol,
all_pairs=True)
self.assertEqual(id_pairs, expected_result)
# Check for same result when using degrees:
target_angle = np.rad2deg(target_angle)
id_pairs = filters.filter_pairs_by_angle(poses,
target_angle,
tol,
all_pairs=True,
degrees=True)
self.assertEqual(id_pairs, expected_result)
def test_poses6_all_pairs(self):
target_angle = math.pi
tol = 0.001
expected_result = [(0, 3), (0, 4)]
# Result should be unaffected by global transformation.
for poses in (POSES_6, POSES_6_TRANSFORMED):
id_pairs = filters.filter_pairs_by_angle(poses,
target_angle,
tol,
all_pairs=True)
self.assertEqual(id_pairs, expected_result)
def id_pairs_from_delta(poses: typing.Sequence[np.ndarray],
delta: float,
delta_unit: Unit,
rel_tol: float = 0.1,
all_pairs: bool = False) -> filters.IdPairs:
"""
high-level function - get index tuples of pairs with distance==delta
from a pose list
:param poses: list of SE(3) poses
:param delta: the interval step for indices
:param delta_unit: unit of delta (metrics.Unit enum member)
:param rel_tol: relative tolerance to accept or reject deltas
:param all_pairs: use all pairs instead of consecutive pairs
:return: list of index tuples (pairs)
"""
if delta_unit == Unit.frames:
id_pairs = filters.filter_pairs_by_index(poses, int(delta), all_pairs)
elif delta_unit == Unit.meters:
id_pairs = filters.filter_pairs_by_path(poses, delta, delta * rel_tol,
all_pairs)
elif delta_unit in {Unit.degrees, Unit.radians}:
use_degrees = (delta_unit == Unit.degrees)
id_pairs = filters.filter_pairs_by_angle(poses, delta, delta * rel_tol,
use_degrees, all_pairs)
else:
raise filters.FilterException(
"unsupported delta unit: {}".format(delta_unit))
if len(id_pairs) == 0:
raise filters.FilterException(
"delta = {} ({}) produced an empty index list - try lower values "
"or a less strict tolerance".format(delta, delta_unit.value))
logger.debug(
"Found {} pairs with delta {} ({}) "
"among {} poses ".format(len(id_pairs), delta, delta_unit.value,
len(poses)) +
("using consecutive pairs." if not all_pairs else "using all pairs."))
return id_pairs
def id_pairs_from_delta(poses,
delta,
delta_unit,
rel_tol=0.1,
all_pairs=False):
"""
get index tuples of pairs with distance==delta from a pose list
:param poses: list of SE(3) poses
:param delta: the interval step for indices
:param delta_unit: unit of delta (Unit enum member)
:param rel_tol: relative tolerance to accept or reject deltas
:param all_pairs: use all possible pairs instead of consecutive pairs
:return: list of index tuples (pairs)
"""
if delta_unit == Unit.frames:
id_pairs = filters.filter_pairs_by_index(poses, delta, all_pairs)
elif delta_unit == Unit.meters:
id_pairs = filters.filter_pairs_by_path(poses, delta, delta * rel_tol,
all_pairs)
elif delta_unit in {Unit.degrees, Unit.radians}:
use_degrees = (delta_unit == Unit.degrees)
id_pairs = filters.filter_pairs_by_angle(poses, delta, delta * rel_tol,
use_degrees, all_pairs)
else:
raise MetricsException("unsupported delta unit: " + str(delta_unit))
if len(id_pairs) == 0:
raise MetricsException(
"delta = " + str(delta) + " (" + str(delta_unit.value) + ") " +
"produced empty index list - try lower values or higher tolerance")
logger.debug("found " + str(len(id_pairs)) + " pairs with delta " +
str(delta) + " (" + str(delta_unit.value) + ") among " +
str(len(poses)) + " poses " +
("using consecutive pairs "
if not all_pairs else "using all possible pairs"))
return id_pairs
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.core import metrics, result
from evo.core import filters
from evo.core 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:
logger.debug(SEP)
if correct_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,
correct_only_scale)
results = []
for bin, rel_tol, in zip(bins, rel_tols):
logger.debug(SEP)
logger.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)"
rpe_for_each_result = result.Result()
rpe_for_each_result.add_info({
"label": "RPE ({})".format(rpe_unit.value),
"est_name": est_name,
"ref_name": ref_name,
"title": title,
"xlabel": "{} sub-sequences ({})".format(mode, bin_unit.value)
})
rpe_for_each_result.add_stats({bin: result for bin, result in zip(bins, results)})
# TODO use a more suitable name than seconds
rpe_for_each_result.add_np_array("seconds_from_start", bins)
rpe_for_each_result.add_np_array("error_array", results)
logger.debug(SEP)
logger.info(rpe_for_each_result.pretty_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:
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:
rpe_for_each_result.add_trajectory("traj_ref", traj_ref)
rpe_for_each_result.add_trajectory("traj_est", traj_est)
file_interface.save_res_file(save_results, rpe_for_each_result,
confirm_overwrite=not no_warnings)
return rpe_for_each_result
