Merge remote-tracking branch 'origin/develop' into feat/regulatory-el…

…ement
tier4 · May 23, 2024 · a6f7cce · a6f7cce
2 parents 1cf9c50 + 965bb63
commit a6f7cce
Show file tree

Hide file tree

Showing 2 changed files with 47 additions and 15 deletions.
diff --git a/perception_eval/perception_eval/common/transform.py b/perception_eval/perception_eval/common/transform.py
@@ -373,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)`.
@@ -455,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
@@ -50,13 +50,44 @@ def test_homogenous_matrix_dot():
         ),
     )
     assert np.allclose(cam2map.position, np.array([3, 3, 3]))  # cam position in map coords
-    assert np.allclose(cam2map.rotation, np.eye(3))  # cam rotation matrix 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():
-    pass
+    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():