From c91f3900a0aa997e6480a93832a2c4c2930c3400 Mon Sep 17 00:00:00 2001 From: GAECHTER TOYA Stefan Date: Tue, 7 Jun 2022 12:34:58 +0200 Subject: [PATCH 1/2] Add error metrics for in-plane and height error --- evo/common_ape_rpe.py | 4 ++++ evo/core/metrics.py | 24 +++++++++++++++++++++--- 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/evo/common_ape_rpe.py b/evo/common_ape_rpe.py index 695f1d1c..92b6240d 100644 --- a/evo/common_ape_rpe.py +++ b/evo/common_ape_rpe.py @@ -83,6 +83,10 @@ def get_pose_relation(args: argparse.Namespace) -> PoseRelation: pose_relation = PoseRelation.rotation_part elif args.pose_relation == "trans_part": pose_relation = PoseRelation.translation_part + elif args.pose_relation == "in_plane_part": + pose_relation = PoseRelation.in_plane_part + elif args.pose_relation == "height_part": + pose_relation = PoseRelation.height_part elif args.pose_relation == "angle_deg": pose_relation = PoseRelation.rotation_angle_deg elif args.pose_relation == "angle_rad": diff --git a/evo/core/metrics.py b/evo/core/metrics.py index 4066df63..1b59abd8 100644 --- a/evo/core/metrics.py +++ b/evo/core/metrics.py @@ -60,6 +60,8 @@ class StatisticsType(Enum): class PoseRelation(Enum): full_transformation = "full transformation" translation_part = "translation part" + in_plane_part = "in-plane part" + height_part = "height part" rotation_part = "rotation part" rotation_angle_rad = "rotation angle in radians" rotation_angle_deg = "rotation angle in degrees" @@ -196,7 +198,9 @@ def __init__(self, self.error = np.array([]) self.delta_ids: typing.List[int] = [] if pose_relation in (PoseRelation.translation_part, - PoseRelation.point_distance): + PoseRelation.point_distance, + PoseRelation.in_plane_part, + PoseRelation.height_part): self.unit = Unit.meters elif pose_relation == PoseRelation.point_distance_error_ratio: self.unit = Unit.percent @@ -303,6 +307,10 @@ def process_data(self, data: PathPair) -> None: pass elif self.pose_relation == PoseRelation.translation_part: self.error = [np.linalg.norm(E_i[:3, 3]) for E_i in self.E] + elif self.pose_relation == PoseRelation.in_plane_part: + self.error = [np.linalg.norm(E_i[0:2, 3]) for E_i in self.E] + elif self.pose_relation == PoseRelation.height_part: + self.error = [E_i[2, 3] for E_i in self.E] elif self.pose_relation == PoseRelation.rotation_part: # ideal: rot(E_i) = 3x3 identity self.error = np.array([ @@ -335,7 +343,9 @@ def __init__(self, self.E: typing.List[np.ndarray] = [] self.error = np.array([]) if pose_relation in (PoseRelation.translation_part, - PoseRelation.point_distance): + PoseRelation.point_distance, + PoseRelation.in_plane_part, + PoseRelation.height_part): self.unit = Unit.meters elif pose_relation == PoseRelation.rotation_angle_deg: self.unit = Unit.degrees @@ -377,7 +387,9 @@ def process_data(self, data: PathPair) -> None: "trajectories must have same number of poses") if self.pose_relation in (PoseRelation.translation_part, - PoseRelation.point_distance): + PoseRelation.point_distance, + PoseRelation.in_plane_part, + PoseRelation.height_part): # Translation part of APE is equivalent to distance between poses, # we don't require full SE(3) matrices for faster computation. self.E = traj_est.positions_xyz - traj_ref.positions_xyz @@ -394,6 +406,12 @@ def process_data(self, data: PathPair) -> None: PoseRelation.point_distance): # E is an array of position vectors only in this case self.error = np.array([np.linalg.norm(E_i) for E_i in self.E]) + elif self.pose_relation == PoseRelation.in_plane_part: + # E is an array of position vectors only in this case + self.error = np.array([np.linalg.norm(E_i[0:2]) for E_i in self.E]) + elif self.pose_relation == PoseRelation.height_part: + # E is an array of position vectors only in this case + self.error = np.array([E_i[2] for E_i in self.E]) elif self.pose_relation == PoseRelation.rotation_part: self.error = np.array([ np.linalg.norm(lie.so3_from_se3(E_i) - np.eye(3)) From 284a5305b6693ab52d2b7b2eea2f392fb996c6d4 Mon Sep 17 00:00:00 2001 From: GAECHTER TOYA Stefan Date: Thu, 9 Jun 2022 11:20:24 +0200 Subject: [PATCH 2/2] Complete help of main scripts --- evo/main_ape.py | 4 ++-- evo/main_rpe.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/evo/main_ape.py b/evo/main_ape.py index ab60f2a8..54f831e8 100755 --- a/evo/main_ape.py +++ b/evo/main_ape.py @@ -50,8 +50,8 @@ def parser() -> argparse.ArgumentParser: algo_opts.add_argument( "-r", "--pose_relation", default="trans_part", help="pose relation on which the APE is based", choices=[ - "full", "trans_part", "rot_part", "angle_deg", "angle_rad", - "point_distance" + "full", "trans_part", "in_plane_part", "height_part", "rot_part", + "angle_deg", "angle_rad", "point_distance" ]) algo_opts.add_argument("-a", "--align", help="alignment with Umeyama's method (no scale)", diff --git a/evo/main_rpe.py b/evo/main_rpe.py index 6886789e..1d6f5838 100755 --- a/evo/main_rpe.py +++ b/evo/main_rpe.py @@ -50,8 +50,8 @@ def parser() -> argparse.ArgumentParser: algo_opts.add_argument( "-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" + "full", "trans_part", "in_plane_part", "height_part", "rot_part", + "angle_deg", "angle_rad", "point_distance", "point_distance_error_ratio" ]) algo_opts.add_argument("-a", "--align", help="alignment with Umeyama's method (no scale)",