diff --git a/evo/common_ape_rpe.py b/evo/common_ape_rpe.py index 36263f8c..e7ae4e11 100644 --- a/evo/common_ape_rpe.py +++ b/evo/common_ape_rpe.py @@ -91,6 +91,8 @@ def get_pose_relation(args: argparse.Namespace) -> PoseRelation: pose_relation = PoseRelation.point_distance elif args.pose_relation == "point_distance_error_ratio": pose_relation = PoseRelation.point_distance_error_ratio + elif args.pose_relation == "scale_error": + pose_relation = PoseRelation.scale_error return pose_relation diff --git a/evo/core/metrics.py b/evo/core/metrics.py index 4066df63..acd4c062 100644 --- a/evo/core/metrics.py +++ b/evo/core/metrics.py @@ -65,6 +65,7 @@ class PoseRelation(Enum): rotation_angle_deg = "rotation angle in degrees" point_distance = "point distance" point_distance_error_ratio = "point distance error ratio" + scale_error = "scale error" class Unit(Enum): @@ -198,7 +199,8 @@ def __init__(self, if pose_relation in (PoseRelation.translation_part, PoseRelation.point_distance): self.unit = Unit.meters - elif pose_relation == PoseRelation.point_distance_error_ratio: + elif pose_relation in (PoseRelation.point_distance_error_ratio, + PoseRelation.scale_error): self.unit = Unit.percent elif pose_relation == PoseRelation.rotation_angle_deg: self.unit = Unit.degrees @@ -280,6 +282,23 @@ def process_data(self, data: PathPair) -> None: self.delta_ids = [self.delta_ids[i] for i in nonzero] self.error = np.divide(self.error[nonzero], ref_distances[nonzero]) * 100 + elif self.pose_relation == PoseRelation.scale_error: + # Compares the magnitude of the traveled distance. + ref_traveled_distances = np.array([ + traj_ref.distances[i] - traj_ref.distances[j] for i, j in id_pairs + ]) + est_traveled_distances = np.array([ + traj_est.distances[i] - traj_est.distances[j] for i, j in id_pairs + ]) + self.error = est_traveled_distances + nonzero = ref_traveled_distances.nonzero()[0] + if nonzero.size != ref_traveled_distances.size: + logger.warning( + f"Ignoring {ref_traveled_distances.size - nonzero.size} zero " + "divisions in ratio calculations.") + self.delta_ids = [self.delta_ids[i] for i in nonzero] + self.error = (np.divide(self.error[nonzero], + ref_traveled_distances[nonzero]) - 1) * 100 else: # All other pose relations require the full pose error. self.E = [ @@ -298,7 +317,8 @@ def process_data(self, data: PathPair) -> None: self.pose_relation.value)) if self.pose_relation in (PoseRelation.point_distance, - PoseRelation.point_distance_error_ratio): + PoseRelation.point_distance_error_ratio, + PoseRelation.scale_error): # Already computed, see above. pass elif self.pose_relation == PoseRelation.translation_part: diff --git a/evo/main_rpe.py b/evo/main_rpe.py index 6886789e..34d7e8d4 100755 --- a/evo/main_rpe.py +++ b/evo/main_rpe.py @@ -51,7 +51,7 @@ def parser() -> argparse.ArgumentParser: "-r", "--pose_relation", default="trans_part", help="pose relation on which the RPE is based", choices=[ "full", "trans_part", "rot_part", "angle_deg", "angle_rad", - "point_distance", "point_distance_error_ratio" + "point_distance", "point_distance_error_ratio", "scale_error" ]) algo_opts.add_argument("-a", "--align", help="alignment with Umeyama's method (no scale)",