simple-autonomous-car 0.1.2__py3-none-any.whl

simple_autonomous_car/costmap/inflation.py

@@ -0,0 +1,126 @@
+"""Inflation utilities for costmaps."""
+
+import numpy as np
+from scipy.ndimage import maximum_filter  # type: ignore[import-untyped]
+
+
+def compute_inflation_kernel(radius: float, resolution: float) -> np.ndarray:
+    """
+    Compute inflation kernel for obstacle inflation.
+
+    Parameters
+    ----------
+    radius : float
+        Inflation radius in meters.
+    resolution : float
+        Costmap resolution in meters per cell.
+
+    Returns
+    -------
+    np.ndarray
+        2D kernel array for inflation.
+
+    Examples
+    --------
+    >>> kernel = compute_inflation_kernel(radius=1.0, resolution=0.5)
+    >>> print(kernel.shape)
+    (5, 5)
+    """
+    kernel_size = int(2 * radius / resolution) + 1
+    kernel = np.zeros((kernel_size, kernel_size))
+
+    center = kernel_size // 2
+    for i in range(kernel_size):
+        for j in range(kernel_size):
+            dx = (i - center) * resolution
+            dy = (j - center) * resolution
+            distance = np.sqrt(dx**2 + dy**2)
+            if distance <= radius:
+                # Linear decay from 1.0 at center to 0.0 at radius
+                cost = 1.0 - (distance / radius)
+                kernel[i, j] = cost
+
+    return kernel
+
+
+def inflate_obstacles(
+    costmap: np.ndarray,
+    inflation_radius: float,
+    resolution: float,
+    method: str = "linear",
+) -> np.ndarray:
+    """
+    Inflate obstacles in a costmap.
+
+    Parameters
+    ----------
+    costmap : np.ndarray
+        2D costmap array (0.0 = free, 1.0 = occupied).
+    inflation_radius : float
+        Inflation radius in meters.
+    resolution : float
+        Costmap resolution in meters per cell.
+    method : str, default="linear"
+        Inflation method: "linear" (linear decay) or "binary" (binary expansion).
+
+    Returns
+    -------
+    np.ndarray
+        Inflated costmap.
+
+    Examples
+    --------
+    >>> costmap = np.zeros((100, 100))
+    >>> costmap[50, 50] = 1.0  # Obstacle
+    >>> inflated = inflate_obstacles(costmap, inflation_radius=2.0, resolution=0.5)
+    """
+    if method == "linear":
+        # More efficient linear inflation using distance transform
+        from scipy.ndimage import distance_transform_edt
+
+        # Create binary obstacle map
+        obstacles = costmap >= 0.5
+
+        # Compute distance to nearest obstacle
+        distances = distance_transform_edt(~obstacles) * resolution
+
+        # Apply linear decay
+        result = np.zeros_like(costmap)
+        mask = distances <= inflation_radius
+        result[mask] = 1.0 - (distances[mask] / inflation_radius)
+        result[~mask] = costmap[~mask]
+
+        # Preserve original obstacles (set to max cost)
+        result[obstacles] = 1.0
+
+        return result
+    else:  # binary
+        kernel_size = int(2 * inflation_radius / resolution) + 1
+        kernel = np.ones((kernel_size, kernel_size))
+        inflated = maximum_filter(costmap, footprint=kernel)
+        return np.asarray(inflated, dtype=costmap.dtype)
+
+
+def compute_distance_transform(costmap: np.ndarray, resolution: float) -> np.ndarray:
+    """
+    Compute distance transform of costmap.
+
+    Parameters
+    ----------
+    costmap : np.ndarray
+        2D costmap array.
+    resolution : float
+        Costmap resolution in meters per cell.
+
+    Returns
+    -------
+    np.ndarray
+        Distance transform (distance to nearest obstacle in meters).
+    """
+    from scipy.ndimage import distance_transform_edt
+
+    # Invert: obstacles become 0, free space becomes 1
+    inverted = 1.0 - costmap
+    # Compute distance transform
+    distances = distance_transform_edt(inverted) * resolution
+    return np.asarray(distances, dtype=np.float64)

simple_autonomous_car/detection/__init__.py

@@ -0,0 +1,5 @@
+"""Localization error detection modules."""
+
+from simple_autonomous_car.detection.error_detector import LocalizationErrorDetector
+
+__all__ = ["LocalizationErrorDetector"]

simple_autonomous_car/detection/error_detector.py

@@ -0,0 +1,165 @@
+"""Localization error detection between perceived and ground truth maps."""
+
+import numpy as np
+
+from simple_autonomous_car.car.car import CarState
+from simple_autonomous_car.maps.ground_truth_map import GroundTruthMap
+from simple_autonomous_car.maps.perceived_map import PerceivedMap
+
+
+class LocalizationErrorDetector:
+    """Detects errors between perceived and ground truth localization."""
+
+    def __init__(
+        self,
+        ground_truth_map: GroundTruthMap,
+        perceived_map: PerceivedMap,
+        error_threshold: float = 1.0,
+    ):
+        """
+        Initialize error detector.
+
+        Args:
+            ground_truth_map: Ground truth map
+            perceived_map: Perceived map
+            error_threshold: Distance threshold for error detection (meters)
+        """
+        self.ground_truth_map = ground_truth_map
+        self.perceived_map = perceived_map
+        self.error_threshold = error_threshold
+
+    def compute_errors(
+        self, car_state: CarState, horizon: float, fov: float = 2 * np.pi
+    ) -> dict[str, np.ndarray]:
+        """
+        Compute errors between perceived and ground truth segments.
+
+        Args:
+            car_state: Current car state
+            horizon: Maximum distance to consider
+            fov: Field of view angle in radians
+
+        Returns:
+            Dictionary with error metrics:
+            - centerline_errors: Distance errors for centerline points
+            - inner_bound_errors: Distance errors for inner boundary points
+            - outer_bound_errors: Distance errors for outer boundary points
+            - max_error: Maximum error across all segments
+            - mean_error: Mean error across all segments
+        """
+        # Update perceived state
+        self.perceived_map.update_perceived_state(car_state)
+
+        # Get ground truth segments in car frame
+        gt_centerline, gt_inner, gt_outer = self.ground_truth_map.get_visible_segments(
+            car_state.position(), car_state.heading, horizon, fov
+        )
+
+        if len(gt_centerline) == 0:
+            return {
+                "centerline_errors": np.array([]),
+                "inner_bound_errors": np.array([]),
+                "outer_bound_errors": np.array([]),
+                "max_error": 0.0,  # type: ignore[dict-item]
+                "mean_error": 0.0,  # type: ignore[dict-item]
+            }
+
+        # Transform GT to car frame
+        gt_centerline_car = np.array(
+            [car_state.transform_to_car_frame(point) for point in gt_centerline]
+        )
+        gt_inner_car = np.array([car_state.transform_to_car_frame(point) for point in gt_inner])
+        gt_outer_car = np.array([car_state.transform_to_car_frame(point) for point in gt_outer])
+
+        # Get perceived segments in car frame
+        perceived_centerline_car, perceived_inner_car, perceived_outer_car = (
+            self.perceived_map.get_perceived_segments(horizon, fov)
+        )
+
+        # Compute errors (using nearest neighbor matching)
+        centerline_errors = self._compute_point_errors(gt_centerline_car, perceived_centerline_car)
+        inner_bound_errors = self._compute_point_errors(gt_inner_car, perceived_inner_car)
+        outer_bound_errors = self._compute_point_errors(gt_outer_car, perceived_outer_car)
+
+        # Aggregate errors
+        all_errors = np.concatenate([centerline_errors, inner_bound_errors, outer_bound_errors])
+        max_error = np.max(all_errors) if len(all_errors) > 0 else 0.0
+        mean_error = np.mean(all_errors) if len(all_errors) > 0 else 0.0
+
+        return {
+            "centerline_errors": centerline_errors,
+            "inner_bound_errors": inner_bound_errors,
+            "outer_bound_errors": outer_bound_errors,
+            "max_error": max_error,  # type: ignore[dict-item]
+            "mean_error": mean_error,  # type: ignore[dict-item]
+        }
+
+    def _compute_point_errors(
+        self, ground_truth_points: np.ndarray, perceived_points: np.ndarray
+    ) -> np.ndarray:
+        """
+        Compute distance errors between ground truth and perceived points.
+
+        Args:
+            ground_truth_points: Ground truth points (N, 2)
+            perceived_points: Perceived points (M, 2)
+
+        Returns:
+            Array of errors for each ground truth point
+        """
+        if len(perceived_points) == 0:
+            return np.zeros(len(ground_truth_points))
+
+        errors = []
+        for gt_point in ground_truth_points:
+            # Find nearest perceived point
+            distances = np.linalg.norm(perceived_points - gt_point, axis=1)
+            min_distance = np.min(distances)
+            errors.append(min_distance)
+
+        return np.array(errors)
+
+    def detect_errors(
+        self, car_state: CarState, horizon: float, fov: float = 2 * np.pi
+    ) -> dict[str, bool]:
+        """
+        Detect if errors exceed threshold.
+
+        Args:
+            car_state: Current car state
+            horizon: Maximum distance to consider
+            fov: Field of view angle in radians
+
+        Returns:
+            Dictionary with detection flags:
+            - has_error: True if any error exceeds threshold
+            - centerline_has_error: True if centerline errors exceed threshold
+            - inner_bound_has_error: True if inner bound errors exceed threshold
+            - outer_bound_has_error: True if outer bound errors exceed threshold
+        """
+        errors = self.compute_errors(car_state, horizon, fov)
+
+        centerline_has_error = bool(
+            np.any(errors["centerline_errors"] > self.error_threshold)
+            if len(errors["centerline_errors"]) > 0
+            else False
+        )
+        inner_bound_has_error = bool(
+            np.any(errors["inner_bound_errors"] > self.error_threshold)
+            if len(errors["inner_bound_errors"]) > 0
+            else False
+        )
+        outer_bound_has_error = bool(
+            np.any(errors["outer_bound_errors"] > self.error_threshold)
+            if len(errors["outer_bound_errors"]) > 0
+            else False
+        )
+
+        has_error = bool(centerline_has_error or inner_bound_has_error or outer_bound_has_error)
+
+        return {
+            "has_error": has_error,
+            "centerline_has_error": centerline_has_error,
+            "inner_bound_has_error": inner_bound_has_error,
+            "outer_bound_has_error": outer_bound_has_error,
+        }

simple_autonomous_car/filters/__init__.py

@@ -0,0 +1,7 @@
+"""Filtering components for pose and object estimation."""
+
+from simple_autonomous_car.filters.base_filter import BaseFilter
+from simple_autonomous_car.filters.kalman_filter import KalmanFilter
+from simple_autonomous_car.filters.particle_filter import ParticleFilter
+
+__all__ = ["BaseFilter", "KalmanFilter", "ParticleFilter"]

simple_autonomous_car/filters/base_filter.py

@@ -0,0 +1,119 @@
+"""Base filter class for pose and object estimation."""
+
+from abc import ABC, abstractmethod
+
+import numpy as np
+
+
+class BaseFilter(ABC):
+    """
+    Base class for all filters.
+
+    Filters estimate state (position, velocity, etc.) from noisy measurements.
+    They can be used for:
+    - Ego pose estimation (car's own position/heading)
+    - Object tracking (obstacle positions/velocities)
+
+    Attributes
+    ----------
+    name : str
+        Filter name/identifier.
+    enabled : bool
+        Whether the filter is enabled.
+    """
+
+    def __init__(self, name: str = "filter", enabled: bool = True):
+        """
+        Initialize base filter.
+
+        Parameters
+        ----------
+        name : str, default="filter"
+            Filter name/identifier.
+        enabled : bool, default=True
+            Whether the filter is enabled.
+        """
+        self.name = name
+        self.enabled = enabled
+
+    @abstractmethod
+    def predict(self, dt: float, control: dict[str, float] | None = None) -> None:
+        """
+        Predict state forward in time.
+
+        Parameters
+        ----------
+        dt : float
+            Time step in seconds.
+        control : dict, optional
+            Control inputs (e.g., {"acceleration": 0.5, "steering_rate": 0.1}).
+        """
+        pass
+
+    @abstractmethod
+    def update(
+        self, measurement: np.ndarray, measurement_covariance: np.ndarray | None = None
+    ) -> None:
+        """
+        Update state estimate with measurement.
+
+        Parameters
+        ----------
+        measurement : np.ndarray
+            Measurement vector (e.g., [x, y] for position).
+        measurement_covariance : np.ndarray, optional
+            Measurement noise covariance matrix.
+        """
+        pass
+
+    @abstractmethod
+    def get_state(self) -> np.ndarray:
+        """
+        Get current state estimate.
+
+        Returns
+        -------
+        np.ndarray
+            State vector (e.g., [x, y, vx, vy] for position and velocity).
+        """
+        pass
+
+    @abstractmethod
+    def get_covariance(self) -> np.ndarray:
+        """
+        Get current state covariance (uncertainty).
+
+        Returns
+        -------
+        np.ndarray
+            Covariance matrix.
+        """
+        pass
+
+    def is_enabled(self) -> bool:
+        """Check if filter is enabled."""
+        return self.enabled
+
+    def enable(self) -> None:
+        """Enable the filter."""
+        self.enabled = True
+
+    def disable(self) -> None:
+        """Disable the filter."""
+        self.enabled = False
+
+    def reset(
+        self, initial_state: np.ndarray, initial_covariance: np.ndarray | None = None
+    ) -> None:
+        """
+        Reset filter to initial state.
+
+        Parameters
+        ----------
+        initial_state : np.ndarray
+            Initial state vector.
+        initial_covariance : np.ndarray, optional
+            Initial covariance matrix.
+        """
+        # Default implementation - subclasses should override
+        pass

simple_autonomous_car/filters/kalman_filter.py

@@ -0,0 +1,131 @@
+"""Kalman filter for state estimation."""
+
+import numpy as np
+
+from simple_autonomous_car.filters.base_filter import BaseFilter
+
+
+class KalmanFilter(BaseFilter):
+    """
+    Kalman filter for state estimation.
+
+    Estimates state (position, velocity, etc.) from noisy measurements
+    using the Kalman filter algorithm.
+
+    Parameters
+    ----------
+    initial_state : np.ndarray
+        Initial state vector (e.g., [x, y, vx, vy]).
+    initial_covariance : np.ndarray
+        Initial covariance matrix.
+    process_noise : np.ndarray
+        Process noise covariance matrix (Q).
+    measurement_noise : np.ndarray
+        Measurement noise covariance matrix (R).
+    name : str, default="kalman_filter"
+        Filter name.
+    """
+
+    def __init__(
+        self,
+        initial_state: np.ndarray,
+        initial_covariance: np.ndarray,
+        process_noise: np.ndarray,
+        measurement_noise: np.ndarray,
+        name: str = "kalman_filter",
+    ):
+        super().__init__(name=name)
+        self.state = np.asarray(initial_state, dtype=np.float64).copy()
+        self.covariance = np.asarray(initial_covariance, dtype=np.float64).copy()
+        self.process_noise = np.asarray(process_noise, dtype=np.float64)
+        self.measurement_noise = np.asarray(measurement_noise, dtype=np.float64)
+
+        # State dimension
+        self.state_dim = len(initial_state)
+        self.measurement_dim = measurement_noise.shape[0]
+
+        # State transition matrix (identity by default, can be overridden)
+        self.F = np.eye(self.state_dim)
+        # Measurement matrix (identity by default, can be overridden)
+        self.H = np.eye(self.measurement_dim, self.state_dim)
+
+    def predict(self, dt: float, control: dict[str, float] | None = None) -> None:
+        """
+        Predict state forward in time.
+
+        Parameters
+        ----------
+        dt : float
+            Time step in seconds.
+        control : dict, optional
+            Control inputs (not used in basic Kalman filter).
+        """
+        if not self.enabled:
+            return
+
+        # Update state transition matrix with dt if needed
+        # For constant velocity model: x' = x + vx*dt, y' = y + vy*dt
+        if self.state_dim >= 4:  # Has velocity components
+            f_matrix = np.eye(self.state_dim)
+            f_matrix[0, 2] = dt  # x += vx*dt
+            f_matrix[1, 3] = dt  # y += vy*dt
+            self.F = f_matrix
+
+        # Predict state: x' = F * x
+        self.state = self.F @ self.state
+
+        # Predict covariance: P' = F * P * F^T + Q
+        self.covariance = self.F @ self.covariance @ self.F.T + self.process_noise
+
+    def update(
+        self, measurement: np.ndarray, measurement_covariance: np.ndarray | None = None
+    ) -> None:
+        """
+        Update state estimate with measurement.
+
+        Parameters
+        ----------
+        measurement : np.ndarray
+            Measurement vector.
+        measurement_covariance : np.ndarray, optional
+            Measurement noise covariance (uses default if not provided).
+        """
+        if not self.enabled:
+            return
+
+        measurement = np.asarray(measurement, dtype=np.float64)
+        r_matrix = (
+            measurement_covariance if measurement_covariance is not None else self.measurement_noise
+        )
+
+        # Innovation (measurement residual)
+        y = measurement - self.H @ self.state
+
+        # Innovation covariance
+        s_matrix = self.H @ self.covariance @ self.H.T + r_matrix
+
+        # Kalman gain
+        k_gain = self.covariance @ self.H.T @ np.linalg.inv(s_matrix)
+
+        # Update state: x = x + K * y
+        self.state = self.state + k_gain @ y
+
+        # Update covariance: P = (I - K*H) * P
+        identity_matrix = np.eye(self.state_dim)
+        self.covariance = (identity_matrix - k_gain @ self.H) @ self.covariance
+
+    def get_state(self) -> np.ndarray:
+        """Get current state estimate."""
+        return np.array(self.state.copy(), dtype=np.float64)
+
+    def get_covariance(self) -> np.ndarray:
+        """Get current state covariance."""
+        return np.array(self.covariance.copy(), dtype=np.float64)
+
+    def reset(
+        self, initial_state: np.ndarray, initial_covariance: np.ndarray | None = None
+    ) -> None:
+        """Reset filter to initial state."""
+        self.state = np.asarray(initial_state, dtype=np.float64).copy()
+        if initial_covariance is not None:
+            self.covariance = np.asarray(initial_covariance, dtype=np.float64).copy()