Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

In plane and height error #477

Open
wants to merge 3 commits into
base: master
Choose a base branch
from
Open

In plane and height error #477

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c91f390
Add error metrics for in-plane and height error
stefan-gaechter-hxgn-geosystems Jun 7, 2022
83fb8db
Merge branch 'master' into in_plane_and_height_error
stefan-gaechter-hxgn-geosystems Jun 7, 2022
284a530
Complete help of main scripts
stefan-gaechter-hxgn-geosystems Jun 9, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
4 changes: 4 additions & 0 deletions evo/common_ape_rpe.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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":
Expand Down
24 changes: 21 additions & 3 deletions evo/core/metrics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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([
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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))
Expand Down
4 changes: 2 additions & 2 deletions evo/main_ape.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)",
Expand Down
4 changes: 2 additions & 2 deletions evo/main_rpe.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)",
Expand Down