scenario-characterization 0.1.0__py3-none-any.whl

characterization/features/__init__.py

@@ -0,0 +1,14 @@
+SUPPORTED_FEATURES = [
+    "random_feature",
+    "speed",
+    "acceleration",
+    "deceleration",
+    "jerk",
+    "waiting_period",
+    "waiting_interval",
+    "waiting_distance",
+    "separation",
+    "intersection",
+    "collision",
+    "mttcp",
+]

characterization/features/base_feature.py

@@ -0,0 +1,45 @@
+import re
+from abc import ABC, abstractmethod
+
+from omegaconf import DictConfig
+
+from characterization.utils.schemas import Scenario, ScenarioFeatures
+
+
+class BaseFeature(ABC):
+    def __init__(self, config: DictConfig) -> None:
+        """Initializes the BaseFeature with a configuration.
+
+        Args:
+            config (DictConfig): Configuration for the feature.
+        """
+        self.config = config
+        self.features = config.features
+        self.characterizer_type = "feature"
+
+    @property
+    def name(self) -> str:
+        """Gets the class name formatted as a lowercase string with spaces.
+
+        Returns:
+            str: The formatted class name (e.g., 'base feature').
+        """
+        # Get the class name and add a space before each capital letter (except the first)
+        return re.sub(r"(?<!^)([A-Z])", r" \1", self.__class__.__name__).lower()
+
+    @abstractmethod
+    def compute(self, scenario: Scenario) -> ScenarioFeatures:
+        """Computes features for a given scenario.
+
+        This method should be overridden by subclasses to compute actual features.
+
+        Args:
+            scenario (Scenario): Scenario data to compute features for.
+
+        Returns:
+            ScenarioFeatures: Computed features for the scenario.
+
+        Raises:
+            ValueError: If the scenario does not contain the required information.
+        """
+        pass

characterization/features/individual_features.py

@@ -0,0 +1,158 @@
+import numpy as np
+from omegaconf import DictConfig
+
+import characterization.features.individual_utils as individual
+from characterization.features.base_feature import BaseFeature
+from characterization.utils.common import get_logger
+from characterization.utils.schemas import Scenario, ScenarioFeatures
+
+logger = get_logger(__name__)
+
+
+class IndividualFeatures(BaseFeature):
+    def __init__(self, config: DictConfig) -> None:
+        """Initializes the IndividualFeatures class.
+
+        Args:
+            config (DictConfig): Configuration for the feature. Expected to contain key-value pairs
+                relevant to feature computation, such as thresholds or parameters. Must include
+                'return_criteria' (str), which determines whether to return 'critical' or 'average'
+                statistics for each feature.
+        """
+        super(IndividualFeatures, self).__init__(config)
+
+        self.return_criteria = config.get("return_criteria", "critical")
+
+    def compute(self, scenario: Scenario) -> ScenarioFeatures:
+        """Computes features for each agent in the scenario.
+
+        Args:
+            scenario (Scenario): Scenario object containing agent positions, velocities, validity masks,
+                timestamps, map conflict points, stationary speed, and other scenario-level information.
+
+        Returns:
+            ScenarioFeatures: An object containing computed features for each valid agent, including
+                speed, speed limit difference, acceleration, deceleration, jerk, waiting period,
+                waiting interval, waiting distance, and agent-to-agent closest distances.
+
+        Raises:
+            ValueError: If an unknown return criteria is provided in the configuration.
+        """
+        agent_positions = scenario.agent_positions
+        agent_velocities = scenario.agent_velocities
+        agent_valid = scenario.agent_valid
+        scenario_timestamps = scenario.timestamps
+        conflict_points = scenario.map_conflict_points
+        stationary_speed = scenario.stationary_speed
+
+        # Meta information to be included within ScenarioFeatures. For an agent to be valid it needs to have at least
+        # two valid timestamps. The indeces of such agents will be added to `valid_idxs` list.
+        scenario_valid_idxs = []
+
+        # Features to be included in ScenarioFeatures
+        scenario_speeds = []
+        scenario_speed_limit_diffs = []
+        scenario_accelerations = []
+        scenario_decelerations = []
+        scenario_jerks = []
+        scenario_waiting_periods = []
+        scenario_waiting_intervals = []
+        scenario_waiting_distances = []
+
+        # NOTE: Handling sequentially since each agent may have different valid masks which will
+        # result in trajectories of different lengths.
+        for n in range(scenario.num_agents):
+            mask = agent_valid[n].squeeze(-1)
+            if not mask.any() or mask.sum() < 2:
+                continue
+
+            velocities = agent_velocities[n][mask, :]
+            positions = agent_positions[n][mask, :]
+            timestamps = scenario_timestamps[mask]
+
+            # Compute agent features
+
+            # Speed Profile
+            speeds, speed_limit_diffs = individual.compute_speed(velocities)
+            if speeds is None or speed_limit_diffs is None:
+                continue
+
+            # Acceleration/Deceleration Profile
+            # NOTE: acc and dec are accumulated abs acceleration and deceleration profiles.
+            _, accelerations, decelerations = individual.compute_acceleration_profile(speeds, timestamps)
+            if accelerations is None or decelerations is None:
+                continue
+
+            # Jerk Profile
+            jerks = individual.compute_jerk(speeds, timestamps)
+
+            # Waiting period
+            waiting_periods, waiting_intervals, waiting_distances = individual.compute_waiting_period(
+                positions,
+                speeds,
+                timestamps,
+                conflict_points,
+                stationary_speed,
+            )
+
+            if self.return_criteria == "critical":
+                speed = speeds.max()
+                speed_limit_diff = speed_limit_diffs.max()
+                acceleration = accelerations.max()
+                deceleration = decelerations.max()
+                jerk = jerks.max()
+                waiting_period = waiting_periods.max()
+                waiting_interval = waiting_intervals.max()
+                waiting_distance = waiting_distances.min()
+
+            elif self.return_criteria == "average":
+                speed = speeds.mean()
+                speed_limit_diff = speed_limit_diffs.mean()
+                acceleration = accelerations.mean()
+                deceleration = decelerations.mean()
+                jerk = jerks.mean()
+                waiting_period = waiting_periods.mean()
+                waiting_interval = waiting_intervals.mean()
+                waiting_distance = waiting_distances.mean()
+
+            else:
+                raise ValueError(f"Unknown return criteria: {self.return_criteria}")
+
+            scenario_valid_idxs.append(n)
+            scenario_speeds.append(speed)
+            scenario_speed_limit_diffs.append(speed_limit_diff)
+            scenario_accelerations.append(acceleration)
+            scenario_decelerations.append(deceleration)
+            scenario_jerks.append(jerk)
+            scenario_waiting_periods.append(waiting_period)
+            scenario_waiting_intervals.append(waiting_interval)
+            scenario_waiting_distances.append(waiting_distance)
+
+        # NOTE: this is not really an individual feature and would be useful for interactive features.
+        agent_to_agent_closest_dists = (
+            np.linalg.norm(agent_positions[:, np.newaxis, :] - agent_positions[np.newaxis, :, :], axis=-1)
+            .min(axis=-1)
+            .astype(np.float32)
+        )
+
+        return ScenarioFeatures(
+            num_agents=scenario.num_agents,
+            scenario_id=scenario.scenario_id,
+            agent_types=scenario.agent_types,
+            valid_idxs=np.array(scenario_valid_idxs, dtype=np.int32) if scenario_valid_idxs else None,
+            speed=np.array(scenario_speeds, dtype=np.float32) if scenario_speeds else None,
+            speed_limit_diff=(
+                np.array(scenario_speed_limit_diffs, dtype=np.float32) if scenario_speed_limit_diffs else None
+            ),
+            acceleration=np.array(scenario_accelerations, dtype=np.float32) if scenario_accelerations else None,
+            deceleration=np.array(scenario_decelerations, dtype=np.float32) if scenario_decelerations else None,
+            jerk=np.array(scenario_jerks, dtype=np.float32) if scenario_jerks else None,
+            waiting_period=np.array(scenario_waiting_periods, dtype=np.float32) if scenario_waiting_periods else None,
+            waiting_interval=(
+                np.array(scenario_waiting_intervals, dtype=np.float32) if scenario_waiting_intervals else None
+            ),
+            waiting_distance=(
+                np.array(scenario_waiting_distances, dtype=np.float32) if scenario_waiting_distances else None
+            ),
+            agent_to_agent_closest_dists=agent_to_agent_closest_dists,
+        )

characterization/features/individual_utils.py

@@ -0,0 +1,196 @@
+import numpy as np
+
+from characterization.utils.common import EPS, get_logger
+
+logger = get_logger(__name__)
+
+
+def compute_speed(velocities: np.ndarray) -> tuple[np.ndarray | None, ...]:
+    """Computes the speed profile of an agent.
+
+    Args:
+        velocities (np.ndarray): The velocity vectors of the agent over time (shape: [T, D]).
+
+    Returns:
+        tuple:
+            speeds (np.ndarray or None): The speed time series (shape: [T,]), or None if NaN values are present.
+            speeds_limit_diff (np.ndarray or None): The difference between speed and speed limit (currently zeros),
+                or None if NaN values are present.
+    """
+    speeds = np.linalg.norm(velocities, axis=-1)
+    if np.isnan(speeds).any():
+        logger.warning(f"Nan value in agent speed: {speeds}")
+        return None, None
+
+    # -----------------------------------------------------------------------------------------
+    # TODO: Add speed limit difference feature. Depends on the context and lane information.
+    speeds_limit_diff = np.zeros_like(speeds, dtype=np.float32)
+    # speed_limits = np.zeros(velocities.shape[0])
+    # in_lane = np.zeros(velocities.shape[0]).astype(bool)
+
+    #     for i in range(lane_idx.shape[0]):
+    #         speed_limits[i] = speed_limits[-1]
+    #         if lane_idx[i][0] > 0 and lane_idx[i][0] < len(lane_info):
+    #             speed_limits[i] = mph_to_ms(lane_info[int(lane_idx[i][0])]['speed_limit_mph'])
+    #             in_lane[i] = True
+    #         else:
+    #             in_lane[i] = False
+    # speed_limit_diff = speed - speed_limits
+    # -----------------------------------------------------------------------------------------
+
+    return speeds, speeds_limit_diff
+
+
+def compute_acceleration_profile(speed: np.ndarray, timestamps: np.ndarray) -> tuple[np.ndarray | None, ...]:
+    """Computes the acceleration profile from the speed (m/s) and time delta.
+
+    Args:
+        speed (np.ndarray): The speed time series (m/s) (shape: [T,]).
+        timestamps (np.ndarray): The timestamps corresponding to each speed measurement (shape: [T,]).
+
+    Returns:
+        tuple:
+            acceleration_raw (np.ndarray or None): The raw acceleration time series (shape: [T,]), or None if NaN values
+                are present.
+            acceleration (np.ndarray or None): The sum of positive acceleration intervals, or None if NaN values are
+                present.
+            deceleration (np.ndarray or None): The sum of negative acceleration intervals (absolute value), or None if
+                NaN values are present.
+
+    Raises:
+        ValueError: If speed and timestamps do not have the same shape.
+    """
+
+    def get_acc_sums(acc: np.array, idx: np.array) -> tuple[np.array, np.array]:
+        diff = idx[1:] - idx[:-1]
+        diff = np.array([-1] + np.where(diff > 1)[0].tolist() + [diff.shape[0]])
+        se_idxs = [(idx[s + 1], idx[e] + 1) for s, e in zip(diff[:-1], diff[1:])]
+        sums = np.array([acc[s:e].sum() for s, e in se_idxs])
+        return sums, se_idxs
+
+    if not speed.shape == timestamps.shape:
+        raise ValueError("Speed and timestamps must have the same shape.")
+
+    acceleration_raw = np.gradient(speed, timestamps)  # m/s^2
+
+    if np.isnan(acceleration_raw).any():
+        logger.warning(f"Nan value in agent acceleration: {acceleration_raw}")
+        return None, None, None
+
+    dr_idx = np.where(acceleration_raw < 0.0)[0]
+
+    # If the agent is accelerating or maintaining acceleration
+    if dr_idx.shape[0] == 0:
+        deceleration = np.zeros(shape=(1,))
+        acceleration = acceleration_raw.copy()
+    # If the agent is decelerating
+    elif dr_idx.shape[0] == acceleration_raw.shape[0]:
+        deceleration = acceleration_raw.copy()
+        acceleration = np.zeros(shape=(1,))
+    # If both
+    else:
+        deceleration, idx_dec = get_acc_sums(acceleration_raw, dr_idx)
+
+        ar_idx = np.where(acceleration_raw >= 0.0)[0]
+        acceleration, idx_acc = get_acc_sums(acceleration_raw, ar_idx)
+
+    return acceleration_raw, acceleration, np.abs(deceleration)
+
+
+def compute_jerk(speed: np.ndarray, timestamps: np.ndarray) -> np.ndarray:
+    """Computes the jerk from the acceleration profile and time delta.
+
+    Args:
+        speed (np.ndarray): The speed time series (m/s) (shape: [T,]).
+        timestamps (np.ndarray): The timestamps corresponding to each speed measurement (shape: [T,]).
+
+    Returns:
+        np.ndarray or None: The jerk time series (m/s^3), or None if NaN values are present.
+
+    Raises:
+        ValueError: If speed and timestamps do not have the same shape.
+    """
+    if not speed.shape == timestamps.shape:
+        raise ValueError("Speed and timestamps must have the same shape.")
+
+    acceleration = np.gradient(speed, timestamps)
+    jerk = np.gradient(acceleration, timestamps)
+
+    if np.isnan(jerk).any():
+        logger.warning(f"Nan value in agent jerk: {jerk}")
+        return None
+
+    return jerk
+
+
+def compute_waiting_period(
+    position: np.ndarray,
+    speed: np.ndarray,
+    timestamps: np.ndarray,
+    conflict_points: np.ndarray | None,
+    stationary_speed: float = 0.0,
+) -> tuple[np.ndarray, ...]:
+    """Computes the waiting period for an agent based on its position and speed.
+
+    Args:
+        position (np.ndarray): The positions of the agent over time (shape: [T, 2]).
+        speed (np.ndarray): The speeds of the agent over time (shape: [T,]).
+        timestamps (np.ndarray): The timestamps corresponding to each position/speed (shape: [T,]).
+        conflict_points (np.ndarray or None): The conflict points to check against (shape: [C, 2] or None).
+        stationary_speed (float, optional): The speed threshold below which the agent is considered stationary. Defaults
+            to 0.0.
+
+    Returns:
+        tuple:
+            waiting_period (np.ndarray): The waiting interval over the distance to the closest conflict point at that
+                distance (shape: [N,]).
+            waiting_intervals (np.ndarray): The duration of each waiting interval (shape: [N,]).
+            waiting_distances (np.ndarray): The minimum distance to conflict points during each waiting interval
+                (shape: [N,]).
+    """
+    waiting_intervals = np.zeros(shape=(position.shape[0]))
+    waiting_distances = np.inf * np.ones(shape=(position.shape[0]))
+    waiting_period = np.zeros(shape=(position.shape[0]))
+    if conflict_points is None or conflict_points.shape[0] == 0:
+        return waiting_period, waiting_intervals, waiting_distances
+
+    dt = timestamps[1:] - timestamps[:-1]
+    # On a per-timestep basis, this considers an agent to be waiting if its speed is less than or
+    # equal to the predefined stationary speed.
+    is_waiting = speed <= stationary_speed
+    if sum(is_waiting) > 0:
+        # Find all the transitions between moving and being stationary
+        is_waiting = np.hstack([[False], is_waiting, [False]])
+        is_waiting = np.diff(is_waiting.astype(int))
+
+        # Get all intervals where the agent is waiting
+        starts = np.where(is_waiting == 1)[0]
+        ends = np.where(is_waiting == -1)[0]
+
+        waiting_intervals = np.array([dt[start:end].sum() for start, end in zip(starts, ends)])
+        # intervals = np.array([end - start for start, end in zip(starts, ends)])
+
+        # For every timestep, get the minimum distance to the set of conflict points
+        waiting_distances = np.linalg.norm(conflict_points[:, None] - position[starts], axis=-1).min(axis=0)
+
+        # TODO:
+        # # Get the index of the longest interval. Then, get the longest interval and the distance to
+        # # the closest conflict point at that interval
+        # idx = intervals.argmax()
+        # # breakpoint()
+        # waiting_period_interval_longest = intervals[idx]
+        # waiting_period_distance_longest = dists_cps[idx] + EPS
+
+        # # Get the index of the closest conflict point for each interval. Then get the interval for
+        # # that index and the distance to that conflict point
+        # idx = dists_cps.argmin()
+        # waiting_period_interval_closest_conflict = intervals[idx]
+        # waiting_period_distance_closest_conflict = dists_cps[idx] + EPS
+
+    # waiting_intervals = np.asarray(
+    #     [waiting_period_interval_longest, waiting_period_interval_closest_conflict])
+    # waiting_distances_to_conflict = np.asarray(
+    #     [waiting_period_distance_longest, waiting_period_distance_closest_conflict])
+
+    waiting_period = waiting_intervals / (waiting_distances + EPS)
+    return waiting_period, waiting_intervals, waiting_distances

characterization/features/interaction_features.py

@@ -0,0 +1,153 @@
+import itertools
+
+import numpy as np
+from omegaconf import DictConfig
+
+import characterization.features.interaction_utils as interaction
+from characterization.features.base_feature import BaseFeature
+from characterization.utils.common import EPS, InteractionStatus, get_logger
+from characterization.utils.schemas import Scenario, ScenarioFeatures
+
+logger = get_logger(__name__)
+
+
+class InteractionFeatures(BaseFeature):
+    def __init__(self, config: DictConfig) -> None:
+        """Initializes the InteractionFeatures class.
+
+        Args:
+            config (DictConfig): Configuration for the feature. Expected to contain key-value pairs
+                relevant to feature computation, such as thresholds or parameters. Must include
+                'return_criteria' (str), which determines whether to return 'critical' or 'average'
+                statistics for each feature.
+        """
+        super(InteractionFeatures, self).__init__(config)
+
+        self.return_criteria = config.get("return_criteria", "critical")
+        self.agent_i = interaction.InteractionAgent()
+        self.agent_j = interaction.InteractionAgent()
+
+    def compute(self, scenario: Scenario) -> ScenarioFeatures:
+        """Computes pairwise interaction features for each agent pair in the scenario.
+
+        Args:
+            scenario (Scenario): Scenario object containing agent positions, velocities, headings,
+                validity masks, timestamps, map conflict points, distances to conflict points,
+                and other scenario-level information.
+
+        Returns:
+            ScenarioFeatures: An object containing computed interaction features for each valid agent pair,
+                including separation, intersection, collision, minimum time to conflict point (mTTCP),
+                interaction status, agent pair indices, and agent pair types.
+
+        Raises:
+            ValueError: If no agent combinations are found (i.e., less than two agents in the scenario).
+        """
+        agent_combinations = list(itertools.combinations(range(scenario.num_agents), 2))
+        if agent_combinations is None:
+            raise ValueError("No agent combinations found. Ensure that the scenario has at least two agents.")
+
+        agent_types = scenario.agent_types
+        agent_masks = scenario.agent_valid
+        agent_positions = scenario.agent_positions
+        # NOTE: this is also computed as a feature in the individual features.
+        agent_velocities = np.linalg.norm(scenario.agent_velocities, axis=-1) + EPS
+        agent_headings = scenario.agent_headings.squeeze(-1)
+        conflict_points = scenario.map_conflict_points
+        dists_to_conflict_points = scenario.agent_distances_to_conflict_points
+
+        # TODO: Figure out where's best place to get these from
+        stationary_speed = scenario.stationary_speed
+        agent_to_agent_max_distance = scenario.agent_to_agent_max_distance
+        agent_to_conflict_point_max_distance = scenario.agent_to_conflict_point_max_distance
+        agent_to_agent_distance_breach = scenario.agent_to_agent_distance_breach
+
+        # Meta information to be included in ScenarioFeatures Valid interactions will be added 'agent_pair_indeces' and
+        # 'interaction_status'
+        scenario_interaction_statuses = [InteractionStatus.UNKNOWN for _ in agent_combinations]
+        scenario_agent_pair_indeces = [(i, j) for i, j in agent_combinations]
+        scenario_agents_pair_types = [(agent_types[i], agent_types[j]) for i, j in agent_combinations]
+
+        num_interactions = len(agent_combinations)
+        # Features to be included in ScenarioFeatures
+        scenario_separations = np.full(num_interactions, np.nan, dtype=np.float32)
+        scenario_intersections = np.full(num_interactions, np.nan, dtype=np.float32)
+        scenario_collisions = np.full(num_interactions, np.nan, dtype=np.float32)
+        scenario_mttcps = np.full(num_interactions, np.nan, dtype=np.float32)
+
+        # Compute distance to conflict points
+        for n, (i, j) in enumerate(agent_combinations):
+            self.agent_i.reset()
+            self.agent_j.reset()
+
+            # There should be at leat two valid timestaps for the combined agents masks
+            mask_i, mask_j = agent_masks[i], agent_masks[j]
+            mask = np.where(mask_i & mask_j)[0]
+            if not mask.sum():
+                # No valid data for this pair of agents
+                scenario_interaction_statuses[n] = InteractionStatus.MASK_NOT_VALID
+                continue
+
+            self.agent_i.position, self.agent_j.position = agent_positions[i][mask], agent_positions[j][mask]
+            self.agent_i.velocity, self.agent_j.velocity = agent_velocities[i][mask], agent_velocities[j][mask]
+            self.agent_i.heading, self.agent_j.heading = agent_headings[i][mask], agent_headings[j][mask]
+            self.agent_i.agent_type, self.agent_j.agent_type = agent_types[i], agent_types[j]
+
+            # type: ignore
+            if conflict_points is not None and dists_to_conflict_points is not None:
+                self.agent_j.dists_to_conflict = dists_to_conflict_points[i][mask]
+                self.agent_j.dists_to_conflict = dists_to_conflict_points[j][mask]
+
+            # Check if agents are within a valid distance threshold to compute interactions
+            separations = interaction.compute_separation(self.agent_i, self.agent_j)
+            if not np.any(separations <= agent_to_agent_max_distance):
+                scenario_interaction_statuses[n] = InteractionStatus.AGENT_DISTANCE_TOO_FAR
+                continue
+
+            # Check if agents are stationary
+            self.agent_i.stationary_speed = stationary_speed
+            self.agent_j.stationary_speed = stationary_speed
+            if self.agent_i.is_stationary and self.agent_i.is_stationary:
+                scenario_interaction_statuses[n] = InteractionStatus.AGENTS_STATIONARY
+                continue
+
+            # Compute interaction features
+            # separations = interaction.compute_separation(self.agent_i, self.agent_j)
+            intersections = interaction.compute_intersections(self.agent_i, self.agent_j)
+            collisions = (separations <= agent_to_agent_distance_breach) | intersections
+            intersections = intersections.astype(np.float32)
+            collisions = collisions.astype(np.float32)
+
+            # Minimum time to conflict point (mTTCP)
+            mttcps = interaction.compute_mttcp(self.agent_i, self.agent_j, agent_to_conflict_point_max_distance)
+
+            if self.return_criteria == "critical":
+                separation = separations.min()
+                intersection = intersections.sum()
+                collision = collisions.sum()
+                mttcp = mttcps.min()
+            elif self.return_criteria == "average":
+                separation = separations.mean()
+                intersection = intersections.mean()
+                collision = collisions.mean()
+                mttcp = mttcps.mean()
+
+            # Store computed features in the state dictionary
+            scenario_separations[n] = separation
+            scenario_intersections[n] = intersection
+            scenario_collisions[n] = collision
+            scenario_mttcps[n] = mttcp
+            scenario_interaction_statuses[n] = InteractionStatus.COMPUTED_OK
+
+        return ScenarioFeatures(
+            num_agents=scenario.num_agents,
+            scenario_id=scenario.scenario_id,
+            agent_types=scenario.agent_types,
+            separation=scenario_separations,
+            intersection=scenario_intersections,
+            collision=scenario_collisions,
+            mttcp=scenario_mttcps,
+            interaction_status=scenario_interaction_statuses,
+            interaction_agent_indices=scenario_agent_pair_indeces,
+            interaction_agent_types=scenario_agents_pair_types,
+        )