feat(prediction): add prediction metrics for displacement error

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

perception_eval/perception_eval/common/object.py

@@ -405,6 +405,55 @@ def get_velocity_error(
 
         return err_vx, err_vy, err_vz
 
+    def get_path_error(
+        self,
+        other: Optional[DynamicObject],
+        top_k: Optional[int] = None,
+        num_path_frames: Optional[int] = None,
+    ) -> Optional[np.ndarray]:
+        """[summary]
+        Returns errors of path as numpy.ndarray.
+
+        Args:
+            other (Optional[DynamicObject]): DynamicObject instance.
+            top_k (Optional[int]): Select the top kth confidential paths. If None, calculate for all paths.
+                It must be integer bigger than 1. Defaults to None.
+
+        Returns:
+            numpy.ndarray: in shape (K, T)
+        """
+        if other is None:
+            return None
+
+        self_paths: List[ObjectPath] = self.predicted_paths.copy()
+        other_paths: List[ObjectPath] = other.predicted_paths.copy()
+
+        if top_k:
+            if top_k <= 0:
+                raise ValueError(f"top_k must be > 0, but got {top_k}")
+            self_paths.sort(key=lambda x: x.confidence, reverse=True)
+            other_paths.sort(key=lambda x: x.confidence, reverse=True)
+            self_paths = self_paths[:top_k]
+            other_paths = other_paths[:top_k]
+
+        displacements: List[List[List[float]]] = []
+        for self_path in self_paths:
+            for other_path in other_paths:
+                errors: List[List[float]] = []
+                num_path_frames_ = (
+                    num_path_frames if num_path_frames else min(len(self_path), len(other_path))
+                )
+                for self_state, other_state in zip(
+                    self_path[:num_path_frames_],
+                    other_path[:num_path_frames_],
+                ):
+                    err_x = other_state.position[0] - self_state.position[0]
+                    err_y = other_state.position[1] - self_state.position[1]
+                    err_z = other_state.position[2] - self_state.position[2]
+                    errors.append([err_x, err_y, err_z])
+                displacements.append(errors)
+        return np.array(displacements)
+
     def get_area_bev(self) -> float:
         """[summary]
         Get area of object BEV.

perception_eval/perception_eval/evaluation/metrics/metrics.py

@@ -19,6 +19,9 @@
 from perception_eval.evaluation.matching.object_matching import MatchingMode
 from perception_eval.evaluation.metrics.detection.map import Map
 from perception_eval.evaluation.metrics.metrics_score_config import MetricsScoreConfig
+from perception_eval.evaluation.metrics.prediction.prediction_metrics_score import (
+    PredictionMetricsScore,
+)
 from perception_eval.evaluation.metrics.tracking.tracking_metrics_score import TrackingMetricsScore
 from perception_eval.evaluation.result.object_result import DynamicObjectWithPerceptionResult
 
@@ -54,7 +57,7 @@ def __init__(
         self.maps: List[Map] = []
         # tracking metrics scores for each matching method
         self.tracking_scores: List[TrackingMetricsScore] = []
-        # TODO: prediction metrics scores for each matching method
+        # prediction metrics scores for each matching method
         self.prediction_scores: List = []
 
         self.__num_frame: int = len(used_frame)
@@ -86,7 +89,9 @@ def __str__(self) -> str:
         for track_score in self.tracking_scores:
             str_ += str(track_score)
 
-        # TODO: prediction
+        # prediction
+        for pred_score in self.prediction_scores:
+            str_ += str(pred_score)
 
         return str_
 
@@ -223,4 +228,41 @@ def evaluate_prediction(
         Args:
             object_results (List[DynamicObjectWithPerceptionResult]): The list of object result
         """
-        pass
+        self.__num_gt += sum(num_ground_truth.values())
+
+        for distance_threshold_ in self.prediction_config.center_distance_thresholds:
+            prediction_score_ = PredictionMetricsScore(
+                object_results_dict=object_results,
+                num_ground_truth_dict=num_ground_truth,
+                target_labels=self.prediction_config.target_labels,
+                matching_mode=MatchingMode.CENTERDISTANCE,
+                matching_threshold_list=distance_threshold_,
+            )
+            self.prediction_scores.append(prediction_score_)
+        for iou_threshold_bev_ in self.prediction_config.iou_bev_thresholds:
+            prediction_score_ = PredictionMetricsScore(
+                object_results_dict=object_results,
+                num_ground_truth_dict=num_ground_truth,
+                target_labels=self.prediction_config.target_labels,
+                matching_mode=MatchingMode.IOUBEV,
+                matching_threshold_list=iou_threshold_bev_,
+            )
+            self.prediction_scores.append(prediction_score_)
+        for iou_threshold_3d_ in self.prediction_config.iou_3d_thresholds:
+            prediction_score_ = PredictionMetricsScore(
+                object_results_dict=object_results,
+                num_ground_truth_dict=num_ground_truth,
+                target_labels=self.prediction_config.target_labels,
+                matching_mode=MatchingMode.IOU3D,
+                matching_threshold_list=iou_threshold_3d_,
+            )
+            self.prediction_scores.append(prediction_score_)
+        for plane_distance_threshold_ in self.prediction_config.plane_distance_thresholds:
+            prediction_score_ = PredictionMetricsScore(
+                object_results_dict=object_results,
+                num_ground_truth_dict=num_ground_truth,
+                target_labels=self.prediction_config.target_labels,
+                matching_mode=MatchingMode.PLANEDISTANCE,
+                matching_threshold_list=plane_distance_threshold_,
+            )
+            self.prediction_scores.append(prediction_score_)

perception_eval/perception_eval/evaluation/metrics/prediction/__init__.py

@@ -11,3 +11,8 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
+
+from .path_displacement_error import PathDisplacementError
+from .soft_ap import SoftAp
+
+__all__ = ["PathDisplacementError", "SoftAp"]

perception_eval/perception_eval/evaluation/metrics/prediction/path_displacement_error.py

@@ -0,0 +1,154 @@
+# Copyright 2022 TIER IV, Inc.
+
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+
+#     http://www.apache.org/licenses/LICENSE-2.0
+
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import List
+from typing import Optional
+
+import numpy as np
+from perception_eval.common.label import AutowareLabel
+from perception_eval.common.threshold import get_label_threshold
+from perception_eval.evaluation.matching.object_matching import MatchingMode
+from perception_eval.evaluation.result.object_result import DynamicObjectWithPerceptionResult
+
+
+class PathDisplacementError:
+    """[summary]
+    A class to calculate path displacement errors for motion prediction task.
+
+    Support metrics:
+        ADE; Average Displacement Error
+        FDE; Final Displacement Error
+        Miss Rate
+
+    Attributes:
+        self.ade (float)
+        self.fde (float)
+        self.miss_rate (float)
+        self.num_ground_truth (int)
+        self.target_labels (List[AutowareLabel])
+        self.matching_mode (MatchingMode)
+        self.matching_threshold_list (List[float])
+        self.num_path_frames (int)
+        self.top_k (Optional[int])
+        self.miss_tolerance (float)
+    """
+
+    def __init__(
+        self,
+        object_results: List[DynamicObjectWithPerceptionResult],
+        num_ground_truth: int,
+        target_labels: List[AutowareLabel],
+        matching_mode: MatchingMode,
+        matching_threshold_list: List[float],
+        num_path_frames: int = 1,
+        top_k: Optional[int] = 1,
+        miss_tolerance: float = 2.0,
+    ) -> None:
+        """[summary]
+
+        Args:
+            object_results (List[DynamicObjectWithPerceptionResult]):
+            num_ground_truth (int):
+            target_labels (List[AutowareLabel]):
+            matching_mode (MatchingMode):
+            matching_threshold_list (List[float])
+            num_path_frames (int): Number of horizontal frames. Defaults to 10[frames].
+            top_k (Optional[int]): Number of top kth confidential paths. If None, calculate all paths. Defaults to None.
+            miss_tolerance (float): Tolerance value to determine miss. Defaults to 2.0[m].
+        """
+
+        self.num_ground_truth: int = num_ground_truth
+
+        self.target_labels: List[AutowareLabel] = target_labels
+        self.matching_mode: MatchingMode = matching_mode
+        self.matching_threshold_list: List[float] = matching_threshold_list
+
+        all_object_results: List[DynamicObjectWithPerceptionResult] = []
+        if len(object_results) == 0 or not isinstance(object_results[0], list):
+            all_object_results = object_results
+        else:
+            for obj_results in object_results:
+                all_object_results += obj_results
+        self.objects_results_num: int = len(all_object_results)
+        self.num_path_frames: int = num_path_frames
+        self.top_k: Optional[int] = top_k
+        self.miss_tolerance: float = miss_tolerance
+
+        displacements: np.ndarray = self.__get_displacements(all_object_results)
+        if len(displacements) > 0:
+            self.ade: float = np.linalg.norm(displacements[:, :, :, :2], axis=-1).mean()
+            self.fde: float = displacements[:, :, -1, :2].mean()
+            self.miss_rate: float = self.__get_miss_rate(displacements)
+            self.isnan: bool = False
+        else:
+            self.ade = np.nan
+            self.fde = np.nan
+            self.miss_rate = np.nan
+            self.isnan: bool = True
+
+    def __get_displacements(
+        self,
+        object_results: List[DynamicObjectWithPerceptionResult],
+    ) -> np.ndarray:
+        """[summary]
+        Returns the displacement errors.
+
+        Args:
+            object_results (List[DynamicObjectWithPerceptionResult])
+
+        Returns:
+            numpy.ndarray: Tensor of errors, in shape (N, K, T, 3).
+                N is number of objects, K is top k confidential paths, T is length of trajectory.
+        """
+        displacements = []
+        for obj_result in object_results:
+            matching_threshold: float = get_label_threshold(
+                semantic_label=obj_result.estimated_object.semantic_label,
+                target_labels=self.target_labels,
+                threshold_list=self.matching_threshold_list,
+            )
+            is_result_correct: bool = obj_result.is_result_correct(
+                matching_mode=self.matching_mode,
+                matching_threshold=matching_threshold,
+            )
+            if not is_result_correct:
+                continue
+            estimation = obj_result.estimated_object
+            ground_truth = obj_result.ground_truth_object
+            # (K, T, 3)
+            err: np.ndarray = estimation.get_path_error(
+                ground_truth, self.top_k, self.num_path_frames
+            )
+            displacements.append(err.tolist())
+
+        # (N, K, T, 3)
+        return np.array(displacements)
+
+    def __get_miss_rate(self, displacements: np.ndarray) -> float:
+        """[summary]
+        Returns miss rate.
+
+        Args:
+            displacements (numpy.ndarray): in shape (N, K, T, 3).
+
+        Returns:
+            float: Miss rate.
+        """
+        # (N, K)
+        max_displacements: np.ndarray = np.linalg.norm(
+            displacements[:, :, :, :2],
+            axis=-1,
+        ).max(axis=-1)
+        is_miss: np.ndarray = max_displacements[max_displacements >= self.miss_tolerance]
+        return is_miss.size / max_displacements.size