feat(prediction): add prediction metrics for displacement error

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

.github/.cspell-addition.json

@@ -2,6 +2,8 @@
     "words": [
         "allclose",
         "APHs",
+        "argmax",
+        "argmin",
         "astype",
         "AWML",
         "autoware",

.vscode/settings.json

@@ -24,6 +24,8 @@
     "allclose",
     "APHs",
     "arange",
+    "argmax",
+    "argmin",
     "argsort",
     "astype",
     "AUTOWARE",

perception_eval/perception_eval/common/object.py

@@ -19,85 +19,22 @@
 from typing import List
 from typing import Optional
 from typing import Tuple
-from typing import Union
 
 import numpy as np
 from perception_eval.common.label import AutowareLabel
 from perception_eval.common.point import distance_points
 from perception_eval.common.point import distance_points_bev
+from perception_eval.common.state import ObjectPath
+from perception_eval.common.state import ObjectState
+from perception_eval.common.state import set_object_paths
+from perception_eval.common.state import set_object_states
 from perception_eval.common.status import Visibility
 from pyquaternion import Quaternion
 from shapely.geometry import Polygon
 
 logger = getLogger(__name__)
 
 
-class ObjectState:
-    """[summary]
-    Object state class
-    """
-
-    def __init__(
-        self,
-        position: Tuple[float, float, float],
-        orientation: Quaternion,
-        size: Tuple[float, float, float],
-        velocity: Tuple[float, float, float],
-    ) -> None:
-        """
-        Args:
-            position (Tuple[float, float, float]) : center_x, center_y, center_z [m]
-            orientation (Quaternion) : Quaternion class.
-                                       See reference http://kieranwynn.github.io/pyquaternion/
-            size (Tuple[float, float, float]): bounding box size of (wx, wy, wz) [m]
-            velocity (Tuple[float, float, float]): velocity of (vx, vy, vz) [m/s]
-        """
-
-        self.position: Tuple[float, float, float] = position
-        self.orientation: Quaternion = orientation
-        self.size: Tuple[float, float, float] = size
-        self.velocity: Tuple[float, float, float] = velocity
-
-
-class ObjectPath:
-    """[summary]"""
-
-    def __init__(self, states: List[ObjectState], confidence: float) -> None:
-        self.states: List[ObjectState] = states
-        self.confidence: float = confidence
-
-    def __getitem__(self, idx: int) -> Union[ObjectState, List[ObjectState]]:
-        """[summary]
-        Returns Nth state.
-
-        Args:
-            idx (int)
-
-        Returns:
-            Union[ObjectState, List[ObjectState]]
-        """
-        return self.states[idx]
-
-    def __iter__(self) -> ObjectPath:
-        self.__n = 0
-        return self
-
-    def __next__(self):
-        if self.__n < len(self):
-            self.__n += 1
-            return self[self.__n - 1]
-        raise StopIteration
-
-    def __len__(self) -> int:
-        """[summary]
-        Returns length of states.
-
-        Returns:
-            int: length of states.
-        """
-        return len(self.states)
-
-
 class DynamicObject:
     """
     Dynamic object class
@@ -344,9 +281,9 @@ def get_position_error(
         """
         if other is None:
             return None
-        err_x: float = abs(other.state.position[0] - self.state.position[0])
-        err_y: float = abs(other.state.position[1] - self.state.position[1])
-        err_z: float = abs(other.state.position[2] - self.state.position[2])
+        err_x: float = other.state.position[0] - self.state.position[0]
+        err_y: float = other.state.position[1] - self.state.position[1]
+        err_z: float = other.state.position[2] - self.state.position[2]
         return (err_x, err_y, err_z)
 
     def get_heading_error(
@@ -399,12 +336,50 @@ def get_velocity_error(
         if self.state.velocity is None or other.state.velocity is None:
             return None
 
-        err_vx: float = abs(other.state.velocity[0] - self.state.velocity[0])
-        err_vy: float = abs(other.state.velocity[1] - self.state.velocity[1])
-        err_vz: float = abs(other.state.velocity[2] - self.state.velocity[2])
+        err_vx: float = other.state.velocity[0] - self.state.velocity[0]
+        err_vy: float = other.state.velocity[1] - self.state.velocity[1]
+        err_vz: float = other.state.velocity[2] - self.state.velocity[2]
 
         return err_vx, err_vy, err_vz
 
+    def get_path_error(
+        self,
+        other: Optional[DynamicObject],
+        num_waypoints: Optional[int] = None,
+        padding: float = np.nan,
+    ) -> Optional[np.ndarray]:
+        """[summary]
+        Returns errors of path as numpy.ndarray.
+
+        Args:
+            other (Optional[DynamicObject]): DynamicObject instance.
+            num_waypoints (optional[int]): Number of waypoints. Defaults to None.
+            padding (float): Padding value. Defaults to numpy.nan.
+
+        Returns:
+            numpy.ndarray: in shape (K, T, 3)
+        """
+        if other is None:
+            return None
+
+        self_paths: List[ObjectPath] = self.predicted_paths.copy()
+        other_paths: List[ObjectPath] = other.predicted_paths.copy()
+
+        path_errors: List[List[List[float]]] = []
+        for self_path, other_path in zip(self_paths, other_paths):
+            if self_path is None or other_path is None:
+                continue
+            num_waypoints_ = (
+                num_waypoints if num_waypoints else min(len(self_path), len(other_path))
+            )
+            self_path_, other_path_ = self_path[:num_waypoints_], other_path[:num_waypoints_]
+            err: List[Tuple[float, float, float]] = [
+                self_state.get_position_error(other_state)
+                for self_state, other_state in zip(self_path_, other_path_)
+            ]
+            path_errors.append(err)
+        return np.array(path_errors)
+
     def get_area_bev(self) -> float:
         """[summary]
         Get area of object BEV.
@@ -547,82 +522,3 @@ def distance_objects_bev(object_1: DynamicObject, object_2: DynamicObject) -> fl
     Returns: float: The 2d center distance from object_1 to object_2.
     """
     return distance_points_bev(object_1.state.position, object_2.state.position)
-
-
-def set_object_states(
-    positions: Optional[List[Tuple[float, float, float]]] = None,
-    orientations: Optional[List[Quaternion]] = None,
-    sizes: Optional[List[Tuple[float, float, float]]] = None,
-    velocities: Optional[List[Tuple[float, float, float]]] = None,
-) -> Optional[ObjectPath]:
-    """[summary]
-    Set list of object states.
-    """
-    if positions is None or orientations is None:
-        return None
-
-    assert len(positions) == len(orientations), (
-        "length of positions and orientations must be same, "
-        f"but got {len(positions)} and {len(orientations)}"
-    )
-    states: List[ObjectState] = []
-    for i, (pos, orient) in enumerate(zip(positions, orientations)):
-        states.append(
-            ObjectState(
-                position=pos,
-                orientation=orient,
-                size=sizes[i] if sizes else None,
-                velocity=velocities[i] if velocities else None,
-            )
-        )
-    return states
-
-
-def set_object_path(
-    positions: Optional[List[Tuple[float, float, float]]] = None,
-    orientations: Optional[List[Quaternion]] = None,
-    sizes: Optional[List[Tuple[float, float, float]]] = None,
-    velocities: Optional[List[Tuple[float, float, float]]] = None,
-    confidence: Optional[float] = None,
-) -> Optional[ObjectPath]:
-    """[summary]
-    Set single path.
-    """
-    states: Optional[List[ObjectState]] = set_object_states(
-        positions=positions,
-        orientations=orientations,
-        velocities=velocities,
-        sizes=sizes,
-    )
-    return ObjectPath(states, confidence) if states else None
-
-
-def set_object_paths(
-    positions: Optional[List[List[Tuple[float, float, float]]]] = None,
-    orientations: Optional[List[List[Quaternion]]] = None,
-    sizes: Optional[List[List[Tuple[float, float, float]]]] = None,
-    velocities: Optional[List[List[Tuple[float, float, float]]]] = None,
-    confidences: Optional[List[float]] = None,
-) -> Optional[List[ObjectPath]]:
-    """[summary]
-    Set multiple paths.
-    """
-    if positions is None or orientations is None:
-        return None
-
-    assert len(positions) == len(orientations), (
-        "length of positions and orientations must be same, "
-        f"but got {len(positions)} and {len(orientations)}"
-    )
-    paths: List[ObjectPath] = []
-    for i, (poses, orients) in enumerate(zip(positions, orientations)):
-        paths.append(
-            set_object_path(
-                positions=poses,
-                orientations=orients,
-                velocities=velocities[i] if velocities else None,
-                sizes=sizes[i] if sizes else None,
-                confidence=confidences[i] if confidences else None,
-            )
-        )
-    return paths