feat: add average precision metric (#134)

* feat: add AP metric

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

* feat: add scorer for heading yaw and APH

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

* feat: specify ego2map

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

* refactor: add method to configure scorer

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

---------

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

Author: ktro2828

t4_devkit/evaluation/matching/parameter.py

@@ -87,6 +87,7 @@ class MatchingScorer(str, Enum):
     PLANE_DISTANCE = "PLANE_DISTANCE"
     IOU2D = "IOU2D"
     IOU3D = "IOU3D"
+    HEADING_YAW = "HEADING_YAW"
 
 
 class MatchingPolicy(str, Enum):

t4_devkit/evaluation/matching/scorer.py

@@ -19,6 +19,7 @@
     "PlaneDistance",
     "Iou2D",
     "Iou3D",
+    "HeadingYaw",
 ]
 
 
@@ -75,12 +76,18 @@ def is_smaller_score_better(cls) -> bool:
         pass
 
     @abstractmethod
-    def _calculate_score(self, box1: BoxLike, box2: BoxLike) -> float:
+    def _calculate_score(
+        self,
+        box1: BoxLike,
+        box2: BoxLike,
+        ego2map: HomogeneousMatrix | None = None,
+    ) -> float:
         """Calculate the matching score of the input two boxes.
 
         Args:
             box1 (BoxLike): A box.
             box2 (BoxLike): A box.
+            ego2map (HomogeneousMatrix | None, optional): Transformation matrix from map to ego frame.
 
         Returns:
             Calculated matching score.
@@ -159,7 +166,11 @@ def _calculate_score(
         return distance
 
     def _transform2ego(self, box: Box3D, ego2map: HomogeneousMatrix | None = None) -> Box3D:
-        """Transform the box to base link frame if it is not."""
+        """Transform the box to base link frame if it is not.
+
+        Todo:
+            This method should be function.
+        """
         if box.frame_id == "base_link":
             return box
 
@@ -235,3 +246,61 @@ def _calculate_score(
         intersection = compute_volume_intersection(box1, box2)
         union = box1.volume + box2.volume - intersection
         return intersection / union
+
+
+class HeadingYaw(MatchingScorerImpl):
+    def _validate(self, box1, box2):
+        super()._validate(box1, box2)
+
+        if not isinstance(box1, Box3D):
+            raise TypeError("For Iou3D, input boxes must be 3D.")
+
+    @classmethod
+    def is_smaller_score_better(cls) -> bool:
+        return True
+
+    def _calculate_score(
+        self,
+        box1: Box3D,
+        box2: Box3D,
+        ego2map: HomogeneousMatrix | None = None,
+    ) -> float:
+        box1 = self._transform2ego(box1, ego2map)
+        box2 = self._transform2ego(box2, ego2map)
+
+        return abs(box2.diff_yaw(box1))
+
+    def _transform2ego(self, box: Box3D, ego2map: HomogeneousMatrix | None = None) -> Box3D:
+        """Transform the box to base link frame if it is not.
+
+        Todo:
+            This method should be function.
+        """
+        if box.frame_id == "base_link":
+            return box
+
+        if ego2map is None:
+            raise ValueError(f"For {box.frame_id}, `ego2map` must be specified.")
+
+        matrix = HomogeneousMatrix(
+            position=box.position,
+            rotation=box.rotation,
+            src=box.frame_id,
+            dst="base_link",
+        )
+
+        tf = ego2map.inv().transform(matrix=matrix)
+
+        return Box3D(
+            unix_time=box.unix_time,
+            frame_id="base_link",
+            semantic_label=box.semantic_label,
+            position=tf.position,
+            rotation=tf.rotation,
+            shape=box.shape,
+            velocity=box.velocity,
+            num_points=box.num_points,
+            future=box.future,
+            confidence=box.confidence,
+            uuid=box.uuid,
+        )

t4_devkit/evaluation/metric/__init__.py

@@ -0,0 +1 @@
+from .ap import *  # noqa

t4_devkit/evaluation/metric/ap.py

@@ -0,0 +1,102 @@
+from __future__ import annotations
+
+from typing import TYPE_CHECKING
+
+import numpy as np
+from attrs import define, field
+
+from ..matching import CenterDistance, HeadingYaw
+
+if TYPE_CHECKING:
+    from t4_devkit.evaluation import FrameBoxMatch, MatchingScorerLike
+
+__all__ = ["Ap", "ApH"]
+
+
+class Ap:
+    num_recall_point = 101
+    min_precision = 0.1
+    min_recall = 0.1
+
+    @define
+    class ApBuffer:
+        """Buffer to compute AP."""
+
+        num_gt: int = field(init=False, default=0)
+        tp_list: list[float] = field(init=False, factory=list)
+        fp_list: list[float] = field(init=False, factory=list)
+        confidences: list[float] = field(init=False, factory=list)
+
+        def compute_ap(self) -> float:
+            """Compute average precision."""
+            if self.num_gt == 0:
+                return 0.0
+
+            sorted_idx = np.argsort(self.confidences)[::-1]
+            tp_sorted = np.array(self.tp_list)[sorted_idx]
+            fp_sorted = np.array(self.fp_list)[sorted_idx]
+
+            tps = np.cumsum(tp_sorted)
+            fps = np.cumsum(fp_sorted)
+
+            precisions = []
+            recalls = []
+            for tp, fp in zip(tps, fps, strict=True):
+                denominator = tp + fp
+                precisions.append(0.0) if denominator == 0.0 else precisions.append(
+                    tp / denominator
+                )
+                recalls.append(tp / self.num_gt)
+
+            precision_envelope = np.maximum.accumulate(precisions[::-1])[::-1]
+
+            recall_interp = np.linspace(0.0, 1.0, Ap.num_recall_point)
+
+            precision_interp = np.interp(recall_interp, recalls, precision_envelope, right=0)
+
+            first_idx = int(round(100 * Ap.min_recall))
+
+            filtered_precision = precision_interp[first_idx:] - Ap.min_precision
+            filtered_precision = np.clip(filtered_precision, 0.0, None)
+
+            return float(np.mean(filtered_precision)) / (1.0 - Ap.min_precision)
+
+    def __init__(self, threshold: float) -> None:
+        self.scorer = self._configure_scorer()
+        self.threshold = threshold
+
+    def _configure_scorer(self) -> MatchingScorerLike:
+        return CenterDistance()
+
+    def __call__(self, frames: list[FrameBoxMatch]) -> float:
+        component = self._compute_tp_fp(frames)
+        return component.compute_ap()
+
+    def _compute_tp_fp(self, frames: list[FrameBoxMatch]) -> ApBuffer:
+        buffer = self.ApBuffer()
+        for frame in frames:
+            buffer.num_gt += frame.num_gt
+            for box_match in frame.matches:
+                if box_match.estimation is None:
+                    continue
+
+                buffer.confidences.append(box_match.estimation.confidence)
+                if box_match.is_tp(
+                    scorer=self.scorer,
+                    threshold=self.threshold,
+                    ego2map=frame.ego2map,
+                ):
+                    buffer.tp_list.append(1.0)
+                    buffer.fp_list.append(0.0)
+                else:
+                    buffer.tp_list.append(0.0)
+                    buffer.fp_list.append(1.0)
+        return buffer
+
+
+class ApH(Ap):
+    def __init__(self, threshold: float) -> None:
+        super().__init__(threshold=threshold)
+
+    def _configure_scorer(self) -> HeadingYaw:
+        return HeadingYaw()