Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(perception): enable to specify region filter. #68

Merged
merged 15 commits into from
Dec 25, 2024
Original file line number Diff line number Diff line change
Expand Up @@ -26,10 +26,13 @@
from perception_eval.common.evaluation_task import EvaluationTask
from perception_eval.evaluation.matching import MatchingMode
from perception_eval.tool.utils import filter_frame_by_distance
from perception_eval.tool.utils import filter_frame_by_region

if TYPE_CHECKING:
from perception_eval.evaluation import PerceptionFrameResult

from driving_log_replayer_v2.perception import Filter


class SuccessFail(Enum):
"""Enum object represents evaluated result is success or fail."""
Expand Down Expand Up @@ -471,8 +474,9 @@ def is_error(self) -> bool:


class CriteriaFilter:
def __init__(self, distance_range: tuple[Number, Number] | None = None) -> None:
self.distance_range = distance_range
def __init__(self, filters: Filter | None = None) -> None:
self.distance_range = getattr(filters, "Distance", None)
self.region = getattr(filters, "Region", None)

def is_all_none(self) -> bool:
"""
Expand All @@ -483,7 +487,7 @@ def is_all_none(self) -> bool:
bool: True if all filter params are None.

"""
return self.distance_range is None
return self.distance_range is None and self.region is None

def filter_frame_result(self, frame: PerceptionFrameResult) -> PerceptionFrameResult:
"""
Expand All @@ -500,10 +504,17 @@ def filter_frame_result(self, frame: PerceptionFrameResult) -> PerceptionFrameRe
PerceptionFrameResult: Filtered result.

"""
if self.is_all_none() or self.distance_range is None:
if self.is_all_none():
return frame
min_distance, max_distance = self.distance_range
return filter_frame_by_distance(frame, min_distance, max_distance)
if self.distance_range is not None:
min_distance, max_distance = self.distance_range
return filter_frame_by_distance(frame, min_distance, max_distance)
if self.region is not None:
x_position: tuple[Number, Number] = self.region.x_position
y_position: tuple[Number, Number] = self.region.y_position
return filter_frame_by_region(frame, x_position, y_position)
error_msg = "only select one filter condition"
raise ValueError(error_msg)


class PerceptionCriteria:
Expand All @@ -516,7 +527,7 @@ class PerceptionCriteria:
If None, `CriteriaMethod.NUM_TP` is used. Defaults to None.
levels (str | list[str] | Number | list[Number] | CriteriaLevel | list[CriteriaLevel]): Criteria level instance or name.
If None, `CriteriaLevel.Easy` is used. Defaults to None.
distance_range (tuple[Number, Number] | None): Range of distance to filter frame result. Defaults to None.
filters (Filter | None): Filter instance. Defaults to None.

"""

Expand All @@ -526,7 +537,7 @@ def __init__( # noqa: C901
levels: (
str | list[str] | Number | list[Number] | CriteriaLevel | list[CriteriaLevel] | None
) = None,
distance_range: tuple[Number, Number] | None = None,
filters: Filter | None = None,
) -> None:
methods = [CriteriaMethod.NUM_TP] if methods is None else self.load_methods(methods)
levels = [CriteriaLevel.EASY] if levels is None else self.load_levels(levels)
Expand Down Expand Up @@ -558,7 +569,7 @@ def __init__( # noqa: C901
error_msg: str = f"Unsupported method: {method}"
raise NotImplementedError(error_msg)

self.criteria_filter = CriteriaFilter(distance_range)
self.criteria_filter = CriteriaFilter(filters)

@staticmethod
def load_methods(
Expand Down
63 changes: 61 additions & 2 deletions driving_log_replayer_v2/driving_log_replayer_v2/perception.py
Original file line number Diff line number Diff line change
Expand Up @@ -12,17 +12,23 @@
# See the License for the specific language governing permissions and
# limitations under the License.

from __future__ import annotations

from dataclasses import dataclass
import sys
from typing import Literal
from typing import TYPE_CHECKING

from perception_eval.evaluation import PerceptionFrameResult
from pydantic import BaseModel
from pydantic import field_validator
from pydantic import model_validator
from std_msgs.msg import ColorRGBA
from std_msgs.msg import Header
from visualization_msgs.msg import MarkerArray

if TYPE_CHECKING:
from perception_eval.evaluation import PerceptionFrameResult

from driving_log_replayer_v2.criteria import PerceptionCriteria
import driving_log_replayer_v2.perception_eval_conversions as eval_conversions
from driving_log_replayer_v2.perception_eval_conversions import FrameDescriptionWriter
Expand All @@ -33,8 +39,38 @@
from driving_log_replayer_v2.scenario import Scenario


class Region(BaseModel):
x_position: tuple[float, float] | None = None
y_position: tuple[float, float] | None = None

@field_validator("x_position", "y_position", mode="before")
@classmethod
def validate_region_range(cls, v: str | None) -> tuple[number, number] | None:
if v is None:
return None

err_non_specify_msg = "both min and max values must be specified."
err_range_msg = (
f"{v} is not valid distance range, expected ordering min-max with min < max."
)

s_lower, s_upper = v.split(",")

if s_upper == "" or s_lower == "":
raise ValueError(err_non_specify_msg)

lower = float(s_lower)
upper = float(s_upper)

if lower >= upper:
raise ValueError(err_range_msg)

return (lower, upper)


class Filter(BaseModel):
Distance: tuple[float, float] | None = None
Region: Region | None
# add filter condition here

@field_validator("Distance", mode="before")
Expand All @@ -56,6 +92,29 @@ def validate_distance_range(cls, v: str | None) -> tuple[number, number] | None:
raise ValueError(err_msg)
return (lower, upper)

@field_validator("Region", mode="after")
@classmethod
def validate_region_value(cls, v: Region | None) -> Region | None:
if v is None:
return None
if v.x_position is None and v.y_position is None:
return None
return v

@model_validator(mode="before")
@classmethod
def set_region_default_value(cls, v: dict) -> dict:
if "Region" not in v:
v["Region"] = None
return v

@model_validator(mode="after")
def validate_duplicate_filter(self) -> Filter:
if self.Distance is not None and self.Region is not None:
error_msg = "Distance and Region filter cannot be used at the same time."
raise ValueError(error_msg)
return self


class Criteria(BaseModel):
PassRate: number
Expand Down Expand Up @@ -106,7 +165,7 @@ def __post_init__(self) -> None:
self.criteria = PerceptionCriteria(
methods=self.condition.CriteriaMethod,
levels=self.condition.CriteriaLevel,
distance_range=self.condition.Filter.Distance,
filters=self.condition.Filter,
)

def set_frame(self, frame: PerceptionFrameResult) -> dict:
Expand Down
6 changes: 6 additions & 0 deletions sample/perception/scenario.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -16,11 +16,17 @@ Evaluation:
CriteriaLevel: hard # Level of criteria (perfect/hard/normal/easy, or custom value 0.0-100.0. For error criteria, set custom value.)
Filter:
Distance: 0.0-50.0 # [m] null [Do not filter by distance] or lower_limit-(upper_limit) [Upper limit can be omitted. If omitted value is 1.7976931348623157e+308]
Region:
x_position: null # [m] null [Do not filter by x_position] or lower_limit,upper_limit [Lower and upper limit can be omitted. If omitted value is +-1.7976931348623157e+308]
y_position: null # [m] null [Do not filter by y_position] or lower_limit,upper_limit [Lower and upper limit can be omitted. If omitted value is +-1.7976931348623157e+308]
- PassRate: 95.0 # How much (%) of the evaluation attempts are considered successful.
CriteriaMethod: num_gt_tp # Method name of criteria (num_tp/metrics_score/metrics_score_mph/label/velocity_x_error/velocity_y_error/speed_error/yaw_error)
CriteriaLevel: easy # Level of criteria (perfect/hard/normal/easy, or custom value 0.0-100.0. For error criteria, set custom value.)
Filter:
Distance: 50.0- # [m] null [Do not filter by distance] or lower_limit-(upper_limit) [Upper limit can be omitted. If omitted value is 1.7976931348623157e+308]
Region:
x_position: null # [m] null [Do not filter by x_position] or lower_limit,upper_limit [Lower and upper limit can be omitted. If omitted value is +-1.7976931348623157e+308]
y_position: null # [m] null [Do not filter by y_position] or lower_limit,upper_limit [Lower and upper limit can be omitted. If omitted value is +-1.7976931348623157e+308]
PerceptionEvaluationConfig:
evaluation_config_dict:
evaluation_task: detection # detection or tracking. Evaluate the objects specified here
Expand Down
Loading