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 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 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