fix: resolve wrong transformation

Signed-off-by: ktro2828 <[email protected]>

perception_eval/perception_eval/common/dataset_utils.py

@@ -29,6 +29,9 @@
 from nuscenes.nuscenes import NuScenes
 from nuscenes.prediction.helper import PredictHelper
 from nuscenes.utils.data_classes import Box
+from PIL import Image
+from pyquaternion.quaternion import Quaternion
+
 from perception_eval.common.evaluation_task import EvaluationTask
 from perception_eval.common.label import Label
 from perception_eval.common.label import LabelConverter
@@ -41,8 +44,6 @@
 from perception_eval.common.shape import Shape
 from perception_eval.common.shape import ShapeType
 from perception_eval.common.transform import HomogeneousMatrix
-from PIL import Image
-from pyquaternion.quaternion import Quaternion
 
 from . import dataset
 
@@ -268,21 +269,27 @@ def _get_transforms(nusc: NuScenes, sample_data_token: str) -> List[HomogeneousM
     ego2map = HomogeneousMatrix(ego_position, ego_rotation, src=FrameID.BASE_LINK, dst=FrameID.MAP)
 
     matrices = [ego2map]
+    tlr_avg_pos: List[NDArray] = []
+    tlr_avg_quat: List[Quaternion] = []
     for cs_record in nusc.calibrated_sensor:
         sensor_position = cs_record["translation"]
         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))
-        # TODO: A transform BASE_LINK->TRAFFIC_LIGHT would be overwritten if the dataset contains multiple TLR cameras.
+        sensor2ego = HomogeneousMatrix(sensor_position, sensor_rotation, src=sensor_frame_id, dst=FrameID.BASE_LINK)
+        sensor2map = ego2map.dot(sensor2ego)
+        matrices.extend((sensor2ego, sensor2map))
         if "CAM_TRAFFIC_LIGHT" in sensor_frame_id.value.upper():
-            ego2tlr = HomogeneousMatrix(
-                sensor_position, sensor_rotation, src=FrameID.BASE_LINK, dst=FrameID.TRAFFIC_LIGHT
-            )
-            tlr2map = ego2tlr.inv().dot(ego2map)
-            matrices.extend((ego2tlr, tlr2map))
+            tlr_avg_pos.append(sensor_position)
+            tlr_avg_quat.append(sensor_rotation)
+
+    # NOTE: Average positions and rotations are used for matrices of cameras related to TLR.
+    if len(tlr_avg_pos) > 0 and len(tlr_avg_quat) > 0:
+        tlr_cam_pos: NDArray = np.mean(tlr_avg_pos, axis=0)
+        tlr_cam_rot: Quaternion = sum(tlr_avg_quat) / sum(tlr_avg_quat).norm
+        tlr2ego = HomogeneousMatrix(tlr_cam_pos, tlr_cam_rot, src=FrameID.TRAFFIC_LIGHT, dst=FrameID.BASE_LINK)
+        tlr2map = ego2map.dot(tlr2ego)
+        matrices.extend((tlr2ego, tlr2map))
 
     return matrices
 

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.

perception_eval/perception_eval/evaluation/matching/objects_filter.py

@@ -553,6 +553,8 @@ def _is_target_object(
     else:
         position_ = bev_distance_ = None
 
+    print(f"{position_=}, {bev_distance_=}")
+
     if position_ is not None:
         if is_target and max_x_position_list is not None:
             max_x_position = (