fix: resolve wrong transformation (#154)

* fix: resolve wrong transformation Signed-off-by: ktro2828 <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: ktro2828 <[email protected]> Co-authored-by: ktro2828 <[email protected]>
tier4 · May 23, 2024 · 965bb63 · 965bb63
1 parent 8e69692
commit 965bb63
Show file tree

Hide file tree

Showing 3 changed files with 88 additions and 20 deletions.
diff --git a/perception_eval/perception_eval/common/dataset_utils.py b/perception_eval/perception_eval/common/dataset_utils.py
@@ -272,9 +272,9 @@ def _get_transforms(nusc: NuScenes, sample_data_token: str) -> List[HomogeneousM
         sensor_rotation = Quaternion(cs_record["rotation"])
         sensor_record = nusc.get("sensor", cs_record["sensor_token"])
         sensor_frame_id = FrameID.from_value(sensor_record["channel"])
-        ego2sensor = HomogeneousMatrix(sensor_position, sensor_rotation, src=FrameID.BASE_LINK, dst=sensor_frame_id)
-        sensor2map = ego2sensor.inv().dot(ego2map)
-        matrices.extend((ego2sensor, sensor2map))
+        sensor2ego = HomogeneousMatrix(sensor_position, sensor_rotation, src=sensor_frame_id, dst=FrameID.BASE_LINK)
+        sensor2map = ego2map.dot(sensor2ego)
+        matrices.extend((sensor2ego, sensor2map))
     return matrices
 
 

diff --git a/perception_eval/perception_eval/common/transform.py b/perception_eval/perception_eval/common/transform.py
@@ -333,18 +333,29 @@ def dot(self, other: HomogeneousMatrix) -> HomogeneousMatrix:
 
         Raises:
         -------
-            ValueError: Expecting `self.dst` and `other.src` frame id is same.
+            ValueError: Expecting `self.src` and `other.dst` frame id is same.
 
         Returns:
         --------
             HomogeneousMatrix: Result of dot product.
+
+        Examples:
+        ---------
+            >>> ego2map = HomogeneousMatrix((1.0, 1.0, 1.0), (1.0, 0.0, 0.0, 0.0), src=FrameID.BASE_LINK, dst=FrameID.MAP)
+            >>> cam2ego = HomogeneousMatrix((2.0, 2.0, 2.0), (1.0, 0.0, 0.0, 0.0), src=FrameID.CAM_FRONT, dst=FrameID.BASE_LINK)
+            >>> cam2map = ego2map.dot(cam2ego)
+            >>> cam2map.matrix
+                array([[1., 0., 0., 3.],
+                       [0., 1., 0., 3.],
+                       [0., 0., 1., 3.],
+                       [0., 0., 0., 1.]])
         """
-        if self.dst != other.src:
-            raise ValueError(f"self.dst != other.src: self.dst={self.dst}, other.src={other.src}")
+        if self.src != other.dst:
+            raise ValueError(f"self.src != other.dst: self.src={self.src}, other.dst={other.dst}")
 
         ret_mat = self.matrix.dot(other.matrix)
         position, rotation = self.__extract_position_and_rotation_from_matrix(ret_mat)
-        return HomogeneousMatrix(position, rotation, src=self.src, dst=other.dst)
+        return HomogeneousMatrix(position, rotation, src=other.src, dst=self.dst)
 
     def inv(self) -> HomogeneousMatrix:
         """Return an inverse matrix.
@@ -362,13 +373,13 @@ def __transform_matrix(self, matrix: HomogeneousMatrix) -> HomogeneousMatrix:
 
         Args:
         -----
-            matrix (HomogeneousMatrix): A `HomogeneousMatrix` instance, which `matrix.src` frame id must be same with `self.dst`.
+            matrix (HomogeneousMatrix): A `HomogeneousMatrix` instance, which `matrix.dst` frame id must be same with `self.src`.
 
         Returns:
         --------
             HomogeneousMatrix: Result of a dot product.
         """
-        return self.dot(matrix)
+        return matrix.dot(self)
 
     def __transform_position(self, position: ArrayLike) -> NDArray:
         """Transform with the specified 3D position ordering `(x, y, z)`.
@@ -444,17 +455,18 @@ def transform(self, *args, **kwargs) -> TransformArgType:
             >>> matrix.transform(position=(1.0, 0.0, 0.0), rotation=(1.0, 0.0, 0.0, 0.0))
             (array([2., 0., 0.]), Quaternion(1.0, 0.0, 0.0, 0.0))
             # specify homogeneous matrix
-            >>> other = HomogeneousMatrix((-1.0, 0.0, 0.0), (1.0, 0.0, 0.0, 0.0), src=FrameID.MAP, dst=FrameID.BASE_LINK)
-            >>> matrix.transform(other).matrix
-            array([[1., 0., 0., 0.],
-                   [0., 1., 0., 0.],
-                   [0., 0., 1., 0.],
-                   [0., 0., 0., 1.]])
-            >>> matrix.transform(matrix=other).matrix
-            array([[1., 0., 0., 0.],
-                   [0., 1., 0., 0.],
-                   [0., 0., 1., 0.],
-                   [0., 0., 0., 1.]])
+            >>> ego2map = HomogeneousMatrix((1.0, 1.0, 1.0), (1.0, 0.0, 0.0, 0.0), src=FrameID.BASE_LINK, dst=FrameID.MAP)
+            >>> cam2ego = HomogeneousMatrix((2.0, 2.0, 2.0), (1.0, 0.0, 0.0, 0.0), src=FrameID.CAM_FRONT, dst=FrameID.BASE_LINK)
+            >>> cam2map = cam2ego.transform(ego2map)
+            >>> cam2map.matrix
+                array([[1., 0., 0., 3.],
+                       [0., 1., 0., 3.],
+                       [0., 0., 1., 3.],
+                       [0., 0., 0., 1.]])
+            >>> cam2map.src
+                <FrameID.CAM_FRONT: 'cam_front'>
+            >>> cam2map.dst
+                <FrameID.MAP: 'map'>
         """
         s = len(args)
         if s == 0:

diff --git a/perception_eval/test/common/test_transform.py b/perception_eval/test/common/test_transform.py
@@ -33,6 +33,62 @@ def test_homogeneous_matrix():
     assert np.allclose(mat2.rotation_matrix, np.eye(3))
 
 
+def test_homogenous_matrix_dot():
+    ego2map = HomogeneousMatrix((1, 1, 1), (1, 0, 0, 0), src=FrameID.BASE_LINK, dst=FrameID.MAP)
+    cam2ego = HomogeneousMatrix((2, 2, 2), (1, 0, 0, 0), src=FrameID.CAM_FRONT, dst=FrameID.BASE_LINK)
+    cam2map = ego2map.dot(cam2ego)
+    assert np.allclose(
+        cam2map.matrix,
+        np.array(
+            [
+                [1, 0, 0, 3],
+                [0, 1, 0, 3],
+                [0, 0, 1, 3],
+                [0, 0, 0, 1],
+            ],
+        ),
+    )
+    assert np.allclose(cam2map.position, np.array([3, 3, 3]))  # cam position in map coords
+    assert np.allclose(cam2map.rotation_matrix, np.eye(3))  # cam rotation matrix in map coords
+    assert cam2map.src == FrameID.CAM_FRONT
+    assert cam2map.dst == FrameID.MAP
+
+
+def test_homogenous_matrix_inv():
+    matrix = np.array(
+        [
+            [0.70710678, -0.70710678, 0.0, 1.0],
+            [0.70710678, 0.70710678, 0.0, 2.0],
+            [0.0, 0.0, 1.0, 3.0],
+            [0.0, 0.0, 0.0, 1.0],
+        ]
+    )
+    ego2map = HomogeneousMatrix.from_matrix(matrix, FrameID.BASE_LINK, FrameID.MAP)
+    inv = ego2map.inv()
+    assert np.allclose(
+        inv.matrix,
+        np.array(
+            [
+                [0.70710678, 0.70710678, 0.0, -2.12132034],
+                [-0.70710678, 0.70710678, 0.0, -0.70710678],
+                [0.0, 0.0, 1.0, -3.0],
+                [0.0, 0.0, 0.0, 1.0],
+            ]
+        ),
+    )
+    assert np.allclose(inv.position, np.array([-2.12132034, -0.70710678, -3.0]))
+    assert np.allclose(
+        inv.rotation_matrix,
+        np.array(
+            [
+                [0.70710678, 0.70710678, 0.0],
+                [-0.70710678, 0.70710678, 0.0],
+                [0.0, 0.0, 1.0],
+            ]
+        ),
+    )
+
+
 def test_transform_dict():
     ego2map = HomogeneousMatrix((1, 0, 0), (1, 0, 0, 0), src=FrameID.BASE_LINK, dst=FrameID.MAP)
     transforms = TransformDict(ego2map)