From ff43a9efb6bd57b90393c74e6d7d4308658e7648 Mon Sep 17 00:00:00 2001 From: rakadu1 Date: Fri, 27 Mar 2026 12:52:26 +0530 Subject: [PATCH] Upgrade Ground Truth Schema (Critical for ADAS use-case) Expanded object state to include physical (acceleration), geometric (bounding boxes), and relative ADAS metrics (range, azimuth, closing velocity) for vehicles and pedestrians. --- intel/context.md | 32 ++++++++--- src/recorder.py | 140 +++++++++++++++++++++++++++++++++++++++-------- 2 files changed, 141 insertions(+), 31 deletions(-) diff --git a/intel/context.md b/intel/context.md index 408344a..50bbf4d 100644 --- a/intel/context.md +++ b/intel/context.md @@ -135,17 +135,35 @@ Writes one frame's worth of data to disk each tick. { "frame_id": 82, "timestamp": 1234.56, - "scenario": "braking", - "brake_frame": 80, - "braked": true, "ego_pose": {"x": 12.3, "y": 4.5, "z": 0.0, "yaw": -91.2}, - "camera": "frame_000082.png", - "radar": "frame_000082.npy", - "lidar": "frame_000082.npy", - "ground_truth": [{"id": 42, "x": ..., "speed": 8.3}] + "ground_truth": [ + { + "id": 123, + "class": "vehicle", + "type": "vehicle.tesla.model3", + "transform": {"x": 10.5, "y": 2.1, "z": 0.5, "yaw": 90.0, ...}, + "velocity": {"vx": 5.0, "vy": 0.0, "vz": 0.0, "speed": 5.0}, + "acceleration": {"ax": 0.1, "ay": 0.0, "az": 0.0}, + "bounding_box": {"l": 4.5, "w": 2.0, "h": 1.5}, + "relative": { + "range": 15.2, + "azimuth": -2.5, + "closing_velocity": 1.2 + } + } + ], + "scenario": "braking", + "brake_frame": 80 } ``` +**ADAS Relative Metrics**: +- `range`: Euclidean distance (m). +- `azimuth`: Angle in ego-forward frame (degrees). +- `closing_velocity`: Rate of approach (m/s). Positive means getting closer. + +**Scope**: Now tracks both `vehicle.*` and `walker.*` (pedestrians). + **`extra_meta` pattern:** `save()` accepts `extra_meta: dict` which is merged into the record. Scenarios use `get_scenario_metadata()` to supply this — no recorder changes needed per scenario. diff --git a/src/recorder.py b/src/recorder.py index 3f40b6c..2765e5e 100644 --- a/src/recorder.py +++ b/src/recorder.py @@ -1,8 +1,11 @@ import os import json +import math +import datetime +from types import SimpleNamespace + import numpy as np import cv2 -import datetime class Recorder: @@ -68,46 +71,66 @@ class Recorder: # -------- EGO POSE -------- transform = vehicle.get_transform() - # -------- GROUND TRUTH VEHICLES -------- + # -------- GROUND TRUTH ACTORS -------- world = vehicle.get_world() - actors = world.get_actors().filter("vehicle.*") + # Track both vehicles and pedestrians + actors = list(world.get_actors().filter("vehicle.*")) + \ + list(world.get_actors().filter("walker.*")) + + gt_actors = [] - gt_vehicles = [] + ego_loc = transform.location + ego_vel = vehicle.get_velocity() for actor in actors: if actor.id == vehicle.id: continue # skip ego + # 1. Physical State t = actor.get_transform() v = actor.get_velocity() - - speed = (v.x**2 + v.y**2 + v.z**2) ** 0.5 - - gt_vehicles.append({ - "id": actor.id, - "x": t.location.x, - "y": t.location.y, - "z": t.location.z, - "vx": v.x, - "vy": v.y, - "vz": v.z, - "speed": speed + a = actor.get_acceleration() + + # 2. Geometry + bb = actor.bounding_box + extent = bb.extent # half-size + + # 3. Relative Metrics (ADAS) + rel_metrics = self._calculate_relative_metrics(vehicle, actor) + + gt_actors.append({ + "id": actor.id, + "class": self._get_actor_class(actor), + "type": actor.type_id, + "transform": { + "x": t.location.x, "y": t.location.y, "z": t.location.z, + "yaw": t.rotation.yaw, "pitch": t.rotation.pitch, "roll": t.rotation.roll + }, + "velocity": { + "vx": v.x, "vy": v.y, "vz": v.z, + "speed": math.sqrt(v.x**2 + v.y**2 + v.z**2) + }, + "acceleration": { + "ax": a.x, "ay": a.y, "az": a.z + }, + "bounding_box": { + "l": extent.x * 2, "w": extent.y * 2, "h": extent.z * 2 + }, + "relative": rel_metrics }) # -------- RECORD -------- record = { - "frame_id": self.frame_id, + "frame_id": self.frame_id, "timestamp": cam.timestamp, "ego_pose": { - "x": transform.location.x, - "y": transform.location.y, - "z": transform.location.z, + "x": transform.location.x, "y": transform.location.y, "z": transform.location.z, "yaw": transform.rotation.yaw }, "camera": cam_file, - "radar": radar_file, - "lidar": lidar_file, - "ground_truth": gt_vehicles, + "radar": radar_file, + "lidar": lidar_file, + "ground_truth": gt_actors, } # Merge scenario metadata (e.g. {"scenario": "braking", "brake_frame": 80}) @@ -119,4 +142,73 @@ class Recorder: # ---------------- CLEANUP ---------------- def close(self): - self.meta_f.close() \ No newline at end of file + self.meta_f.close() + + # ---------------- HELPERS ---------------- + def _get_actor_class(self, actor) -> str: + """Categorise actor into broad ADAS classes.""" + type_id = actor.type_id + if "walker" in type_id: + return "pedestrian" + if "vehicle" in type_id: + if "bicycle" in type_id or "motorcycle" in type_id: + return "vru" # Vulnerable Road User + return "vehicle" + return "unknown" + + def _calculate_relative_metrics(self, ego, npc) -> dict: + """ + Calculate relative range, azimuth, and closing velocity. + Uses right-handed coordinate projections where necessary. + """ + e_t = ego.get_transform() + n_t = npc.get_transform() + e_v = ego.get_velocity() + n_v = npc.get_velocity() + + # 1. Range (Euclidean distance) + rel_pos_v = n_t.location - e_t.location + rng = rel_pos_v.length() + + if rng < 0.1: # avoid div by zero + return {"range": rng, "azimuth": 0.0, "closing_velocity": 0.0} + + # 2. Azimuth (Local angle in degrees) + # Project relative position into ego's local frame + # CARLA forward is X+, Right is Y+ (left-handed) + forward = e_t.get_forward_vector() + + # Simple dot-product for angle (cosine similarity) + # We use atan2 for full 360 bearing + # rel_pos_v in ego local coordinates + rel_pos_local = self._to_local_location(e_t, n_t.location) + azimuth = math.degrees(math.atan2(rel_pos_local.y, rel_pos_local.x)) + + # 3. Closing Velocity (V_c) + # Projection of relative velocity onto the unit line-of-sight vector + # V_c = -(V_npc - V_ego) . (P_npc - P_ego) / |P_npc - P_ego| + rel_vel = n_v - e_v + unit_los = rel_pos_v / rng + closing_vel = -(rel_vel.x * unit_los.x + rel_vel.y * unit_los.y + rel_vel.z * unit_los.z) + + return { + "range": round(rng, 3), + "azimuth": round(azimuth, 3), + "closing_velocity": round(closing_vel, 3) + } + + def _to_local_location(self, transform, location): + """Helper to convert world location to transform-local location.""" + # Simple translation and rotation inverse + # CARLA locations also have raw_data but subtraction is easier + rel_loc = location - transform.location + + # Inverse rotation (Yaw only for simplicity of horizontal metrics) + yaw_rad = math.radians(transform.rotation.yaw) + c, s = math.cos(yaw_rad), math.sin(yaw_rad) + + # Rotate rel_loc vector by -yaw + lx = rel_loc.x * c + rel_loc.y * s + ly = -rel_loc.x * s + rel_loc.y * c + + return SimpleNamespace(x=lx, y=ly, z=rel_loc.z) \ No newline at end of file