(fix) Fixes geometry transformation utils

Author: mkabtoul

src/kompass_core/mapping/local_mapper.py

@@ -9,7 +9,7 @@
     OCCUPANCY_TYPE,
 )
 
-from ..utils.geometry import from_frame1_to_frame2, get_pose_target_in_reference_frame
+from ..utils.geometry import transform_point_from_local_to_global, get_relative_pose
 
 from .laserscan_model import LaserScanModelConfig
 from ..utils.common import BaseAttrs, base_validators
@@ -221,8 +221,8 @@ def _calculate_grid_shift(self, current_robot_pose: PoseData):
         # i.e. we have a t+1 state
         # get current shift in translation and orientation of the new center
         # with respect to the previous old center
-        pose_current_robot_in_previous_robot = get_pose_target_in_reference_frame(
-            reference_pose=self._pose_robot_in_world, target_pose=current_robot_pose
+        pose_current_robot_in_previous_robot = get_relative_pose(
+            pose_1_in_ref=self._pose_robot_in_world, pose_2_in_ref=current_robot_pose
         )
         # new position and orientation with respect to the previous pose
         _position_in_previous_pose = pose_current_robot_in_previous_robot.get_position()
@@ -264,8 +264,8 @@ def update_from_scan(
 
         # Calculate new grid pose
         self._pose_robot_in_world = robot_pose
-        self.lower_right_corner_pose = from_frame1_to_frame2(
-            robot_pose, self._local_lower_right_corner_point
+        self.lower_right_corner_pose = transform_point_from_local_to_global(
+            self._local_lower_right_corner_point, robot_pose
         )
 
         if self.config.baysian_update:

src/kompass_core/utils/geometry.py

@@ -116,74 +116,72 @@ def _rotate_vector_by_quaternion(q: np.ndarray, v: List[float]) -> List[float]:
     return rotated_vq[1:].tolist()
 
 
-def get_pose_target_in_reference_frame(
-    reference_pose: PoseData, target_pose: PoseData
-) -> PoseData:
+def _transform_pose(pose: PoseData, reference_pose: PoseData) -> PoseData:
     """
-    Computes a target pose with respect to the frame (coordinate system) defined by a reference.
-    target and reference should be given first in a common frame
-
-    :param      reference_pose:     Pose of reference frame in a common frame
-    :type       reference_pose:     PoseData
-    :param      target_pose:        Pose of target frame in a common frame
-    :type       target_pose:        PoseData
+    Transforms a pose from a local frame to a global frame using a reference pose.
+    Equivalent to: global_pose = reference_pose * local_pose
 
-    :return:    The pose of target in the reference frame
-    :rtype:     PoseData
+    :param pose: PoseData in local frame
+    :type pose: PoseData
+    :param reference_pose: PoseData of local frame in global frame
+    :type reference_pose: PoseData
+    :return: PoseData in global frame
+    :rtype: PoseData
     """
-    position_target = target_pose.get_position()
-    orientation_target = target_pose.get_orientation()
-
-    reference_position = reference_pose.get_position()
-    reference_rotation = reference_pose.get_orientation()
-
-    orientation_target_in_ref = (
-        _np_quaternion_conjugate(reference_rotation) * orientation_target
-    )
+    rotated_position = _rotate_vector_by_quaternion(reference_pose.get_orientation(), pose.get_position().tolist())
+    translated_position = reference_pose.get_position() + np.array(rotated_position)
 
-    position_target_in_ref = _rotate_vector_by_quaternion(
-        _np_quaternion_conjugate(reference_rotation),
-        (position_target - reference_position).tolist(),
-    )
+    combined_orientation = _np_quaternion_multiply(reference_pose.get_orientation(), pose.get_orientation())
 
-    target_pose_in_ref = PoseData()
-    target_pose_in_ref.set_pose(
-        x=position_target_in_ref[0],
-        y=position_target_in_ref[1],
-        z=position_target_in_ref[2],
-        qw=orientation_target_in_ref[0],
-        qx=orientation_target_in_ref[1],
-        qy=orientation_target_in_ref[2],
-        qz=orientation_target_in_ref[3],
+    result = PoseData()
+    result.set_pose(
+        *translated_position,
+        *combined_orientation
     )
+    return result
 
-    return target_pose_in_ref
 
+def _inverse_pose(pose: PoseData) -> PoseData:
+    """
+    Computes the inverse of a pose (converts pose_in_frame to frame_in_pose)
 
-def from_frame1_to_frame2(
-    pose_1_in_2: PoseData, pose_target_in_1: PoseData
-) -> PoseData:
+    :param pose: PoseData to invert
+    :type pose: PoseData
+    :return: Inverse pose
+    :rtype: PoseData
     """
-    get the pose of a target in frame 2 instead of frame 1
+    inv_orientation = _np_quaternion_conjugate(pose.get_orientation())
+    inv_position = _rotate_vector_by_quaternion(inv_orientation, (-pose.get_position()).tolist())
+
+    result = PoseData()
+    result.set_pose(
+        *inv_position,
+        *inv_orientation
+    )
+    return result
 
-    :param      pose_1_in_2:        pose of frame 1 in frame 2
-    :type       pose_1_in_2:        PoseData
-    :param      pose_target_in_1:   pose of target in frame 1
-    :type       pose_target_in_1:   PoseData
+def transform_point_from_local_to_global(point_pose_in_local: PoseData, robot_pose_in_global: PoseData) -> PoseData:
+    """Transforms a point from the robot frame to the global frame.
 
-    :return:    pose of target in frame 2
-    :rtype:     PoseData
+    :param point_pose_in_local: Target point in robot frame
+    :type point_pose_in_local: PoseData
+    :param robot_pose_in_global: Robot pose in global frame
+    :type robot_pose_in_global: PoseData
+    :return: Target point in global frame
+    :rtype: PoseData
     """
-    pose_2_origin = PoseData()
-    pose_2_in_1 = get_pose_target_in_reference_frame(
-        reference_pose=pose_1_in_2, target_pose=pose_2_origin
-    )
+    return _transform_pose(point_pose_in_local, robot_pose_in_global)
 
-    pose_target_in_2 = get_pose_target_in_reference_frame(
-        reference_pose=pose_2_in_1, target_pose=pose_target_in_1
-    )
+def get_relative_pose(pose_1_in_ref: PoseData, pose_2_in_ref: PoseData) -> PoseData:
+    """
+    Computes the pose of pose_2 in the coordinate frame of pose_1.
 
-    return pose_target_in_2
+    :param pose_1_in_ref: PoseData of frame 1 in reference frame
+    :param pose_2_in_ref: PoseData of frame 2 in reference frame
+    :return: PoseData of pose_2 in frame 1
+    """
+    pose_ref_in_1 = _inverse_pose(pose_1_in_ref)
+    return _transform_pose(pose_2_in_ref, pose_ref_in_1)
 
 
 def from_euler_to_quaternion(yaw: float, pitch: float, roll: float) -> np.ndarray:
@@ -275,7 +273,7 @@ def from_frame1_to_frame2_2d(
     )
 
     # Get transformd pose
-    pose_target_in_2: PoseData = from_frame1_to_frame2(pose_1_in_2, pose_target_in_1)
+    pose_target_in_2: PoseData = get_relative_pose(pose_1_in_2, pose_target_in_1)
 
     return pose_target_in_2