def test_poses2_all_pairs_low_tolerance(self): target_path = 1.0 tol = 0.001 id_pairs = filters.filter_pairs_by_path(poses_2, target_path, tol, all_pairs=True) self.assertEqual(id_pairs, [(0, 3)])
def test_convergence_all_pairs(self): target_path = 1.0 tol = 0.2 id_pairs = filters.filter_pairs_by_path(poses_3, target_path, tol, all_pairs=True) self.assertEqual(id_pairs, [(0, 7)])
def test_poses1_wrong_target(self): target_path = 2.5 tol = 0.0 id_pairs = filters.filter_pairs_by_path(poses_1, target_path, tol, all_pairs=True) self.assertEqual(id_pairs, [])
def test_poses1_all_pairs(self): target_path = 1.0 tol = 0.0 id_pairs = filters.filter_pairs_by_path(poses_1, target_path, tol, all_pairs=True) self.assertEqual(id_pairs, [(0, 2), (2, 3)])
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