feat(prediction): add support of prediction interface

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

perception_eval/perception_eval/common/dataset.py

@@ -86,6 +86,7 @@ def load_all_datasets(
     evaluation_task: EvaluationTask,
     label_converter: LabelConverter,
     frame_id: str,
+    path_seconds: float = 10.0,
 ) -> List[FrameGroundTruth]:
     """
     Load tier4 datasets.
@@ -94,6 +95,8 @@ def load_all_datasets(
         does_use_pointcloud (bool): The flag of setting pointcloud
         evaluation_tasks (EvaluationTask): The evaluation task
         label_converter (LabelConverter): Label convertor
+        frame_id (str): Objects' frame ID.
+        path_seconds (float): Time length of path in seconds. Defaults to 10.0.
 
     Reference
         https://github.com/nutonomy/nuscenes-devkit/blob/master/python-sdk/nuscenes/eval/common/loaders.py
@@ -112,6 +115,7 @@ def load_all_datasets(
             evaluation_task=evaluation_task,
             label_converter=label_converter,
             frame_id=frame_id,
+            path_seconds=path_seconds,
         )
     logger.info("Finish loading dataset\n" + _get_str_objects_number_info(label_converter))
     return all_datasets
@@ -123,6 +127,7 @@ def _load_dataset(
     evaluation_task: EvaluationTask,
     label_converter: LabelConverter,
     frame_id: str,
+    path_seconds: float = 10.0,
 ) -> List[FrameGroundTruth]:
     """
     Load one tier4 dataset.
@@ -131,6 +136,8 @@ def _load_dataset(
         does_use_pointcloud (bool): The flag of setting pointcloud
         evaluation_tasks (EvaluationTask): The evaluation task
         label_converter (LabelConverter): Label convertor
+        frame_id (str): Objects' frame ID.
+        path_seconds (float): Time length of path in seconds. Defaults to 10.0.
 
     Reference
         https://github.com/nutonomy/nuscenes-devkit/blob/master/python-sdk/nuscenes/eval/common/loaders.py
@@ -164,6 +171,7 @@ def _load_dataset(
             label_converter=label_converter,
             frame_id=frame_id,
             frame_name=str(n),
+            path_seconds=path_seconds,
         )
         dataset.append(frame)
     return dataset
@@ -229,6 +237,7 @@ def _sample_to_frame(
     label_converter: LabelConverter,
     frame_id: str,
     frame_name: str,
+    path_seconds: float,
 ) -> FrameGroundTruth:
     """[summary]
     Convert Nuscenes sample to FrameGroundTruth
@@ -240,7 +249,9 @@ def _sample_to_frame(
         does_use_pointcloud (bool): The flag of setting pointcloud
         evaluation_tasks (EvaluationTask): The evaluation task
         label_converter (LabelConverter): Label convertor
+        frame_id (str): Objects' frame ID.
         frame_name (str): Name of frame, number of frame is used.
+        path_seconds (float): Time length of path in seconds.
 
     Raises:
         NotImplementedError:
@@ -293,6 +304,7 @@ def _sample_to_frame(
             label_converter=label_converter,
             instance_token=instance_token_,
             sample_token=sample_token,
+            path_seconds=path_seconds,
             visibility=visibility,
         )
         objects_.append(object_)
@@ -318,8 +330,8 @@ def _convert_nuscenes_box_to_dynamic_object(
     label_converter: LabelConverter,
     instance_token: str,
     sample_token: str,
-    visibility: Optional[Visibility] = None,
-    seconds: float = 3.0,
+    path_seconds: float,
+    visibility: Visibility,
 ) -> DynamicObject:
     """[summary]
     Convert nuscenes object bounding box to dynamic object
@@ -333,8 +345,8 @@ def _convert_nuscenes_box_to_dynamic_object(
         label_converter (LabelConverter): LabelConverter
         instance_token (str): Instance token
         sample_token (str): Sample token, used to get past/future record
+        path_seconds (float): Seconds to be referenced past/future record.
         visibility (Optional[Visibility]): Visibility status. Defaults to None.
-        seconds (float): Seconds to be referenced past/future record
 
     Returns:
         DynamicObject: Converted dynamic object class
@@ -354,6 +366,7 @@ def _convert_nuscenes_box_to_dynamic_object(
         nusc.box_velocity(sample_annotation_["token"]).tolist()
     )
 
+    # TODO: refactoring following codes
     if evaluation_task == EvaluationTask.TRACKING:
         (
             tracked_positions,
@@ -366,7 +379,7 @@ def _convert_nuscenes_box_to_dynamic_object(
             frame_id=frame_id,
             instance_token=instance_token,
             sample_token=sample_token,
-            seconds=seconds,
+            seconds=path_seconds,
         )
     else:
         tracked_positions = None
@@ -375,7 +388,30 @@ def _convert_nuscenes_box_to_dynamic_object(
         tracked_velocities = None
 
     if evaluation_task == EvaluationTask.PREDICTION:
-        pass
+        (
+            predicted_positions,
+            predicted_orientations,
+            predicted_sizes,
+            predicted_velocities,
+        ) = _get_prediction_data(
+            nusc=nusc,
+            helper=helper,
+            frame_id=frame_id,
+            instance_token=instance_token,
+            sample_token=sample_token,
+            seconds=path_seconds,
+        )
+        predicted_positions = [predicted_positions]
+        predicted_orientations = [predicted_orientations]
+        predicted_sizes = [predicted_sizes]
+        predicted_velocities = [predicted_velocities]
+        predicted_confidences = [1.0]
+    else:
+        predicted_positions = None
+        predicted_orientations = None
+        predicted_sizes = None
+        predicted_velocities = None
+        predicted_confidences = None
 
     dynamic_object = DynamicObject(
         unix_time=unix_time,
@@ -390,7 +426,12 @@ def _convert_nuscenes_box_to_dynamic_object(
         tracked_positions=tracked_positions,
         tracked_orientations=tracked_orientations,
         tracked_sizes=tracked_sizes,
-        tracked_twists=tracked_velocities,
+        tracked_velocities=tracked_velocities,
+        predicted_positions=predicted_positions,
+        predicted_orientations=predicted_orientations,
+        predicted_sizes=predicted_sizes,
+        predicted_velocities=predicted_velocities,
+        predicted_confidences=predicted_confidences,
         visibility=visibility,
     )
     return dynamic_object
@@ -506,24 +547,46 @@ def _get_prediction_data(
     frame_id: str,
     instance_token: str,
     sample_token: str,
-    seconds: str,
+    seconds: float,
 ):
     """Get prediction data with PredictHelper.get_future_for_agent()
-
     Args:
         nusc (NuScenes): NuScenes instance.
         helper (PredictHelper): PredictHelper instance.
         instance_token (str): The unique token to access to instance.
         sample_token (str): The unique token to access to sample.
         seconds (float): Seconds to be referenced.[s]
-
     Returns:
         future_positions (List[Tuple[float, float, float]])
         future_orientations (List[Tuple[float, float, float]])
         future_sizes (List[Tuple[float, float, float]])
         future_velocities (List[Tuple[float, float, float]])
     """
-    pass
+    if frame_id == "base_link":
+        in_agent_frame: bool = True
+    elif frame_id == "map":
+        in_agent_frame: bool = False
+    else:
+        raise ValueError(f"Unexpected frame_id: {frame_id}")
+
+    future_records_: List[Dict[str, Any]] = helper.get_future_for_agent(
+        instance_token=instance_token,
+        sample_token=sample_token,
+        seconds=seconds,
+        in_agent_frame=in_agent_frame,
+        just_xy=False,
+    )
+    future_positions: List[Tuple[float, float, float]] = []
+    future_orientations: List[Quaternion] = []
+    future_sizes: List[Tuple[float, float, float]] = []
+    future_velocities: List[Tuple[float, float, float]] = []
+    for record_ in future_records_:
+        future_positions.append(tuple(record_["translation"]))
+        future_orientations.append(Quaternion(record_["rotation"]))
+        future_sizes.append(record_["size"])
+        future_velocities.append(nusc.box_velocity(record_["token"]))
+
+    return future_positions, future_orientations, future_sizes, future_velocities
 
 
 def get_now_frame(

perception_eval/perception_eval/common/object.py

@@ -58,6 +58,17 @@ def __init__(
         self.velocity: Tuple[float, float, float] = velocity
 
 
+class ObjectPath:
+    """"""
+
+    def __init__(self) -> None:
+        pass
+
+    @classmethod
+    def from_args(cls) -> ObjectPath:
+        return cls()
+
+
 class DynamicObject:
     """
     Dynamic object class
@@ -98,12 +109,12 @@ def __init__(
         tracked_positions: Optional[List[Tuple[float, float, float]]] = None,
         tracked_orientations: Optional[List[Quaternion]] = None,
         tracked_sizes: Optional[List[Tuple[float, float, float]]] = None,
-        tracked_twists: Optional[List[Tuple[float, float, float]]] = None,
-        predicted_positions: Optional[List[Tuple[float, float, float]]] = None,
-        predicted_orientations: Optional[List[Quaternion]] = None,
-        predicted_sizes: Optional[List[Tuple[float, float, float]]] = None,
-        predicted_twists: Optional[List[Tuple[float, float, float]]] = None,
-        predicted_confidence: Optional[float] = None,
+        tracked_velocities: Optional[List[Tuple[float, float, float]]] = None,
+        predicted_positions: Optional[List[List[Tuple[float, float, float]]]] = None,
+        predicted_orientations: Optional[List[List[Quaternion]]] = None,
+        predicted_sizes: Optional[List[List[Tuple[float, float, float]]]] = None,
+        predicted_velocities: Optional[List[List[Tuple[float, float, float]]]] = None,
+        predicted_confidences: Optional[List[float]] = None,
         visibility: Optional[Visibility] = None,
     ) -> None:
         """[summary]
@@ -135,7 +146,7 @@ def __init__(
                     The list of bounding box size for predicted object. Defaults to None.
             predicted_twists (Optional[List[Tuple[float, float, float]]], optional):
                     The list of twist for predicted object. Defaults to None.
-            predicted_confidence (Optional[float], optional): Prediction score. Defaults to None.
+            predicted_confidences (Optional[float], optional): Prediction score. Defaults to None.
             visibility (Optional[Visibility]): Visibility status. Defaults to None.
         """
 
@@ -156,20 +167,20 @@ def __init__(
 
         # tracking
         self.uuid: Optional[str] = uuid
-        self.tracked_path: Optional[List[ObjectState]] = DynamicObject._set_states(
+        self.tracked_path: Optional[List[ObjectState]] = set_object_states(
             positions=tracked_positions,
             orientations=tracked_orientations,
+            velocities=tracked_velocities,
             sizes=tracked_sizes,
-            twists=tracked_twists,
         )
 
         # prediction
-        self.predicted_confidence: Optional[float] = predicted_confidence
-        self.predicted_path: Optional[List[ObjectState]] = DynamicObject._set_states(
+        self.predicted_paths: Optional[List[ObjectPath]] = set_object_paths(
             positions=predicted_positions,
             orientations=predicted_orientations,
+            velocities=predicted_velocities,
             sizes=predicted_sizes,
-            twists=predicted_twists,
+            confidences=predicted_confidences,
         )
 
         self.visibility: Optional[Visibility] = visibility
@@ -507,3 +518,82 @@ 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,
+    velocities: Optional[List[Tuple[float, float, float]]] = None,
+    sizes: 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,
+    velocities: Optional[List[Tuple[float, float, float]]] = None,
+    sizes: 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,
+    velocities: Optional[List[List[Tuple[float, float, float]]]] = None,
+    sizes: 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

perception_eval/perception_eval/evaluation/metrics/metrics_score_config.py

@@ -47,9 +47,7 @@ def __init__(self, evaluation_task: EvaluationTask, **cfg) -> None:
         """
         self.detection_config: Optional[DetectionMetricsConfig] = None
         self.tracking_config: Optional[TrackingMetricsConfig] = None
-
-        # NOTE: prediction_config is under construction
-        self.prediction_config = None
+        self.prediction_config: Optional[PredictionMetricsConfig] = None
 
         self.evaluation_task: EvaluationTask = evaluation_task
         self.target_labels: List[AutowareLabel] = cfg["target_labels"]
@@ -64,9 +62,7 @@ def __init__(self, evaluation_task: EvaluationTask, **cfg) -> None:
             self.detection_config = DetectionMetricsConfig(**cfg)
         elif self.evaluation_task == EvaluationTask.PREDICTION:
             self._check_parameters(PredictionMetricsConfig, cfg)
-            raise NotImplementedError("Prediction config is under construction")
-            # TODO
-            # self.evaluation_tasks.append(task)
+            self.prediction_config = PredictionMetricsConfig(**cfg)
         else:
             raise KeyError(f"Unsupported perception evaluation task: {self.evaluation_task}")