robometrics 0.1.0__py3-none-any.whl

robometrics/__init__.py

@@ -0,0 +1,85 @@
+"""RoboMetrics: lightweight robotics metrics for Python."""
+
+from robometrics.comfort import (
+    acceleration,
+    jerk,
+    jerk_cost,
+    max_acceleration,
+    max_deceleration,
+    smoothness_score,
+)
+from robometrics.evaluator import EvaluationInputError, Evaluator
+from robometrics.io import TrajectoryIOError
+from robometrics.physics import (
+    acceleration_limits_violated,
+    curvature_limits_violated,
+    dynamic_feasibility_score,
+    jerk_limits_violated,
+    speed_profile,
+)
+from robometrics.prediction import min_ade, min_fde, miss_rate, topk_trajectory_error
+from robometrics.registry import MetricDefinition, MetricRegistry, UnknownMetricError, registry
+from robometrics.results import EvaluationResult, MetricResult
+from robometrics.safety import (
+    collision_rate,
+    lane_departure_rate,
+    min_distance_to_actors,
+    time_to_collision,
+)
+from robometrics.schemas import AgentState, Trajectory
+from robometrics.trajectory import (
+    average_displacement_error,
+    curvature,
+    final_displacement_error,
+    hausdorff_distance,
+    lateral_error,
+    longitudinal_error,
+    path_length,
+)
+
+ade = average_displacement_error
+fde = final_displacement_error
+
+__all__ = [
+    "AgentState",
+    "EvaluationInputError",
+    "EvaluationResult",
+    "Evaluator",
+    "MetricDefinition",
+    "MetricRegistry",
+    "MetricResult",
+    "Trajectory",
+    "TrajectoryIOError",
+    "UnknownMetricError",
+    "acceleration",
+    "acceleration_limits_violated",
+    "ade",
+    "average_displacement_error",
+    "collision_rate",
+    "curvature",
+    "curvature_limits_violated",
+    "dynamic_feasibility_score",
+    "fde",
+    "final_displacement_error",
+    "hausdorff_distance",
+    "jerk",
+    "jerk_cost",
+    "jerk_limits_violated",
+    "lane_departure_rate",
+    "lateral_error",
+    "longitudinal_error",
+    "max_acceleration",
+    "max_deceleration",
+    "min_ade",
+    "min_distance_to_actors",
+    "min_fde",
+    "miss_rate",
+    "path_length",
+    "registry",
+    "smoothness_score",
+    "speed_profile",
+    "time_to_collision",
+    "topk_trajectory_error",
+]
+
+__version__ = "0.1.0"

robometrics/comfort.py

@@ -0,0 +1,72 @@
+"""Planning and control smoothness metrics."""
+
+from __future__ import annotations
+
+from typing import Literal
+
+import numpy as np
+from numpy.typing import ArrayLike
+
+from robometrics.geometry import FloatArray, as_trajectory, validate_positive, vector_norms
+
+
+def acceleration(traj: ArrayLike, dt: float) -> FloatArray:
+    """Return approximate acceleration vectors from a position trajectory."""
+    traj_arr = as_trajectory(traj, name="traj")
+    timestep = validate_positive(float(dt), name="dt")
+    if traj_arr.shape[0] < 3:
+        return np.zeros_like(traj_arr, dtype=np.float64)
+    velocity = _gradient(traj_arr, timestep)
+    return _gradient(velocity, timestep)
+
+
+def jerk(traj: ArrayLike, dt: float) -> FloatArray:
+    """Return approximate jerk vectors from a position trajectory."""
+    accel = acceleration(traj, dt)
+    if accel.shape[0] < 3:
+        return np.zeros_like(accel, dtype=np.float64)
+    return _gradient(accel, validate_positive(float(dt), name="dt"))
+
+
+def jerk_cost(traj: ArrayLike, dt: float) -> float:
+    """Return mean squared jerk magnitude."""
+    jerk_values = jerk(traj, dt)
+    return float(np.mean(np.square(vector_norms(jerk_values))))
+
+
+def max_acceleration(traj: ArrayLike, dt: float) -> float:
+    """Return maximum acceleration magnitude."""
+    accel = acceleration(traj, dt)
+    return float(np.max(vector_norms(accel)))
+
+
+def max_deceleration(traj: ArrayLike, dt: float) -> float:
+    """Return maximum longitudinal deceleration magnitude."""
+    traj_arr = as_trajectory(traj, name="traj")
+    timestep = validate_positive(float(dt), name="dt")
+    if traj_arr.shape[0] < 3:
+        return 0.0
+
+    velocity = _gradient(traj_arr, timestep)
+    accel = _gradient(velocity, timestep)
+    speeds = vector_norms(velocity)
+    moving = speeds > 1e-12
+    if not np.any(moving):
+        return 0.0
+
+    velocity_unit = np.zeros_like(velocity, dtype=np.float64)
+    velocity_unit[moving] = velocity[moving] / speeds[moving, None]
+    longitudinal_accel = np.sum(accel * velocity_unit, axis=1)
+    deceleration = np.maximum(-longitudinal_accel, 0.0)
+    return float(np.max(deceleration))
+
+
+def smoothness_score(traj: ArrayLike, dt: float) -> float:
+    """Return a bounded smoothness score where 1.0 is smoother and 0.0 is worse."""
+    return float(1.0 / (1.0 + jerk_cost(traj, dt)))
+
+
+def _gradient(values: FloatArray, dt: float) -> FloatArray:
+    edge_order: Literal[1, 2] = 2 if values.shape[0] > 2 else 1
+    gradient = np.gradient(values, dt, axis=0, edge_order=edge_order)
+    return np.asarray(gradient, dtype=np.float64)

robometrics/evaluator.py

@@ -0,0 +1,294 @@
+"""Lightweight local evaluator for named metric functions."""
+
+from __future__ import annotations
+
+from collections.abc import Mapping, Sequence
+from typing import Any
+
+import numpy as np
+
+from robometrics.registry import MetricDefinition, MetricRegistry, registry
+from robometrics.results import EvaluationResult, MetricResult
+
+
+class EvaluationInputError(ValueError):
+    """Raised when an evaluation request cannot be constructed."""
+
+
+class Evaluator:
+    """Evaluate registered RoboMetrics metrics against local Python inputs."""
+
+    def __init__(self, metric_registry: MetricRegistry | None = None) -> None:
+        self.registry = metric_registry or registry
+
+    def evaluate(
+        self,
+        *,
+        prediction: Any | None = None,
+        ground_truth: Any | None = None,
+        metrics: str | Sequence[str] | None = "all",
+        categories: str | Sequence[str] | None = None,
+        thresholds: Mapping[str, float] | None = None,
+        metric_kwargs: Mapping[str, Mapping[str, Any]] | None = None,
+        **inputs: Any,
+    ) -> EvaluationResult:
+        """Run selected metrics and return an EvaluationResult.
+
+        ``prediction`` and ``ground_truth`` are mapped onto the common argument
+        names used by built-in metric functions. Additional metric-specific
+        inputs, such as ``dt`` or ``actor_trajs``, can be supplied as keyword
+        arguments.
+        """
+        _validate_common_array(prediction, name="prediction", allowed_ndims=(2, 3))
+        _validate_common_array(ground_truth, name="ground_truth", allowed_ndims=(2,))
+        input_values = _build_inputs(
+            prediction=prediction,
+            ground_truth=ground_truth,
+            inputs=inputs,
+        )
+        if not input_values:
+            raise EvaluationInputError("at least one evaluation input is required")
+
+        selected, automatic_selection = self._select_metrics(metrics=metrics, categories=categories)
+        threshold_map = self._normalize_thresholds(thresholds)
+        metric_kwargs_map = self._normalize_metric_kwargs(metric_kwargs)
+
+        results: list[MetricResult] = []
+        skipped_metrics: list[str] = []
+        for metric in selected:
+            if not metric.is_compatible(input_values):
+                if automatic_selection:
+                    skipped_metrics.append(metric.name)
+                    continue
+                results.append(
+                    _error_result(
+                        metric,
+                        "provided inputs are not compatible with this metric",
+                    )
+                )
+                continue
+
+            try:
+                result = self._run_metric(metric, input_values, metric_kwargs_map)
+            except Exception as exc:  # noqa: BLE001 - failures must be stored per metric.
+                result = _error_result(metric, str(exc), exc)
+
+            threshold = threshold_map.get(metric.name)
+            if threshold is not None and "error" not in result.metadata:
+                result.threshold = threshold
+                result.passed = bool(result.value <= threshold)
+            results.append(result)
+
+        if not results and selected:
+            raise EvaluationInputError("no compatible metrics found for the provided inputs")
+
+        return EvaluationResult(
+            results=results,
+            metadata={
+                "selected_metrics": [metric.name for metric in selected],
+                "skipped_metrics": skipped_metrics,
+                "categories": _category_list(categories),
+            },
+        )
+
+    def _select_metrics(
+        self,
+        *,
+        metrics: str | Sequence[str] | None,
+        categories: str | Sequence[str] | None,
+    ) -> tuple[list[MetricDefinition], bool]:
+        requested_categories = _category_list(categories)
+        if requested_categories:
+            known_categories = {category.lower() for category in self.registry.categories()}
+            unknown_categories = [
+                category
+                for category in requested_categories
+                if category.lower() not in known_categories
+            ]
+            if unknown_categories:
+                raise EvaluationInputError(
+                    "unknown metric categories: " + ", ".join(unknown_categories)
+                )
+
+        automatic_selection = metrics is None or (
+            isinstance(metrics, str) and _normalize_name(metrics) == "all"
+        )
+
+        if automatic_selection:
+            selected = self.registry.list_metrics()
+            if requested_categories:
+                requested = {category.lower() for category in requested_categories}
+                selected = [metric for metric in selected if metric.category.lower() in requested]
+            return selected, True
+
+        requested_names = [metrics] if isinstance(metrics, str) else list(metrics or ())
+
+        selected = []
+        seen: set[str] = set()
+        for name in requested_names:
+            metric = self.registry.get(name)
+            if metric.name not in seen:
+                selected.append(metric)
+                seen.add(metric.name)
+        return selected, False
+
+    def _normalize_thresholds(
+        self,
+        thresholds: Mapping[str, float] | None,
+    ) -> dict[str, float]:
+        normalized: dict[str, float] = {}
+        for name, threshold in (thresholds or {}).items():
+            metric = self.registry.get(name)
+            value = float(threshold)
+            if not np.isfinite(value):
+                raise EvaluationInputError(f"threshold for {name} must be finite")
+            normalized[metric.name] = value
+        return normalized
+
+    def _normalize_metric_kwargs(
+        self,
+        metric_kwargs: Mapping[str, Mapping[str, Any]] | None,
+    ) -> dict[str, dict[str, Any]]:
+        normalized: dict[str, dict[str, Any]] = {}
+        for name, kwargs in (metric_kwargs or {}).items():
+            metric = self.registry.get(name)
+            normalized[metric.name] = dict(kwargs)
+        return normalized
+
+    def _run_metric(
+        self,
+        metric: MetricDefinition,
+        inputs: Mapping[str, Any],
+        metric_kwargs: Mapping[str, Mapping[str, Any]],
+    ) -> MetricResult:
+        kwargs: dict[str, Any] = {key: inputs[key] for key in metric.required_inputs}
+        kwargs.update(metric.default_kwargs)
+        for key in metric.default_kwargs:
+            if key in inputs:
+                kwargs[key] = inputs[key]
+        kwargs.update(metric_kwargs.get(metric.name, {}))
+
+        raw_value = metric.fn(**kwargs)
+        return _result_from_raw(metric, raw_value)
+
+
+def _build_inputs(
+    *,
+    prediction: Any | None,
+    ground_truth: Any | None,
+    inputs: Mapping[str, Any],
+) -> dict[str, Any]:
+    values = {key: value for key, value in inputs.items() if value is not None}
+    if prediction is not None:
+        values["prediction"] = prediction
+        for alias in ("pred", "predictions", "traj", "trajectory", "ego_traj"):
+            values.setdefault(alias, prediction)
+    if ground_truth is not None:
+        values["ground_truth"] = ground_truth
+        for alias in ("gt", "ref", "reference"):
+            values.setdefault(alias, ground_truth)
+    return values
+
+
+def _validate_common_array(
+    value: Any | None,
+    *,
+    name: str,
+    allowed_ndims: tuple[int, ...],
+) -> None:
+    if value is None:
+        return
+    try:
+        arr = np.asarray(value, dtype=np.float64)
+    except (TypeError, ValueError) as exc:
+        raise EvaluationInputError(f"{name} must be a numeric array-like value") from exc
+
+    if arr.ndim not in allowed_ndims:
+        allowed = " or ".join(str(ndim) for ndim in allowed_ndims)
+        raise EvaluationInputError(f"{name} must have {allowed} dimensions")
+    if arr.ndim == 2 and (arr.shape[0] == 0 or arr.shape[1] not in (2, 3)):
+        raise EvaluationInputError(f"{name} must be a non-empty Nx2 or Nx3 array")
+    if arr.ndim == 3 and (arr.shape[0] == 0 or arr.shape[1] == 0 or arr.shape[2] not in (2, 3)):
+        raise EvaluationInputError(f"{name} must be a non-empty KxTx2 or KxTx3 array")
+    if not np.all(np.isfinite(arr)):
+        raise EvaluationInputError(f"{name} must contain only finite values")
+
+
+def _result_from_raw(metric: MetricDefinition, raw_value: Any) -> MetricResult:
+    if isinstance(raw_value, MetricResult):
+        metadata = dict(raw_value.metadata)
+        metadata.setdefault("category", metric.category)
+        metadata.setdefault("description", metric.description)
+        return MetricResult(
+            name=metric.name,
+            value=raw_value.value,
+            unit=raw_value.unit or metric.unit,
+            passed=raw_value.passed,
+            threshold=raw_value.threshold,
+            metadata=metadata,
+        )
+
+    value, metadata = _coerce_metric_value(raw_value)
+    metadata.setdefault("category", metric.category)
+    metadata.setdefault("description", metric.description)
+    return MetricResult(name=metric.name, value=value, unit=metric.unit, metadata=metadata)
+
+
+def _coerce_metric_value(raw_value: Any) -> tuple[float, dict[str, Any]]:
+    arr = np.asarray(raw_value, dtype=np.float64)
+    if arr.ndim == 0:
+        return float(arr), {}
+    if arr.size == 0:
+        return float("nan"), {"reduction": "empty", "raw_value": []}
+
+    if arr.ndim >= 2 and arr.shape[-1] in (2, 3):
+        reduced = np.linalg.norm(arr, axis=-1)
+        reduction = "mean_norm"
+    else:
+        reduced = arr
+        reduction = "mean"
+
+    finite_values = reduced[np.isfinite(reduced)]
+    value = float(np.mean(finite_values)) if finite_values.size else float("nan")
+    return (
+        value,
+        {
+            "reduction": reduction,
+            "shape": list(arr.shape),
+            "value_count": int(arr.size),
+            "raw_value": arr.tolist(),
+        },
+    )
+
+
+def _error_result(
+    metric: MetricDefinition,
+    message: str,
+    exc: Exception | None = None,
+) -> MetricResult:
+    metadata: dict[str, Any] = {
+        "category": metric.category,
+        "description": metric.description,
+        "error": message,
+    }
+    if exc is not None:
+        metadata["error_type"] = type(exc).__name__
+    return MetricResult(
+        name=metric.name,
+        value=float("nan"),
+        unit=metric.unit,
+        passed=False,
+        metadata=metadata,
+    )
+
+
+def _category_list(categories: str | Sequence[str] | None) -> list[str]:
+    if categories is None:
+        return []
+    if isinstance(categories, str):
+        return [categories]
+    return list(categories)
+
+
+def _normalize_name(name: str) -> str:
+    return name.strip().lower().replace("-", "_").replace(" ", "_")

robometrics/geometry.py

@@ -0,0 +1,164 @@
+"""Geometry helpers shared by RoboMetrics modules."""
+
+from __future__ import annotations
+
+from collections.abc import Iterable
+from typing import TypeAlias
+
+import numpy as np
+from numpy.typing import ArrayLike, NDArray
+
+FloatArray: TypeAlias = NDArray[np.float64]
+
+
+def as_trajectory(data: ArrayLike, *, name: str = "trajectory") -> FloatArray:
+    """Return a finite non-empty Nx2 or Nx3 trajectory array."""
+    arr = np.asarray(data, dtype=np.float64)
+    if arr.ndim != 2 or arr.shape[1] not in (2, 3):
+        raise ValueError(f"{name} must be a Nx2 or Nx3 array")
+    if arr.shape[0] == 0:
+        raise ValueError(f"{name} must contain at least one point")
+    if not np.all(np.isfinite(arr)):
+        raise ValueError(f"{name} must contain only finite values")
+    return arr
+
+
+def as_prediction_set(data: ArrayLike, *, name: str = "predictions") -> FloatArray:
+    """Return a finite non-empty KxTx2 or KxTx3 prediction array."""
+    arr = np.asarray(data, dtype=np.float64)
+    if arr.ndim != 3 or arr.shape[2] not in (2, 3):
+        raise ValueError(f"{name} must be a KxTx2 or KxTx3 array")
+    if arr.shape[0] == 0 or arr.shape[1] == 0:
+        raise ValueError(f"{name} must contain at least one trajectory and one timestep")
+    if not np.all(np.isfinite(arr)):
+        raise ValueError(f"{name} must contain only finite values")
+    return arr
+
+
+def require_same_shape(
+    left: FloatArray,
+    right: FloatArray,
+    left_name: str,
+    right_name: str,
+) -> None:
+    """Validate that two arrays have identical shape."""
+    if left.shape != right.shape:
+        raise ValueError(f"{left_name} and {right_name} must have the same shape")
+
+
+def require_same_time_and_dim(
+    predictions: FloatArray,
+    ground_truth: FloatArray,
+    predictions_name: str = "predictions",
+    ground_truth_name: str = "gt",
+) -> None:
+    """Validate that KxTxD predictions match TxD ground truth."""
+    if predictions.shape[1:] != ground_truth.shape:
+        raise ValueError(
+            f"{predictions_name} timesteps/dimensions must match {ground_truth_name}"
+        )
+
+
+def xy(data: FloatArray) -> FloatArray:
+    """Return XY columns from an Nx2/Nx3 trajectory-like array."""
+    return data[:, :2]
+
+
+def vector_norms(data: FloatArray) -> FloatArray:
+    """Return row-wise Euclidean norms."""
+    return np.asarray(np.linalg.norm(data, axis=1), dtype=np.float64)
+
+
+def pointwise_distances(left: FloatArray, right: FloatArray) -> FloatArray:
+    """Return pointwise Euclidean distances for equally shaped trajectories."""
+    require_same_shape(left, right, "left", "right")
+    return vector_norms(left - right)
+
+
+def validate_positive(value: float, *, name: str) -> float:
+    """Return value if positive, otherwise raise ValueError."""
+    if not np.isfinite(value) or value <= 0.0:
+        raise ValueError(f"{name} must be a positive finite value")
+    return float(value)
+
+
+def validate_nonnegative(value: float, *, name: str) -> float:
+    """Return value if non-negative, otherwise raise ValueError."""
+    if not np.isfinite(value) or value < 0.0:
+        raise ValueError(f"{name} must be a non-negative finite value")
+    return float(value)
+
+
+def as_actor_trajectories(actor_trajs: object) -> list[FloatArray]:
+    """Normalize actor trajectories to a list of finite Nx2/Nx3 arrays."""
+    try:
+        arr = np.asarray(actor_trajs, dtype=np.float64)
+    except (TypeError, ValueError):
+        if not isinstance(actor_trajs, Iterable):
+            raise ValueError("actor_trajs must be an actor trajectory or iterable") from None
+        return [
+            as_trajectory(actor, name=f"actor_trajs[{index}]")
+            for index, actor in enumerate(actor_trajs)
+        ]
+
+    if arr.size == 0:
+        return []
+    if arr.ndim == 2:
+        return [as_trajectory(arr, name="actor_trajs")]
+    if arr.ndim == 3:
+        return [
+            as_trajectory(arr[index], name=f"actor_trajs[{index}]")
+            for index in range(arr.shape[0])
+        ]
+    raise ValueError("actor_trajs must be Nx2/Nx3, MxNx2/MxNx3, or an iterable of trajectories")
+
+
+def point_in_polygon(point: FloatArray, polygon: FloatArray, *, tolerance: float = 1e-9) -> bool:
+    """Return True when a 2D point lies inside or on the boundary of a polygon."""
+    px = float(point[0])
+    py = float(point[1])
+    poly = xy(polygon)
+    if poly.shape[0] < 3:
+        raise ValueError("lane_boundary must contain at least three polygon vertices")
+
+    inside = False
+    count = poly.shape[0]
+    for index in range(count):
+        start = poly[index]
+        end = poly[(index + 1) % count]
+        if _point_on_segment(np.array([px, py], dtype=np.float64), start, end, tolerance=tolerance):
+            return True
+
+        y_crosses = (start[1] > py) != (end[1] > py)
+        if y_crosses:
+            x_intersection = (end[0] - start[0]) * (py - start[1]) / (end[1] - start[1]) + start[0]
+            if px < x_intersection:
+                inside = not inside
+    return inside
+
+
+def points_in_polygon(points: FloatArray, polygon: FloatArray) -> NDArray[np.bool_]:
+    """Return an inside-mask for points against a polygon."""
+    return np.asarray([point_in_polygon(point, polygon) for point in xy(points)], dtype=np.bool_)
+
+
+def _point_on_segment(
+    point: FloatArray,
+    start: FloatArray,
+    end: FloatArray,
+    *,
+    tolerance: float,
+) -> bool:
+    segment = end - start
+    point_delta = point - start
+    squared_length = float(np.dot(segment, segment))
+    if squared_length <= tolerance * tolerance:
+        return bool(np.linalg.norm(point_delta) <= tolerance)
+
+    cross = abs(float(segment[0] * point_delta[1] - segment[1] * point_delta[0]))
+    if cross > tolerance:
+        return False
+    dot = float(np.dot(point_delta, segment))
+    if dot < -tolerance:
+        return False
+    return dot <= squared_length + tolerance