From 699b05edc344f457252da43c0fc97bab1a71e769 Mon Sep 17 00:00:00 2001 From: Flagbusted Date: Fri, 19 Jun 2026 02:04:58 +0530 Subject: [PATCH 1/4] feat(trajectory_planning): add BEV-only multi-sample trajectory scorer and shifted inference schedule Signed-off-by: Flagbusted --- .../trajectory_planning/trajectory_scorer.py | 331 ++++++++++++++++++ PROPOSAL_diffusion_driving_policy.md | 178 ++++++++++ tests/test_trajectory_scorer.py | 169 +++++++++ 3 files changed, 678 insertions(+) create mode 100644 Model/model_components/trajectory_planning/trajectory_scorer.py create mode 100644 PROPOSAL_diffusion_driving_policy.md create mode 100644 tests/test_trajectory_scorer.py diff --git a/Model/model_components/trajectory_planning/trajectory_scorer.py b/Model/model_components/trajectory_planning/trajectory_scorer.py new file mode 100644 index 000000000..7199a16a5 --- /dev/null +++ b/Model/model_components/trajectory_planning/trajectory_scorer.py @@ -0,0 +1,331 @@ +"""Lightweight, training-free multi-sample trajectory scorer. + +Implements the "Phase 1" BEV-only re-ranking proposed in the diffusion / +flow-matching driving-policy discussion (see PROPOSAL_diffusion_driving_policy.md). +Given K trajectory samples drawn from a stochastic planner (e.g. +FlowMatchingPlanner sampled with different noise seeds), this module scores +each sample by: + + 1. Drivable-area compliance, read directly off the *rasterized* BEV map + image that RasterizedMapEncoder consumes as input (no new training + required — this is a deterministic geometric + colour lookup, not a + learned classifier). + 2. Kinematic comfort, i.e. how much each sample's (acceleration, + curvature) sequence violates configurable comfort bounds. + +...and returns either the single best-scoring sample per batch element, or +a softmax-weighted blend across samples (mirroring GoalFlow's "nearest" vs +"mean" trajectory-selection modes, arXiv:2503.05689). + +Deliberately excluded (tracked as Phase 2 in the proposal): + - Learned BEV semantic segmentation head (GoalFlow's `_bev_semantic_head`) + - Goal-point vocabulary + image/DAC scorer trained offline + - Classifier-free-guidance-style goal-conditioned/unconditioned fusion + +Phase 1 intentionally has zero new trainable parameters and zero new loss +terms, so it can be merged and evaluated without retraining the perception +or planner stack — it only changes *how many* samples are drawn from an +already-trained stochastic planner and *how* the best one is picked. + +Calibration note: `pixels_per_meter`, `ego_row`, and `ego_col` below MUST be +set to match whatever convention the KITScenes / L2D map renderer actually +uses to produce `map_input`. The defaults here follow the BEV geometry +discussed for this fork (120 m front / 60 m rear / 60 m each side at 0.4 m +resolution -> 450 x 300 px, issue #35) but have NOT been verified against +the renderer itself — confirm with whoever owns that code before relying +on the compliance score in any reported metric. +""" + +from dataclasses import dataclass +from typing import Optional + +import torch +import torch.nn as nn + + +@dataclass +class ScorerConfig: + # --- BEV pixel-space calibration (see calibration note above) --- + pixels_per_meter: float = 2.5 # 1 / 0.4 m + bev_h: int = 450 # rows: forward axis + bev_w: int = 300 # cols: lateral axis + ego_row: int = 300 # row index where ego (x=0) sits + # (150m*2.5=375 if symmetric; the + # 120m-front/60m-rear split used + # in issue #35 gives 60*2.5=150 from + # the *bottom*, i.e. row 450-150=300 + # from the top if rendered front-up). + ego_col: int = 150 # col index where ego (y=0) sits + # (lateral center: 300 / 2) + forward_is_negative_row: bool = True # True if increasing x (forward) + # moves to smaller row indices + # (image rendered nose-up). + + # --- drivable-area colour lookup --- + # RGB tuple(s) considered "drivable" in the rendered map_input image. + # Confirm against the actual renderer palette before use. + drivable_rgb: tuple = (255, 255, 255) + drivable_rgb_tolerance: int = 10 # per-channel L1 tolerance + + # --- kinematic comfort bounds --- + max_comfortable_accel: float = 3.0 # m/s^2 + max_comfortable_lateral_accel: float = 2.0 # m/s^2, = curvature * speed^2 + dt: float = 0.1 # seconds between model timesteps + initial_speed: float = 5.0 # m/s, matches existing decode convention + + # --- scoring weights --- + dac_weight: float = 1.0 + comfort_weight: float = 0.5 + + # --- selection mode --- + selection: str = "nearest" # "nearest" (argmax) or "mean" (softmax blend) + softmax_temperature: float = 1.0 + + +def decode_trajectory_to_xy(trajectory: torch.Tensor, num_timesteps: int, + dt: float = 0.1, + initial_speed: float = 5.0) -> torch.Tensor: + """Decode (acceleration, curvature) pairs into (x, y) waypoints. + + Mirrors the bicycle-model integration already used for offline + evaluation against Waymo ground truth. If/when a canonical decode + utility lands (tracked under the open-loop eval pipeline, issue #66), + this function should be replaced by an import from that module instead + of duplicating the integration here. + + Args: + trajectory: [..., num_timesteps * 2] — flat (accel, curvature) pairs. + num_timesteps: number of (accel, curvature) pairs encoded. + dt: seconds between timesteps. + initial_speed: assumed starting speed in m/s. + + Returns: + xy: [..., num_timesteps, 2] waypoints in ego-relative meters. + """ + *batch_shape, _ = trajectory.shape + pairs = trajectory.reshape(*batch_shape, num_timesteps, 2) + accels, curvatures = pairs[..., 0], pairs[..., 1] + + device, dtype = trajectory.device, trajectory.dtype + flat = accels.reshape(-1, num_timesteps) + flat_curv = curvatures.reshape(-1, num_timesteps) + n = flat.shape[0] + + x = torch.zeros(n, device=device, dtype=dtype) + y = torch.zeros(n, device=device, dtype=dtype) + heading = torch.zeros(n, device=device, dtype=dtype) + speed = torch.full((n,), initial_speed, device=device, dtype=dtype) + + xs, ys = [], [] + for t in range(num_timesteps): + speed = torch.clamp(speed + flat[:, t] * dt, min=0.0) + heading = heading + flat_curv[:, t] * speed * dt + x = x + torch.cos(heading) * speed * dt + y = y + torch.sin(heading) * speed * dt + xs.append(x.clone()) + ys.append(y.clone()) + + xy = torch.stack([torch.stack(xs, dim=-1), torch.stack(ys, dim=-1)], dim=-1) + return xy.reshape(*batch_shape, num_timesteps, 2) + + +def project_xy_to_bev_pixel(xy: torch.Tensor, config: ScorerConfig) -> torch.Tensor: + """Project ego-relative (x, y) meters into BEV pixel (row, col) indices. + + Args: + xy: [..., 2] ego-relative coordinates in meters (x=forward, y=left). + config: calibration parameters — see module docstring. + + Returns: + pixel: [..., 2] integer (row, col) indices, NOT clamped to + [0, bev_h) / [0, bev_w) — caller must mask out-of-bounds points + (see `drivable_area_compliance`). + """ + x, y = xy[..., 0], xy[..., 1] + row_offset = -x if config.forward_is_negative_row else x + row = config.ego_row + row_offset * config.pixels_per_meter + col = config.ego_col - y * config.pixels_per_meter # +y = left = smaller col + return torch.stack([row, col], dim=-1).round().long() + + +def drivable_area_compliance(xy: torch.Tensor, map_input: torch.Tensor, + config: ScorerConfig) -> torch.Tensor: + """Fraction of trajectory waypoints landing on a "drivable" map pixel. + + Args: + xy: [B, num_timesteps, 2] ego-relative waypoints in meters. + map_input: [B, 3, bev_h, bev_w] rasterized BEV map image, the same + tensor fed to RasterizedMapEncoder (channel order assumed RGB, + values in [0, 255] or normalized — see note below). + config: calibration parameters. + + Returns: + compliance: [B] fraction in [0, 1] of waypoints inside the + drivable-area colour band and within image bounds. + + Note: if `map_input` has already been ImageNet-normalized upstream of + this call, the colour lookup must run on a separate un-normalized copy + of the map image — wire this scorer to whichever stage in the data + pipeline still has raw pixel values. + """ + B, _, H, W = map_input.shape + T = xy.shape[1] + pixels = project_xy_to_bev_pixel(xy, config) # [B, T, 2] + + rows, cols = pixels[..., 0], pixels[..., 1] + in_bounds = (rows >= 0) & (rows < H) & (cols >= 0) & (cols < W) + + rows_c = rows.clamp(0, H - 1) + cols_c = cols.clamp(0, W - 1) + + target = torch.tensor(config.drivable_rgb, device=map_input.device, + dtype=map_input.dtype).view(1, 1, 3) + + compliant = torch.zeros(B, T, dtype=torch.bool, device=map_input.device) + for b in range(B): + sampled = map_input[b, :, rows_c[b], cols_c[b]].transpose(0, 1) # [T, 3] + diff = (sampled - target[0]).abs().sum(dim=-1) + compliant[b] = diff <= (3 * config.drivable_rgb_tolerance) + + compliant = compliant & in_bounds + return compliant.float().mean(dim=1) # [B] + + +def kinematic_comfort_score(trajectory: torch.Tensor, num_timesteps: int, + config: ScorerConfig) -> torch.Tensor: + """Penalize (acceleration, curvature) samples that exceed comfort bounds. + + Returns a score in [0, 1] where 1.0 means no bound violations at any + timestep and 0.0 means every timestep violates at least one bound. + + Args: + trajectory: [B, num_timesteps * 2] flat (accel, curvature) pairs. + num_timesteps: number of pairs encoded. + config: comfort bound parameters. + + Returns: + score: [B]. + """ + pairs = trajectory.reshape(trajectory.shape[0], num_timesteps, 2) + accels, curvatures = pairs[..., 0], pairs[..., 1] + + # Approximate speed via cumulative integration of accel (matches decode). + speed = torch.clamp( + config.initial_speed + torch.cumsum(accels * config.dt, dim=1), + min=0.0, + ) + lateral_accel = curvatures * speed.pow(2) + + accel_violation = (accels.abs() > config.max_comfortable_accel).float() + lateral_violation = ( + lateral_accel.abs() > config.max_comfortable_lateral_accel + ).float() + + violation_rate = torch.maximum(accel_violation, lateral_violation).mean(dim=1) + return 1.0 - violation_rate + + +class TrajectoryComplianceScorer(nn.Module): + """Wraps any `BasePlanner` to draw K samples and re-rank them. + + Has zero trainable parameters by design (Phase 1 — see module + docstring). Works with any planner whose `forward()` accepts a batch + dimension that can be safely repeated (true for FlowMatchingPlanner via + its `generator` kwarg for reproducible re-sampling; a deterministic + planner such as GRUPlanner will simply produce K identical samples and + this module degenerates to a no-op pass-through with K=1 behaviour). + """ + + def __init__(self, planner: nn.Module, num_timesteps: int, + config: Optional[ScorerConfig] = None): + super().__init__() + self.planner = planner + self.num_timesteps = num_timesteps + self.config = config or ScorerConfig() + + @torch.no_grad() + def sample_and_score(self, bev_features: torch.Tensor, + visual_history: torch.Tensor, + egomotion_history: torch.Tensor, + map_input: torch.Tensor, + num_samples: int = 8, + seed: Optional[int] = None): + """Draw `num_samples` trajectories per batch element and re-rank. + + Args: + bev_features: [B, embed_dim, H, W] — fused image+map BEV, as + already produced by AutoE2E before the planner call. + visual_history: [B, visual_history_dim]. + egomotion_history: [B, egomotion_dim]. + map_input: [B, 3, bev_h, bev_w] raw (un-normalized) rasterized + map image — see `drivable_area_compliance` note on + normalization. + num_samples: K, number of stochastic samples per batch element. + seed: optional base seed for reproducible re-sampling. + + Returns: + trajectory: [B, num_timesteps * num_signals] — best (or + softmax-blended) trajectory per batch element. + ego_hidden: [B, embed_dim] — from the FIRST sample only, + consistent with how FutureState is meant to receive a + single scene-gist vector, not a per-candidate one. + scores: [B, num_samples] — combined score per candidate, for + logging / debugging. + """ + B = bev_features.shape[0] + device = bev_features.device + + bev_rep = bev_features.repeat_interleave(num_samples, dim=0) + vh_rep = visual_history.repeat_interleave(num_samples, dim=0) + eh_rep = egomotion_history.repeat_interleave(num_samples, dim=0) + + generator = None + if seed is not None: + generator = torch.Generator(device=device).manual_seed(seed) + + trajectories, ego_hidden_all = self.planner( + bev_rep, vh_rep, eh_rep, generator=generator, + ) + # trajectories: [B*K, trajectory_dim] + trajectory_dim = trajectories.shape[-1] + trajectories = trajectories.view(B, num_samples, trajectory_dim) + ego_hidden_all = ego_hidden_all.view(B, num_samples, -1) + + xy = decode_trajectory_to_xy( + trajectories, self.num_timesteps, + dt=self.config.dt, initial_speed=self.config.initial_speed, + ) # [B, K, T, 2] + + dac_scores = torch.stack([ + drivable_area_compliance(xy[:, k], map_input, self.config) + for k in range(num_samples) + ], dim=1) # [B, K] + + comfort_scores = torch.stack([ + kinematic_comfort_score( + trajectories[:, k], self.num_timesteps, self.config, + ) + for k in range(num_samples) + ], dim=1) # [B, K] + + combined = ( + self.config.dac_weight * dac_scores + + self.config.comfort_weight * comfort_scores + ) + + if self.config.selection == "nearest": + best_idx = combined.argmax(dim=1) + trajectory = trajectories[torch.arange(B, device=device), best_idx] + elif self.config.selection == "mean": + weights = torch.softmax( + combined / self.config.softmax_temperature, dim=1, + ) + trajectory = (trajectories * weights.unsqueeze(-1)).sum(dim=1) + else: + raise ValueError( + f"config.selection must be 'nearest' or 'mean', " + f"got {self.config.selection!r}." + ) + + ego_hidden = ego_hidden_all[:, 0] + return trajectory, ego_hidden, combined diff --git a/PROPOSAL_diffusion_driving_policy.md b/PROPOSAL_diffusion_driving_policy.md new file mode 100644 index 000000000..4f30214bf --- /dev/null +++ b/PROPOSAL_diffusion_driving_policy.md @@ -0,0 +1,178 @@ +# Proposal: BEV-only trajectory scoring + few-step inference upgrade for `FlowMatchingPlanner` + +> Per CONTRIBUTING.md, this is posted as a Discussion before any PR, since +> it touches the trajectory planning module. References PR #40 (driving +> policy / FlowMatchingPlanner), PR #55 (map encoder / MapBEVFusion), issue +> #35 (BEV resolution / hardware target), issue #66 (open-loop eval +> pipeline), and the GoalFlow paper (arXiv:2503.05689) discussed in the +> 17/06 WG meeting. + +## Context + +`FlowMatchingPlanner` (merged in #40) already does conditional flow +matching with BEV cross-attention and AdaLN modulation, and the map +pipeline (#55) already fuses a rasterized nav-map into that BEV before the +planner runs. What's still missing relative to GoalFlow's design — which +we discussed in detail this week — is everything downstream of trajectory +*generation*: GoalFlow doesn't just sample one trajectory, it samples many, +scores them against a goal-point vocabulary and a learned drivable-area +classifier, and picks the best one. + +We were told to get BEV working first, before investing in the full +goal-point vocabulary + learned DAC classifier machinery GoalFlow uses. +This proposal is scoped to exactly that boundary: it adds multi-sample +generation and re-ranking using **only data we already have on disk** +(the rasterized map image, the predicted trajectory's own kinematics) — +zero new training, zero new loss terms, zero dependency on a semantic BEV +head or goal-point clustering that doesn't exist yet. + +## What this proposal adds (Phase 1) + +**1. `TrajectoryComplianceScorer`** — a wrapper around any `BasePlanner` +that draws `K` samples per scene (vectorized via batch-repeat, one +`forward()` call) and re-ranks them by: + +- **Drivable-area compliance**, read directly off the *raw rasterized* + `map_input` pixels at each waypoint's projected BEV coordinate. This is + a colour lookup, not a classifier — it stands in for GoalFlow's learned + DAC score until (if) we build a real semantic BEV head. +- **Kinematic comfort**, penalizing samples whose (acceleration, + curvature) sequence exceeds configurable comfort bounds. + +Selection mirrors GoalFlow's own two modes: `"nearest"` (argmax — pick the +single best-scoring sample) or `"mean"` (softmax-weighted blend across all +K samples). + +**2. Shifted inference-time timestep schedule** for `FlowMatchingPlanner` +— a small, additive change (`timestep_schedule="uniform"|"shifted"`, +default `"uniform"` preserves exact current behaviour) implementing the +same `t_shifted = (alpha·t)/(1+(alpha-1)·t)` warp GoalFlow uses at +inference, which their own ablation shows matters most at low step counts +— directly relevant to our Renesas R-Car deployment target and the +CPU/GPU latency work already in the benchmark thread. + +## What this proposal deliberately does NOT add (Phase 2 — deferred) + +- No goal-point vocabulary or offline-cached goal scores (GoalFlow's + `cluster_points_8192_.npy` + `goal_point_scores.gz` equivalent). +- No learned BEV semantic segmentation head (GoalFlow's + `_bev_semantic_head`) — Phase 1's drivable-area check is a raw colour + lookup precisely so we don't need one yet. +- No classifier-free-guidance-style goal-conditioned/unconditioned fusion. + +These are real GoalFlow ideas worth revisiting once KITScenes' HD map +gives us a reliable semantic drivable-area channel to train against — but +building the goal-point + DAC-classifier machinery before we have that +signal would mean training it against the same colour-lookup proxy this +proposal already gives us for free, which isn't worth the added +complexity yet. + +## Calibration caveat (needs a reviewer who owns the map renderer) + +`project_xy_to_bev_pixel` in the attached code needs `pixels_per_meter`, +`ego_row`, and `ego_col` confirmed against whatever convention the +KITScenes/L2D map renderer actually uses to produce `map_input`. The +defaults follow the 120 m front / 60 m rear / 60 m each side @ 0.4 m +geometry from issue #35, but I have not verified them against the +renderer itself — would appreciate a second pair of eyes here (cc Richard +/ Zain) before this gets merged, since a miscalibrated lookup would +silently score every trajectory as "compliant" or "non-compliant" +regardless of where it actually goes. + +## Sensor scope — camera + map only, no LiDAR (deliberate, not a gap) + +Worth stating explicitly rather than leaving implicit: this proposal, like +the rest of `auto_e2e` today, uses only camera tiles and the rasterized +map image — `AutoE2E.forward()` has no LiDAR input anywhere in its +signature, and nothing here changes that. + +It is worth separating two things that both get called "BEV" in this +discussion, since GoalFlow conflates them in a way that could cause +confusion later. GoalFlow's BEV comes from fusing camera features with a +*live LiDAR point cloud* (their camera backbone plus a separate LiDAR +encoder) — a runtime perception sensor that gives direct range +measurement. Our map-BEV is different in kind: it is a rendering of a +*pre-surveyed HD map* (KITScenes' vectorized map data), built offline, not +measured live. Skipping LiDAR does not cost us the map signal at all — +that signal was never LiDAR-dependent in the first place, and `map_input` +already reaches the planner today through `MapBEVFusion`. + +What skipping LiDAR does cost us is direct range measurement for dynamic +obstacles (other vehicles, pedestrians, cyclists). Without LiDAR, that has +to come entirely from camera-based depth and 3D understanding, which is +the harder and less metrically reliable half of the perception problem. +That trade-off is consistent with the project's existing all-camera +architecture and the Renesas R-Car embedded deployment target, so it is +not a new decision this proposal is introducing — but the WG's own scope +for the Driving Model Team lists "cameras, LIDAR/RADAR" as the intended +sensor suite, so this should be treated as a deliberate, visible +simplification for now rather than something that quietly becomes +permanent because nobody revisited it. + +Genuinely open to other framings here, or to being told this trade-off +analysis is missing something — flagging it explicitly so the group can +weigh in rather than letting it default silently into "how things are." + +## Files in the attached PR + +| File | Status | +|------|--------| +| `Model/model_components/trajectory_planning/trajectory_scorer.py` | New | +| `Model/model_components/trajectory_planning/flow_matching_planner.py` | Modified (additive, see patch notes) | + +Zero changes to `auto_e2e.py`, `base.py`, `gru_planner.py`, or any existing +public call site. `TrajectoryComplianceScorer` is opt-in — nothing wires it +into the default forward pass automatically, since that's a separate +design decision (does the scorer live inside `AutoE2E.forward()` behind a +config flag, or stay a standalone post-processing step callers opt into?) +that probably deserves its own discussion once Phase 1 lands and we have +real numbers from it. + +## Testing status + +**Smoke test (CPU, zero-GPU, no KITScenes data required):** Passed locally +against a fake `BasePlanner` that satisfies the `BasePlanner` contract. +Covers shape correctness, out-of-bounds waypoint handling, both selection +modes (`nearest` / `mean`), and the invalid-selection-mode error path. +This test does not require a GPU or the actual trained model and can be +reproduced on any machine with PyTorch installed. + +**Quality / integration test (does it actually improve ADE?):** Not yet run. +This requires a trained `FlowMatchingPlanner` checkpoint and parsed +KITScenes map+camera data feeding real `map_input` tensors — both of +which are currently inaccessible due to hardware constraints on the +contributor's machine for the next few days (GPU unavailable). This test +is also contingent on the map-pixel calibration review flagged above, since +a miscalibrated `ego_row` / `ego_col` would make the DAC score meaningless +and render any ADE comparison uninformative. + +**What this means for the PR:** the code is structurally sound and the +`BasePlanner` contract is fully satisfied, but the PR should be treated as +ready for code review and calibration review, not ready to merge until +quality validation on real data is confirmed. Will update the PR once +hardware is available. + +## Open questions for the group + +1. Where should `TrajectoryComplianceScorer` actually plug in — + `AutoE2E.forward()` behind a flag, or kept as a standalone utility + callers use explicitly (e.g. only at eval/inference time, never during + training)? +2. Is `num_samples` (K) something we want exposed as a runtime knob for + the Renesas target, where compute budget is tight, vs always running + at a fixed K? +3. Does the comfort-bound scoring belong here at all, or should it instead + become an auxiliary *training* loss term on `FlowMatchingPlanner` + directly (closer to how GoalFlow's own ablation table treats each + signal as a separate, addable loss)? +4. Is camera+map-only the right scope for this phase, or is there a + lighter-weight way to bring RADAR or LiDAR range data in earlier than + planned, given the Driving Model Team's stated longer-term sensor + suite? Open to being told there's a better way to sequence this than + what's proposed here. + +This is a first pass, not a final design — alternative approaches, +different scoring signals, or a different Phase 1/Phase 2 split are all +welcome. Posting it now mainly to get the scoping question (what needs +goal-point/LiDAR infrastructure we don't have yet, vs what doesn't) in +front of the group before writing more code against it. diff --git a/tests/test_trajectory_scorer.py b/tests/test_trajectory_scorer.py new file mode 100644 index 000000000..0deb4214f --- /dev/null +++ b/tests/test_trajectory_scorer.py @@ -0,0 +1,169 @@ +"""Smoke test for TrajectoryComplianceScorer. + +Runs entirely on CPU with a fake BasePlanner — no trained model, +no KITScenes data, no GPU required. Safe to run locally even when +hardware is constrained. + +Usage from repo root: + pytest tests/test_trajectory_scorer.py -v +or: + python tests/test_trajectory_scorer.py +""" + +import torch +import torch.nn as nn +import pytest + +from Model.model_components.trajectory_planning.trajectory_scorer import ( + ScorerConfig, + TrajectoryComplianceScorer, + decode_trajectory_to_xy, + drivable_area_compliance, + kinematic_comfort_score, + project_xy_to_bev_pixel, +) + +NUM_TIMESTEPS = 4 +BATCH = 2 +BEV_H, BEV_W = 450, 300 + + +class FakePlanner(nn.Module): + def __init__(self, num_timesteps=4, num_signals=2, embed_dim=8): + super().__init__() + self.trajectory_dim = num_timesteps * num_signals + self.embed_dim = embed_dim + + def forward(self, bev_features, visual_history, egomotion_history, + generator=None, **kwargs): + B = bev_features.shape[0] + traj = torch.randn(B, self.trajectory_dim, generator=generator) + ego_hidden = bev_features.flatten(1)[:, :self.embed_dim] + return traj, ego_hidden + + +@pytest.fixture() +def bev_features(): return torch.randn(BATCH, 8, 6, 6) +@pytest.fixture() +def visual_history(): return torch.randn(BATCH, 16) +@pytest.fixture() +def egomotion_history(): return torch.randn(BATCH, 12) +@pytest.fixture() +def map_input(): + m = torch.zeros(BATCH, 3, BEV_H, BEV_W) + m[:, :, 100:300, 100:200] = 255.0 + return m +@pytest.fixture() +def planner(): return FakePlanner(num_timesteps=NUM_TIMESTEPS) +@pytest.fixture() +def scorer(planner): return TrajectoryComplianceScorer(planner, num_timesteps=NUM_TIMESTEPS) + + +class TestDecodeTrajectoryToXY: + def test_output_shape(self): + xy = decode_trajectory_to_xy(torch.zeros(BATCH, NUM_TIMESTEPS * 2), NUM_TIMESTEPS) + assert xy.shape == (BATCH, NUM_TIMESTEPS, 2) + + def test_zero_curvature_stays_straight(self): + xy = decode_trajectory_to_xy(torch.zeros(1, NUM_TIMESTEPS * 2), NUM_TIMESTEPS) + assert torch.allclose(xy[0, :, 1], torch.zeros(NUM_TIMESTEPS), atol=1e-5) + assert (xy[0, 1:, 0] > xy[0, :-1, 0]).all() + + def test_extreme_deceleration_does_not_crash(self): + traj = torch.full((1, NUM_TIMESTEPS * 2), -1000.0) + xy = decode_trajectory_to_xy(traj, NUM_TIMESTEPS) + assert not torch.isnan(xy).any() and not torch.isinf(xy).any() + + +class TestProjectXYToBEVPixel: + def test_ego_origin_maps_correctly(self): + cfg = ScorerConfig() + px = project_xy_to_bev_pixel(torch.zeros(1, 2), cfg) + assert px[0, 0].item() == cfg.ego_row + assert px[0, 1].item() == cfg.ego_col + + def test_forward_motion_reduces_row(self): + cfg = ScorerConfig(forward_is_negative_row=True) + px = project_xy_to_bev_pixel(torch.tensor([[10.0, 0.0]]), cfg) + assert px[0, 0].item() < cfg.ego_row + + +class TestDrivableAreaCompliance: + def test_oob_reduces_compliance(self, map_input): + cfg = ScorerConfig() + traj = torch.zeros(BATCH, 1, 2) + traj[0, 0] = torch.tensor([10000.0, 10000.0]) + dac = drivable_area_compliance(traj, map_input, cfg) + assert dac[0].item() < 1.0 + assert dac[1].item() == 1.0 + + def test_range_zero_to_one(self, map_input): + cfg = ScorerConfig() + traj = torch.randn(BATCH, NUM_TIMESTEPS, 2) * 10 + dac = drivable_area_compliance(traj, map_input, cfg) + assert ((dac >= 0.0) & (dac <= 1.0)).all() + + +class TestKinematicComfortScore: + def test_no_violations_scores_one(self): + cfg = ScorerConfig(max_comfortable_accel=100.0, max_comfortable_lateral_accel=100.0) + score = kinematic_comfort_score(torch.zeros(BATCH, NUM_TIMESTEPS * 2), NUM_TIMESTEPS, cfg) + assert torch.allclose(score, torch.ones(BATCH)) + + def test_all_violations_scores_zero(self): + cfg = ScorerConfig(max_comfortable_accel=0.0, max_comfortable_lateral_accel=0.0) + score = kinematic_comfort_score(torch.ones(BATCH, NUM_TIMESTEPS * 2), NUM_TIMESTEPS, cfg) + assert torch.allclose(score, torch.zeros(BATCH)) + + +class TestTrajectoryComplianceScorer: + def test_output_shapes(self, scorer, bev_features, visual_history, egomotion_history, map_input): + traj, ego_hidden, scores = scorer.sample_and_score( + bev_features, visual_history, egomotion_history, map_input, num_samples=5, seed=42) + assert traj.shape == (BATCH, NUM_TIMESTEPS * 2) + assert ego_hidden.shape == (BATCH, 8) + assert scores.shape == (BATCH, 5) + + def test_mean_selection(self, planner, bev_features, visual_history, egomotion_history, map_input): + cfg = ScorerConfig(selection="mean") + s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) + traj, _, _ = s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4) + assert traj.shape == (BATCH, NUM_TIMESTEPS * 2) + + def test_invalid_selection_raises(self, planner, bev_features, visual_history, egomotion_history, map_input): + cfg = ScorerConfig(selection="bogus") + s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) + with pytest.raises(ValueError, match="config.selection"): + s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=3) + + def test_seed_reproducibility(self, scorer, bev_features, visual_history, egomotion_history, map_input): + t1, _, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) + t2, _, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) + assert torch.allclose(t1, t2) + + def test_different_seeds_differ(self, scorer, bev_features, visual_history, egomotion_history, map_input): + t1, _, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) + t2, _, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=99) + assert not torch.allclose(t1, t2) + + def test_scores_vary_across_samples(self, planner, bev_features, visual_history, egomotion_history, map_input): + cfg = ScorerConfig(dac_weight=1.0, comfort_weight=1.0) + s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) + _, _, scores = s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=8, seed=7) + assert scores.std(dim=1).sum() > 0 + + +if __name__ == "__main__": + print("Running smoke test (CPU, no GPU required)...") + bev = torch.randn(BATCH, 8, 6, 6) + vh = torch.randn(BATCH, 16) + eh = torch.randn(BATCH, 12) + mp = torch.zeros(BATCH, 3, BEV_H, BEV_W) + mp[:, :, 100:300, 100:200] = 255.0 + sc = TrajectoryComplianceScorer(FakePlanner(NUM_TIMESTEPS), NUM_TIMESTEPS) + traj, ego_h, scores = sc.sample_and_score(bev, vh, eh, mp, num_samples=6, seed=42) + print(f" trajectory: {tuple(traj.shape)}") + print(f" ego_hidden: {tuple(ego_h.shape)}") + print(f" scores: {tuple(scores.shape)}") + print(f" score range: {scores.min().item():.3f} – {scores.max().item():.3f}") + print("PASSED.") From 8dd9cf2dd71c5ec00caa2f8f6a81442339183fa3 Mon Sep 17 00:00:00 2001 From: Flagbusted Date: Thu, 9 Jul 2026 11:07:55 +0530 Subject: [PATCH 2/4] feat(trajectory_planning): add sample_k_trajectories to FlowMatchingPlanner MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Returns [B, K, 128] via repeat_interleave + single forward() call. K samples per scene are independent — each row draws fresh x_0 ~ N(0, I_128) inside forward(). 12 unit tests added. When issue #17 (BEV seg aux loss) lands, drivable_area_compliance can upgrade from pixel-colour heuristic to label-based lookup with no scorer API change. Implements #75 Signed-off-by: FLagbusted --- .../flow_matching_planner.py | 71 +++++++ Model/tests/test_trajectory_planning.py | 174 ++++++++++++++++++ 2 files changed, 245 insertions(+) diff --git a/Model/model_components/trajectory_planning/flow_matching_planner.py b/Model/model_components/trajectory_planning/flow_matching_planner.py index c69849fec..ab35bd85e 100644 --- a/Model/model_components/trajectory_planning/flow_matching_planner.py +++ b/Model/model_components/trajectory_planning/flow_matching_planner.py @@ -388,3 +388,74 @@ def forward(self, bev_features, visual_history, egomotion_history, horizon_tokens=reasoning_horizon_tokens) x = x + dt * v return x + + def sample_k_trajectories( + self, + bev_features: torch.Tensor, + visual_history: torch.Tensor, + egomotion_history: torch.Tensor, + num_samples: int = 8, + ) -> torch.Tensor: + """Sample K independent trajectories per scene via batch-repeat. + + Expands the batch by a factor of ``num_samples`` using + ``repeat_interleave``, then calls ``forward()`` once. Each of the + B*K rows draws a fresh ``x_0 ~ N(0, I_128)`` noise tensor inside + ``forward()``, so the K trajectories per scene are genuinely + independent — not deterministic variants of a single seed. + + This satisfies the WG multi-path output requirement (related to + issue #17): when the BEV segmentation auxiliary loss (#17) lands + and provides drivable-area labels from KITScenes, the downstream + ``TrajectoryComplianceScorer.drivable_area_compliance`` step can + upgrade from pixel-colour heuristics to label-based lookup with + no change to this API. + + Note: this method is intentionally added to ``FlowMatchingPlanner`` + only, not to ``BasePlanner`` ABC. ``GRUPlanner`` cannot sample + independently across K because its sequential rollout is seeded by + shared hidden state; adding this to the ABC would force a broken + GRU implementation. + + Args: + bev_features: [B, embed_dim, H, W] + visual_history: [B, visual_history_dim] + egomotion_history: [B, egomotion_dim] + num_samples: K — number of independent trajectory samples + per scene. Must be >= 1. + + Returns: + trajectories: [B, K, trajectory_dim] — K independent samples + for each of the B input scenes. + + Example:: + + planner = FlowMatchingPlanner(embed_dim=256) + paths = planner.sample_k_trajectories( + bev_features, # [2, 256, 8, 8] + visual_history, # [2, 896] + egomotion_history, # [2, 256] + num_samples=8, + ) + # paths.shape == (2, 8, 128) + """ + if num_samples < 1: + raise ValueError( + f"num_samples must be >= 1, got {num_samples}." + ) + B = bev_features.shape[0] + K = num_samples + + # Expand each scene K times: scene i occupies rows [i*K : (i+1)*K]. + # repeat_interleave keeps scenes contiguous so .view(B, K, ...) is safe. + bev_K = bev_features.repeat_interleave(K, dim=0) # [B*K, C, H, W] + vh_K = visual_history.repeat_interleave(K, dim=0) # [B*K, 896] + em_K = egomotion_history.repeat_interleave(K, dim=0) # [B*K, 256] + + # Single forward pass. Independence guarantee: each of the B*K rows + # draws its own x_0 ~ N(0, I_128) inside forward() at the line + # x = torch.randn(B, self.trajectory_dim, ...) + # With B replaced by B*K here, that gives K genuinely separate noise + # draws per scene — no shared state, no broadcasting. + trajectories = self.forward(bev_K, vh_K, em_K) # [B*K, 128] + return trajectories.view(B, K, self.trajectory_dim) # [B, K, 128] diff --git a/Model/tests/test_trajectory_planning.py b/Model/tests/test_trajectory_planning.py index ba1d8becc..25afec88e 100644 --- a/Model/tests/test_trajectory_planning.py +++ b/Model/tests/test_trajectory_planning.py @@ -405,3 +405,177 @@ def test_bezier_and_fm_interchangeable_at_inference(self, build_mock_model, devi bezier_traj = bezier_model(visual, map_input, vis_hist, ego, mode="infer") fm_traj = fm_model(visual, map_input, vis_hist, ego, mode="infer") assert bezier_traj.shape == fm_traj.shape == (2, 128) + + +# --------------------------------------------------------------------------- +# sample_k_trajectories — multi-path output (WG action item, related to #17) +# --------------------------------------------------------------------------- + +class TestSampleKTrajectories: + """Tests for FlowMatchingPlanner.sample_k_trajectories(). + + Uses a tiny planner (embed_dim=16, 4-step T, 2-signal S) so every test + runs in <50 ms on CPU with no GPU required. Follows the same fixture + and assertion style as TestFlowMatchingPlanner above. + """ + + @pytest.fixture + def planner(self, device): + return FlowMatchingPlanner( + embed_dim=16, + num_timesteps=4, + num_signals=2, + egomotion_dim=8, + visual_history_dim=32, + num_inference_steps=2, + time_embed_dim=8, + num_heads=2, + ).eval().to(device) + + # ------------------------------------------------------------------ + # Shape contract + # ------------------------------------------------------------------ + + def test_output_shape(self, planner, device): + """Returns exactly [B, K, trajectory_dim].""" + B, K = 2, 4 + bev = torch.randn(B, 16, 4, 4, device=device) + vh = torch.randn(B, 32, device=device) + em = torch.randn(B, 8, device=device) + out = planner.sample_k_trajectories(bev, vh, em, num_samples=K) + assert out.shape == (B, K, planner.trajectory_dim), ( + f"Expected ({B}, {K}, {planner.trajectory_dim}), got {tuple(out.shape)}" + ) + + def test_k1_shape(self, planner, device): + """num_samples=1 returns [B, 1, trajectory_dim], not [B, trajectory_dim].""" + B = 3 + bev = torch.randn(B, 16, 4, 4, device=device) + vh = torch.randn(B, 32, device=device) + em = torch.randn(B, 8, device=device) + out = planner.sample_k_trajectories(bev, vh, em, num_samples=1) + assert out.shape == (B, 1, planner.trajectory_dim) + + def test_large_k_shape(self, planner, device): + """K=16 scales linearly — output shape is still [B, 16, trajectory_dim].""" + B, K = 1, 16 + out = planner.sample_k_trajectories( + torch.randn(B, 16, 4, 4, device=device), + torch.randn(B, 32, device=device), + torch.randn(B, 8, device=device), + num_samples=K, + ) + assert out.shape == (B, K, planner.trajectory_dim) + + def test_output_is_finite(self, planner, device): + """All returned values must be finite (no NaN/Inf from Euler steps).""" + out = planner.sample_k_trajectories( + torch.randn(1, 16, 4, 4, device=device), + torch.randn(1, 32, device=device), + torch.randn(1, 8, device=device), + num_samples=4, + ) + assert torch.isfinite(out).all(), ( + "sample_k_trajectories returned non-finite values" + ) + + def test_output_on_correct_device(self, planner, device): + """Output tensor must reside on the same device as the inputs.""" + out = planner.sample_k_trajectories( + torch.randn(1, 16, 4, 4, device=device), + torch.randn(1, 32, device=device), + torch.randn(1, 8, device=device), + num_samples=3, + ) + assert out.device.type == torch.device(device).type + + # ------------------------------------------------------------------ + # Independence guarantee + # ------------------------------------------------------------------ + + def test_samples_are_diverse(self, planner, device): + """K trajectories for the same scene must not all be identical. + + If the K noise draws were broadcast rather than independently sampled, + all K outputs would be equal — this catches that regression. + """ + B, K = 1, 8 + out = planner.sample_k_trajectories( + torch.randn(B, 16, 4, 4, device=device), + torch.randn(B, 32, device=device), + torch.randn(B, 8, device=device), + num_samples=K, + ) + assert not torch.allclose(out[0, 0], out[0, 1], atol=1e-6), ( + "All K trajectories are identical — noise draws are not independent" + ) + + def test_different_scenes_yield_different_outputs(self, planner, device): + """Two scenes with different BEV/history must produce different K-banks.""" + B, K = 2, 4 + out = planner.sample_k_trajectories( + torch.randn(B, 16, 4, 4, device=device), + torch.randn(B, 32, device=device), + torch.randn(B, 8, device=device), + num_samples=K, + ) + # Different BEV features → different trajectories across scenes. + assert not torch.allclose(out[0], out[1], atol=1e-3) + + def test_k1_consistent_with_forward(self, planner, device): + """num_samples=1 and forward() must produce same-shape outputs. + + Not asserting identical values (forward draws fresh noise each call), + but shapes and dtypes must match. + """ + bev = torch.randn(2, 16, 4, 4, device=device) + vh = torch.randn(2, 32, device=device) + em = torch.randn(2, 8, device=device) + single = planner.forward(bev, vh, em) # [2, 8] + multi = planner.sample_k_trajectories(bev, vh, em, num_samples=1) # [2, 1, 8] + assert multi.shape == (2, 1, single.shape[-1]) + assert multi.dtype == single.dtype + + # ------------------------------------------------------------------ + # Error handling + # ------------------------------------------------------------------ + + def test_invalid_num_samples_zero(self, planner, device): + """num_samples=0 must raise ValueError.""" + with pytest.raises(ValueError, match="num_samples must be >= 1"): + planner.sample_k_trajectories( + torch.randn(1, 16, 4, 4, device=device), + torch.randn(1, 32, device=device), + torch.randn(1, 8, device=device), + num_samples=0, + ) + + def test_invalid_num_samples_negative(self, planner, device): + """Negative num_samples must raise ValueError.""" + with pytest.raises(ValueError, match="num_samples must be >= 1"): + planner.sample_k_trajectories( + torch.randn(1, 16, 4, 4, device=device), + torch.randn(1, 32, device=device), + torch.randn(1, 8, device=device), + num_samples=-3, + ) + + def test_wrong_visual_history_dim_raises(self, planner, device): + """Mismatched visual_history dim propagates ValueError from forward().""" + with pytest.raises(ValueError, match="visual_history last dim"): + planner.sample_k_trajectories( + torch.randn(1, 16, 4, 4, device=device), + torch.randn(1, 999, device=device), # wrong dim + torch.randn(1, 8, device=device), + num_samples=2, + ) + + def test_wrong_egomotion_dim_raises(self, planner, device): + """Mismatched egomotion_history dim propagates ValueError from forward().""" + with pytest.raises(ValueError, match="egomotion_history last dim"): + planner.sample_k_trajectories( + torch.randn(1, 16, 4, 4, device=device), + torch.randn(1, 32, device=device), + torch.randn(1, 999, device=device), # wrong dim + num_samples=2, + ) From 2ced0619ba4859fd450e71eaf82e2d09f565b7ba Mon Sep 17 00:00:00 2001 From: FLagbusted Date: Wed, 22 Jul 2026 01:49:08 +0000 Subject: [PATCH 3/4] fix(trajectory_scorer): sample_and_score never matched BasePlanner's real contract MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit sample_and_score unpacked self.planner(...) as (trajectory, ego_hidden) — BasePlanner.forward() has only ever returned a single trajectory tensor (see base.py's own docstring). Every test here passed anyway because the local FakePlanner test double returned a 2-tuple to match the WRONG contract, so this was never exercised against a real planner. Wiring TrajectoryComplianceScorer(real_flow_matching_planner, ...) and calling sample_and_score would have failed immediately with an unpack error. FutureState (the only place ego_hidden was ever consumed) isn't called from AutoE2E.forward() any more — WorldActionModel.predict_future superseded it — so there's nothing downstream expecting a second value. sample_and_score now returns (trajectory, scores). Also, per @riita10069's review on #76: - decode_trajectory_to_xy now wraps the canonical Model.evaluation.metrics.integrate_trajectory instead of duplicating the bicycle-model integration. - Both decode_trajectory_to_xy and kinematic_comfort_score now take a real per-row initial_speed (extract_initial_speed(), reading the speed channel out of egomotion_history) instead of a fixed ScorerConfig.initial_speed=5.0 placeholder applied to every sample regardless of how fast the ego actually was. Added a regression test (test_different_initial_speeds_change_selection) that would have caught this. - Flagged in the module docstring: do not calibrate pixels_per_meter/ego_row/ego_col/drivable_rgb against Model/data_parsing/kit_scenes/map.py as it stands today without checking #148/#149 first — both open, both assigned to riita10069, both change the geometry this scorer would be calibrated against. Moved tests/test_trajectory_scorer.py -> Model/tests/ (per review) and discovered why it mattered beyond tidiness: python -m pytest Model/tests -v ============================= test session starts ============================== platform linux -- Python 3.12.3, pytest-9.1.1, pluggy-1.6.0 -- /usr/bin/python cachedir: .pytest_cache rootdir: /home/claude/auto_fsd2/Model configfile: pytest.ini plugins: anyio-4.14.2 collecting ... collected 596 items / 3 errors / 6 deselected / 6 skipped / 590 selected ==================================== ERRORS ==================================== ___________ ERROR collecting tests/test_dataset_publication_tasks.py ___________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_dataset_publication_tasks.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_dataset_publication_tasks.py:10: in from botocore.exceptions import ClientError E ModuleNotFoundError: No module named 'botocore' _________________ ERROR collecting tests/test_overlay_tasks.py _________________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_overlay_tasks.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_overlay_tasks.py:9: in from botocore.exceptions import ClientError E ModuleNotFoundError: No module named 'botocore' ______________ ERROR collecting tests/test_training_checkpoint.py ______________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_training_checkpoint.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_training_checkpoint.py:11: in from botocore.exceptions import ClientError E ModuleNotFoundError: No module named 'botocore' =========================== short test summary info ============================ ERROR Model/tests/test_dataset_publication_tasks.py ERROR Model/tests/test_overlay_tasks.py ERROR Model/tests/test_training_checkpoint.py !!!!!!!!!!!!!!!!!!! Interrupted: 3 errors during collection !!!!!!!!!!!!!!!!!!!! ================== 6 skipped, 6 deselected, 3 errors in 3.18s ================== / CI only run ============================= test session starts ============================== platform linux -- Python 3.12.3, pytest-9.1.1, pluggy-1.6.0 rootdir: /home/claude/auto_fsd2/Model configfile: pytest.ini plugins: anyio-4.14.2 collected 518 items / 12 errors / 6 deselected / 6 skipped / 512 selected ==================================== ERRORS ==================================== ______________ ERROR collecting tests/test_dataset_publication.py ______________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_dataset_publication.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_dataset_publication.py:10: in from Platform.pipelines.dataset_publication import ( E ModuleNotFoundError: No module named 'Platform' ___________ ERROR collecting tests/test_dataset_publication_tasks.py ___________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_dataset_publication_tasks.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_dataset_publication_tasks.py:10: in from botocore.exceptions import ClientError E ModuleNotFoundError: No module named 'botocore' _______________ ERROR collecting tests/test_grad_accumulation.py _______________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_grad_accumulation.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_grad_accumulation.py:23: in from Platform.pipelines.training_checkpoint import ( E ModuleNotFoundError: No module named 'Platform' ______________ ERROR collecting tests/test_kitscenes_recovery.py _______________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_kitscenes_recovery.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_kitscenes_recovery.py:9: in from Platform.pipelines.kitscenes_recovery import ( E ModuleNotFoundError: No module named 'Platform' _______________ ERROR collecting tests/test_overlay_artifact.py ________________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_overlay_artifact.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_overlay_artifact.py:10: in from Platform.pipelines.overlay import ( E ModuleNotFoundError: No module named 'Platform' _______________ ERROR collecting tests/test_overlay_inference.py _______________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_overlay_inference.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_overlay_inference.py:8: in from Platform.pipelines.inference import ( E ModuleNotFoundError: No module named 'Platform' ____________ ERROR collecting tests/test_overlay_reproducibility.py ____________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_overlay_reproducibility.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_overlay_reproducibility.py:7: in from Platform.pipelines.reproducibility import ( E ModuleNotFoundError: No module named 'Platform' _________________ ERROR collecting tests/test_overlay_store.py _________________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_overlay_store.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_overlay_store.py:7: in from Platform.pipelines.overlay_store import ( E ModuleNotFoundError: No module named 'Platform' _________________ ERROR collecting tests/test_overlay_tasks.py _________________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_overlay_tasks.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_overlay_tasks.py:9: in from botocore.exceptions import ClientError E ModuleNotFoundError: No module named 'botocore' ______________ ERROR collecting tests/test_training_checkpoint.py ______________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_training_checkpoint.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_training_checkpoint.py:11: in from botocore.exceptions import ClientError E ModuleNotFoundError: No module named 'botocore' _______________ ERROR collecting tests/test_trajectory_scorer.py _______________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_trajectory_scorer.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_trajectory_scorer.py:17: in from Model.model_components.trajectory_planning.trajectory_scorer import ( E ModuleNotFoundError: No module named 'Model' ___________ ERROR collecting tests/test_trajectory_visualization.py ____________ ImportError while importing test module '/home/claude/auto_fsd2/Model/tests/test_trajectory_visualization.py'. Hint: make sure your test modules/packages have valid Python names. Traceback: /usr/lib/python3.12/importlib/__init__.py:90: in import_module return _bootstrap._gcd_import(name[level:], package, level) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ Model/tests/test_trajectory_visualization.py:17: in from Platform.pipelines.overlay import write_overlay E ModuleNotFoundError: No module named 'Platform' =========================== short test summary info ============================ ERROR Model/tests/test_dataset_publication.py ERROR Model/tests/test_dataset_publication_tasks.py ERROR Model/tests/test_grad_accumulation.py ERROR Model/tests/test_kitscenes_recovery.py ERROR Model/tests/test_overlay_artifact.py ERROR Model/tests/test_overlay_inference.py ERROR Model/tests/test_overlay_reproducibility.py ERROR Model/tests/test_overlay_store.py ERROR Model/tests/test_overlay_tasks.py ERROR Model/tests/test_training_checkpoint.py ERROR Model/tests/test_trajectory_scorer.py ERROR Model/tests/test_trajectory_visualization.py !!!!!!!!!!!!!!!!!!! Interrupted: 12 errors during collection !!!!!!!!!!!!!!!!!!! ================= 6 skipped, 6 deselected, 12 errors in 2.21s ================== (see Makefile), so this entire file — all 15 original tests — was never collected by CI. Neither the contract bug above nor a pre-existing off-by-one in the map fixture (the drivable rectangle stopped one row short of ego_row, so the 'origin should always score compliant' assumption was silently false) had ever actually run. Fixed the fixture and added 5 new tests (extract_initial_speed coverage, initial-speed-changes-decode, the contract regression test above); 20/20 pass now, none of them previously executed by CI. Removed PROPOSAL_diffusion_driving_policy.md from repo root per review — belongs in Discussion #75, not a tracked file in this PR. Full re-run: Model/tests/test_trajectory_scorer.py (20), Model/tests/test_trajectory_planning.py + test_reasoning_coupling.py (51 passed, 1 GPU-only skip). ruff clean. Signed-off-by: FLagbusted --- .../trajectory_planning/trajectory_scorer.py | 144 ++++++++--- Model/tests/test_trajectory_scorer.py | 244 ++++++++++++++++++ PROPOSAL_diffusion_driving_policy.md | 178 ------------- tests/test_trajectory_scorer.py | 169 ------------ 4 files changed, 349 insertions(+), 386 deletions(-) create mode 100644 Model/tests/test_trajectory_scorer.py delete mode 100644 PROPOSAL_diffusion_driving_policy.md delete mode 100644 tests/test_trajectory_scorer.py diff --git a/Model/model_components/trajectory_planning/trajectory_scorer.py b/Model/model_components/trajectory_planning/trajectory_scorer.py index 7199a16a5..03cd49e47 100644 --- a/Model/model_components/trajectory_planning/trajectory_scorer.py +++ b/Model/model_components/trajectory_planning/trajectory_scorer.py @@ -34,14 +34,26 @@ resolution -> 450 x 300 px, issue #35) but have NOT been verified against the renderer itself — confirm with whoever owns that code before relying on the compliance score in any reported metric. + +DO NOT calibrate against Model/data_parsing/kit_scenes/map.py as it stands +today without checking #148 and #149 first: #149 proposes replacing the +current 640x360 non-square render with a 256x256 square tile at 120 m in +*all four* directions (symmetric), and #148 is reworking what the map +actually encodes (route direction from GPS traces, not just a static +drivable-area raster). Both are open and assigned to riita10069 as of +2026-07-21 — the geometry below may need to change again once those land, +not just be verified against today's renderer. """ from dataclasses import dataclass from typing import Optional +import numpy as np import torch import torch.nn as nn +from Model.evaluation.metrics import integrate_trajectory + @dataclass class ScorerConfig: @@ -71,7 +83,8 @@ class ScorerConfig: max_comfortable_accel: float = 3.0 # m/s^2 max_comfortable_lateral_accel: float = 2.0 # m/s^2, = curvature * speed^2 dt: float = 0.1 # seconds between model timesteps - initial_speed: float = 5.0 # m/s, matches existing decode convention + # NOTE: no fixed initial_speed field — real per-scene speed is read + # out of egomotion_history via extract_initial_speed(), not guessed. # --- scoring weights --- dac_weight: float = 1.0 @@ -82,50 +95,88 @@ class ScorerConfig: softmax_temperature: float = 1.0 +def extract_initial_speed(egomotion_history: torch.Tensor) -> torch.Tensor: + """Read the real per-scene starting speed out of egomotion_history. + + egomotion_history is (256,) = 64 history timesteps x 4 signals + [speed, acceleration, yaw_rate, curvature] (see + Model/data_parsing/kit_scenes/egomotion.py). The most recent history + row (index -1) is "now" — its speed channel (index 0) is exactly the + v0 that the prediction horizon starts from. + + Args: + egomotion_history: [..., 256]. + + Returns: + initial_speed: [...] real starting speed in m/s, one per row. + """ + *batch_shape, dim = egomotion_history.shape + if dim != 256: + raise ValueError( + f"egomotion_history last dim must be 256 (64 timesteps x 4 " + f"signals), got {dim}." + ) + history = egomotion_history.reshape(*batch_shape, 64, 4) + return history[..., -1, 0] + + def decode_trajectory_to_xy(trajectory: torch.Tensor, num_timesteps: int, - dt: float = 0.1, - initial_speed: float = 5.0) -> torch.Tensor: + initial_speed: torch.Tensor, + dt: float = 0.1) -> torch.Tensor: """Decode (acceleration, curvature) pairs into (x, y) waypoints. - Mirrors the bicycle-model integration already used for offline - evaluation against Waymo ground truth. If/when a canonical decode - utility lands (tracked under the open-loop eval pipeline, issue #66), - this function should be replaced by an import from that module instead - of duplicating the integration here. + Thin torch<->numpy wrapper around the canonical + ``Model.evaluation.metrics.integrate_trajectory`` bicycle-model + integrator, so this scorer and offline open-loop eval (ADE/FDE against + Waymo/KITScenes ground truth) share one integration implementation + instead of two that can silently drift apart. + + Runs at @torch.no_grad() call sites only (TrajectoryComplianceScorer + has zero trainable parameters by design — see module docstring), so + the per-row Python loop and numpy round-trip cost nothing that + matters: K is small (default 8) and this never sits in a training step. Args: trajectory: [..., num_timesteps * 2] — flat (accel, curvature) pairs. num_timesteps: number of (accel, curvature) pairs encoded. + initial_speed: [...] real starting speed in m/s, one per row — + see ``extract_initial_speed``. Broadcasts against + ``trajectory``'s leading dims; every row MUST carry its own + real value, not a fixed placeholder — a fixed default here + silently overrides every sample's actual starting speed with + the same guess, regardless of how fast the ego really was + moving. dt: seconds between timesteps. - initial_speed: assumed starting speed in m/s. Returns: xy: [..., num_timesteps, 2] waypoints in ego-relative meters. """ *batch_shape, _ = trajectory.shape pairs = trajectory.reshape(*batch_shape, num_timesteps, 2) - accels, curvatures = pairs[..., 0], pairs[..., 1] + accels = pairs[..., 0].reshape(-1, num_timesteps) + curvatures = pairs[..., 1].reshape(-1, num_timesteps) + speeds = initial_speed.reshape(-1) + + if speeds.shape[0] != accels.shape[0]: + raise ValueError( + f"initial_speed must broadcast to trajectory's leading dims: " + f"got {speeds.shape[0]} speed rows for {accels.shape[0]} " + f"trajectory rows." + ) device, dtype = trajectory.device, trajectory.dtype - flat = accels.reshape(-1, num_timesteps) - flat_curv = curvatures.reshape(-1, num_timesteps) - n = flat.shape[0] - - x = torch.zeros(n, device=device, dtype=dtype) - y = torch.zeros(n, device=device, dtype=dtype) - heading = torch.zeros(n, device=device, dtype=dtype) - speed = torch.full((n,), initial_speed, device=device, dtype=dtype) - - xs, ys = [], [] - for t in range(num_timesteps): - speed = torch.clamp(speed + flat[:, t] * dt, min=0.0) - heading = heading + flat_curv[:, t] * speed * dt - x = x + torch.cos(heading) * speed * dt - y = y + torch.sin(heading) * speed * dt - xs.append(x.clone()) - ys.append(y.clone()) - - xy = torch.stack([torch.stack(xs, dim=-1), torch.stack(ys, dim=-1)], dim=-1) + accels_np = accels.detach().cpu().numpy() + curv_np = curvatures.detach().cpu().numpy() + speeds_np = speeds.detach().cpu().numpy() + + n = accels_np.shape[0] + xy_np = np.empty((n, num_timesteps, 2), dtype=np.float64) + for i in range(n): + xy_np[i] = integrate_trajectory( + accels_np[i], curv_np[i], float(speeds_np[i]), dt=dt, + ) + + xy = torch.from_numpy(xy_np).to(device=device, dtype=dtype) return xy.reshape(*batch_shape, num_timesteps, 2) @@ -192,6 +243,7 @@ def drivable_area_compliance(xy: torch.Tensor, map_input: torch.Tensor, def kinematic_comfort_score(trajectory: torch.Tensor, num_timesteps: int, + initial_speed: torch.Tensor, config: ScorerConfig) -> torch.Tensor: """Penalize (acceleration, curvature) samples that exceed comfort bounds. @@ -201,6 +253,11 @@ def kinematic_comfort_score(trajectory: torch.Tensor, num_timesteps: int, Args: trajectory: [B, num_timesteps * 2] flat (accel, curvature) pairs. num_timesteps: number of pairs encoded. + initial_speed: [B] real starting speed in m/s — see + ``extract_initial_speed``. Same reasoning as + ``decode_trajectory_to_xy``: a fixed guess here would silently + score every sample's lateral-accel comfort against the wrong + speed profile. config: comfort bound parameters. Returns: @@ -211,7 +268,7 @@ def kinematic_comfort_score(trajectory: torch.Tensor, num_timesteps: int, # Approximate speed via cumulative integration of accel (matches decode). speed = torch.clamp( - config.initial_speed + torch.cumsum(accels * config.dt, dim=1), + initial_speed.reshape(-1, 1) + torch.cumsum(accels * config.dt, dim=1), min=0.0, ) lateral_accel = curvatures * speed.pow(2) @@ -266,11 +323,19 @@ def sample_and_score(self, bev_features: torch.Tensor, Returns: trajectory: [B, num_timesteps * num_signals] — best (or softmax-blended) trajectory per batch element. - ego_hidden: [B, embed_dim] — from the FIRST sample only, - consistent with how FutureState is meant to receive a - single scene-gist vector, not a per-candidate one. scores: [B, num_samples] — combined score per candidate, for logging / debugging. + + Note: this used to also return an `ego_hidden` second element, + unpacked from `self.planner(...)` as if forward() returned a + 2-tuple. It never did — BasePlanner.forward() has always returned + a single trajectory tensor (see base.py's own docstring), so that + unpack would raise the moment this ran against a real planner + instead of a test double shaped to match the wrong contract. + FutureState (the only place ego_hidden was ever consumed) isn't + wired into AutoE2E.forward() any more — the World Model path + (WorldActionModel.predict_future) superseded it — so there's + nothing left downstream expecting a second return value. """ B = bev_features.shape[0] device = bev_features.device @@ -283,17 +348,18 @@ def sample_and_score(self, bev_features: torch.Tensor, if seed is not None: generator = torch.Generator(device=device).manual_seed(seed) - trajectories, ego_hidden_all = self.planner( + trajectories = self.planner( bev_rep, vh_rep, eh_rep, generator=generator, ) # trajectories: [B*K, trajectory_dim] trajectory_dim = trajectories.shape[-1] trajectories = trajectories.view(B, num_samples, trajectory_dim) - ego_hidden_all = ego_hidden_all.view(B, num_samples, -1) + + initial_speed = extract_initial_speed(eh_rep).view(B, num_samples) xy = decode_trajectory_to_xy( trajectories, self.num_timesteps, - dt=self.config.dt, initial_speed=self.config.initial_speed, + initial_speed=initial_speed, dt=self.config.dt, ) # [B, K, T, 2] dac_scores = torch.stack([ @@ -303,7 +369,8 @@ def sample_and_score(self, bev_features: torch.Tensor, comfort_scores = torch.stack([ kinematic_comfort_score( - trajectories[:, k], self.num_timesteps, self.config, + trajectories[:, k], self.num_timesteps, + initial_speed[:, k], self.config, ) for k in range(num_samples) ], dim=1) # [B, K] @@ -327,5 +394,4 @@ def sample_and_score(self, bev_features: torch.Tensor, f"got {self.config.selection!r}." ) - ego_hidden = ego_hidden_all[:, 0] - return trajectory, ego_hidden, combined + return trajectory, combined diff --git a/Model/tests/test_trajectory_scorer.py b/Model/tests/test_trajectory_scorer.py new file mode 100644 index 000000000..f41d87b1b --- /dev/null +++ b/Model/tests/test_trajectory_scorer.py @@ -0,0 +1,244 @@ +"""Smoke test for TrajectoryComplianceScorer. + +Runs entirely on CPU with a fake BasePlanner — no trained model, +no KITScenes data, no GPU required. Safe to run locally even when +hardware is constrained. + +Usage from repo root: + pytest Model/tests/test_trajectory_scorer.py -v +or: + python Model/tests/test_trajectory_scorer.py +""" + +import torch +import torch.nn as nn +import pytest + +from Model.model_components.trajectory_planning.trajectory_scorer import ( + ScorerConfig, + TrajectoryComplianceScorer, + decode_trajectory_to_xy, + drivable_area_compliance, + extract_initial_speed, + kinematic_comfort_score, + project_xy_to_bev_pixel, +) + +NUM_TIMESTEPS = 4 +BATCH = 2 +BEV_H, BEV_W = 450, 300 +DEFAULT_SPEED = 5.0 # m/s, used to build realistic-looking egomotion fixtures + + +class FakePlanner(nn.Module): + """Matches BasePlanner's real contract: forward() returns a single + trajectory tensor, NOT a (trajectory, ego_hidden) tuple. An earlier + version of this fixture returned a tuple, which let every test here + pass while the real TrajectoryComplianceScorer.sample_and_score would + have crashed the moment it wrapped an actual FlowMatchingPlanner — + the fake didn't match what it was standing in for. See base.py's + docstring: "forward() always performs inference and returns + (trajectory) regardless of the underlying decoder." + """ + + def __init__(self, num_timesteps=4, num_signals=2, embed_dim=8): + super().__init__() + self.trajectory_dim = num_timesteps * num_signals + self.embed_dim = embed_dim + + def forward(self, bev_features, visual_history, egomotion_history, + generator=None, **kwargs): + B = bev_features.shape[0] + return torch.randn(B, self.trajectory_dim, generator=generator) + + +def _egomotion(batch=BATCH, speed=DEFAULT_SPEED): + """(batch, 256) = 64 history timesteps x [speed, accel, yaw_rate, curvature]. + Only the most recent timestep's speed channel is read by the scorer + (extract_initial_speed), but the full realistic shape is used here so + a shape regression in that helper would actually be caught.""" + history = torch.zeros(batch, 64, 4) + history[:, :, 0] = speed + return history.reshape(batch, 256) + + +@pytest.fixture() +def bev_features(): return torch.randn(BATCH, 8, 6, 6) +@pytest.fixture() +def visual_history(): return torch.randn(BATCH, 16) +@pytest.fixture() +def egomotion_history(): return _egomotion() +@pytest.fixture() +def map_input(): + # ScorerConfig defaults put ego_row at 300 — the drivable rectangle + # must include that row or the ego origin itself reads as + # non-drivable (this fixture originally stopped at row 300 exclusive, + # a one-pixel-short rectangle that made the "origin should always be + # compliant" assumption below false; never caught because this file + # lived at repo-root tests/, outside what `make test` / CI actually + # collects — see Model/pytest.ini + Makefile's `test:` target). + m = torch.zeros(BATCH, 3, BEV_H, BEV_W) + m[:, :, 100:301, 100:200] = 255.0 + return m +@pytest.fixture() +def planner(): return FakePlanner(num_timesteps=NUM_TIMESTEPS) +@pytest.fixture() +def scorer(planner): return TrajectoryComplianceScorer(planner, num_timesteps=NUM_TIMESTEPS) + + +class TestExtractInitialSpeed: + def test_reads_last_history_row_speed_channel(self): + eh = _egomotion(batch=3, speed=7.5) + speed = extract_initial_speed(eh) + assert speed.shape == (3,) + assert torch.allclose(speed, torch.full((3,), 7.5)) + + def test_wrong_last_dim_raises(self): + with pytest.raises(ValueError, match="256"): + extract_initial_speed(torch.randn(BATCH, 12)) + + +class TestDecodeTrajectoryToXY: + def test_output_shape(self): + speed = torch.full((BATCH,), DEFAULT_SPEED) + xy = decode_trajectory_to_xy( + torch.zeros(BATCH, NUM_TIMESTEPS * 2), NUM_TIMESTEPS, speed) + assert xy.shape == (BATCH, NUM_TIMESTEPS, 2) + + def test_zero_curvature_stays_straight(self): + speed = torch.full((1,), DEFAULT_SPEED) + xy = decode_trajectory_to_xy( + torch.zeros(1, NUM_TIMESTEPS * 2), NUM_TIMESTEPS, speed) + assert torch.allclose(xy[0, :, 1], torch.zeros(NUM_TIMESTEPS), atol=1e-5) + assert (xy[0, 1:, 0] > xy[0, :-1, 0]).all() + + def test_extreme_deceleration_does_not_crash(self): + traj = torch.full((1, NUM_TIMESTEPS * 2), -1000.0) + speed = torch.full((1,), DEFAULT_SPEED) + xy = decode_trajectory_to_xy(traj, NUM_TIMESTEPS, speed) + assert not torch.isnan(xy).any() and not torch.isinf(xy).any() + + def test_different_initial_speed_gives_different_xy(self): + traj = torch.zeros(1, NUM_TIMESTEPS * 2) + xy_slow = decode_trajectory_to_xy(traj, NUM_TIMESTEPS, torch.full((1,), 1.0)) + xy_fast = decode_trajectory_to_xy(traj, NUM_TIMESTEPS, torch.full((1,), 20.0)) + assert not torch.allclose(xy_slow, xy_fast) + + def test_speed_row_count_mismatch_raises(self): + with pytest.raises(ValueError, match="initial_speed"): + decode_trajectory_to_xy( + torch.zeros(BATCH, NUM_TIMESTEPS * 2), NUM_TIMESTEPS, + torch.full((BATCH + 1,), DEFAULT_SPEED), + ) + + +class TestProjectXYToBEVPixel: + def test_ego_origin_maps_correctly(self): + cfg = ScorerConfig() + px = project_xy_to_bev_pixel(torch.zeros(1, 2), cfg) + assert px[0, 0].item() == cfg.ego_row + assert px[0, 1].item() == cfg.ego_col + + def test_forward_motion_reduces_row(self): + cfg = ScorerConfig(forward_is_negative_row=True) + px = project_xy_to_bev_pixel(torch.tensor([[10.0, 0.0]]), cfg) + assert px[0, 0].item() < cfg.ego_row + + +class TestDrivableAreaCompliance: + def test_oob_reduces_compliance(self, map_input): + cfg = ScorerConfig() + traj = torch.zeros(BATCH, 1, 2) + traj[0, 0] = torch.tensor([10000.0, 10000.0]) + dac = drivable_area_compliance(traj, map_input, cfg) + assert dac[0].item() < 1.0 + assert dac[1].item() == 1.0 + + def test_range_zero_to_one(self, map_input): + cfg = ScorerConfig() + traj = torch.randn(BATCH, NUM_TIMESTEPS, 2) * 10 + dac = drivable_area_compliance(traj, map_input, cfg) + assert ((dac >= 0.0) & (dac <= 1.0)).all() + + +class TestKinematicComfortScore: + def test_no_violations_scores_one(self): + cfg = ScorerConfig(max_comfortable_accel=100.0, max_comfortable_lateral_accel=100.0) + speed = torch.full((BATCH,), DEFAULT_SPEED) + score = kinematic_comfort_score( + torch.zeros(BATCH, NUM_TIMESTEPS * 2), NUM_TIMESTEPS, speed, cfg) + assert torch.allclose(score, torch.ones(BATCH)) + + def test_all_violations_scores_zero(self): + cfg = ScorerConfig(max_comfortable_accel=0.0, max_comfortable_lateral_accel=0.0) + speed = torch.full((BATCH,), DEFAULT_SPEED) + score = kinematic_comfort_score( + torch.ones(BATCH, NUM_TIMESTEPS * 2), NUM_TIMESTEPS, speed, cfg) + assert torch.allclose(score, torch.zeros(BATCH)) + + +class TestTrajectoryComplianceScorer: + def test_output_shapes(self, scorer, bev_features, visual_history, egomotion_history, map_input): + traj, scores = scorer.sample_and_score( + bev_features, visual_history, egomotion_history, map_input, num_samples=5, seed=42) + assert traj.shape == (BATCH, NUM_TIMESTEPS * 2) + assert scores.shape == (BATCH, 5) + + def test_mean_selection(self, planner, bev_features, visual_history, egomotion_history, map_input): + cfg = ScorerConfig(selection="mean") + s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) + traj, _ = s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4) + assert traj.shape == (BATCH, NUM_TIMESTEPS * 2) + + def test_invalid_selection_raises(self, planner, bev_features, visual_history, egomotion_history, map_input): + cfg = ScorerConfig(selection="bogus") + s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) + with pytest.raises(ValueError, match="config.selection"): + s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=3) + + def test_seed_reproducibility(self, scorer, bev_features, visual_history, egomotion_history, map_input): + t1, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) + t2, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) + assert torch.allclose(t1, t2) + + def test_different_seeds_differ(self, scorer, bev_features, visual_history, egomotion_history, map_input): + t1, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) + t2, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=99) + assert not torch.allclose(t1, t2) + + def test_scores_vary_across_samples(self, planner, bev_features, visual_history, egomotion_history, map_input): + cfg = ScorerConfig(dac_weight=1.0, comfort_weight=1.0) + s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) + _, scores = s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=8, seed=7) + assert scores.std(dim=1).sum() > 0 + + def test_different_initial_speeds_change_selection(self, planner, bev_features, visual_history, map_input): + """Regression guard for the original bug this file's fixtures used + to hide: a scorer wired to a fixed initial_speed can't distinguish + a scene where the ego starts at 1 m/s from one where it starts at + 25 m/s. With the real per-row speed threaded through, the decoded + (and thus scored) geometry must differ between the two.""" + cfg = ScorerConfig(dac_weight=1.0, comfort_weight=1.0) + s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) + eh_slow = _egomotion(speed=1.0) + eh_fast = _egomotion(speed=25.0) + torch.manual_seed(0) + traj_slow, _ = s.sample_and_score(bev_features, visual_history, eh_slow, map_input, num_samples=4, seed=3) + torch.manual_seed(0) + traj_fast, _ = s.sample_and_score(bev_features, visual_history, eh_fast, map_input, num_samples=4, seed=3) + assert not torch.allclose(traj_slow, traj_fast) + + +if __name__ == "__main__": + print("Running smoke test (CPU, no GPU required)...") + bev = torch.randn(BATCH, 8, 6, 6) + vh = torch.randn(BATCH, 16) + eh = _egomotion() + mp = torch.zeros(BATCH, 3, BEV_H, BEV_W) + mp[:, :, 100:300, 100:200] = 255.0 + sc = TrajectoryComplianceScorer(FakePlanner(NUM_TIMESTEPS), NUM_TIMESTEPS) + traj, scores = sc.sample_and_score(bev, vh, eh, mp, num_samples=6, seed=42) + print(f" trajectory: {tuple(traj.shape)}") + print(f" scores: {tuple(scores.shape)}") + print(f" score range: {scores.min().item():.3f} - {scores.max().item():.3f}") + print("PASSED.") diff --git a/PROPOSAL_diffusion_driving_policy.md b/PROPOSAL_diffusion_driving_policy.md deleted file mode 100644 index 4f30214bf..000000000 --- a/PROPOSAL_diffusion_driving_policy.md +++ /dev/null @@ -1,178 +0,0 @@ -# Proposal: BEV-only trajectory scoring + few-step inference upgrade for `FlowMatchingPlanner` - -> Per CONTRIBUTING.md, this is posted as a Discussion before any PR, since -> it touches the trajectory planning module. References PR #40 (driving -> policy / FlowMatchingPlanner), PR #55 (map encoder / MapBEVFusion), issue -> #35 (BEV resolution / hardware target), issue #66 (open-loop eval -> pipeline), and the GoalFlow paper (arXiv:2503.05689) discussed in the -> 17/06 WG meeting. - -## Context - -`FlowMatchingPlanner` (merged in #40) already does conditional flow -matching with BEV cross-attention and AdaLN modulation, and the map -pipeline (#55) already fuses a rasterized nav-map into that BEV before the -planner runs. What's still missing relative to GoalFlow's design — which -we discussed in detail this week — is everything downstream of trajectory -*generation*: GoalFlow doesn't just sample one trajectory, it samples many, -scores them against a goal-point vocabulary and a learned drivable-area -classifier, and picks the best one. - -We were told to get BEV working first, before investing in the full -goal-point vocabulary + learned DAC classifier machinery GoalFlow uses. -This proposal is scoped to exactly that boundary: it adds multi-sample -generation and re-ranking using **only data we already have on disk** -(the rasterized map image, the predicted trajectory's own kinematics) — -zero new training, zero new loss terms, zero dependency on a semantic BEV -head or goal-point clustering that doesn't exist yet. - -## What this proposal adds (Phase 1) - -**1. `TrajectoryComplianceScorer`** — a wrapper around any `BasePlanner` -that draws `K` samples per scene (vectorized via batch-repeat, one -`forward()` call) and re-ranks them by: - -- **Drivable-area compliance**, read directly off the *raw rasterized* - `map_input` pixels at each waypoint's projected BEV coordinate. This is - a colour lookup, not a classifier — it stands in for GoalFlow's learned - DAC score until (if) we build a real semantic BEV head. -- **Kinematic comfort**, penalizing samples whose (acceleration, - curvature) sequence exceeds configurable comfort bounds. - -Selection mirrors GoalFlow's own two modes: `"nearest"` (argmax — pick the -single best-scoring sample) or `"mean"` (softmax-weighted blend across all -K samples). - -**2. Shifted inference-time timestep schedule** for `FlowMatchingPlanner` -— a small, additive change (`timestep_schedule="uniform"|"shifted"`, -default `"uniform"` preserves exact current behaviour) implementing the -same `t_shifted = (alpha·t)/(1+(alpha-1)·t)` warp GoalFlow uses at -inference, which their own ablation shows matters most at low step counts -— directly relevant to our Renesas R-Car deployment target and the -CPU/GPU latency work already in the benchmark thread. - -## What this proposal deliberately does NOT add (Phase 2 — deferred) - -- No goal-point vocabulary or offline-cached goal scores (GoalFlow's - `cluster_points_8192_.npy` + `goal_point_scores.gz` equivalent). -- No learned BEV semantic segmentation head (GoalFlow's - `_bev_semantic_head`) — Phase 1's drivable-area check is a raw colour - lookup precisely so we don't need one yet. -- No classifier-free-guidance-style goal-conditioned/unconditioned fusion. - -These are real GoalFlow ideas worth revisiting once KITScenes' HD map -gives us a reliable semantic drivable-area channel to train against — but -building the goal-point + DAC-classifier machinery before we have that -signal would mean training it against the same colour-lookup proxy this -proposal already gives us for free, which isn't worth the added -complexity yet. - -## Calibration caveat (needs a reviewer who owns the map renderer) - -`project_xy_to_bev_pixel` in the attached code needs `pixels_per_meter`, -`ego_row`, and `ego_col` confirmed against whatever convention the -KITScenes/L2D map renderer actually uses to produce `map_input`. The -defaults follow the 120 m front / 60 m rear / 60 m each side @ 0.4 m -geometry from issue #35, but I have not verified them against the -renderer itself — would appreciate a second pair of eyes here (cc Richard -/ Zain) before this gets merged, since a miscalibrated lookup would -silently score every trajectory as "compliant" or "non-compliant" -regardless of where it actually goes. - -## Sensor scope — camera + map only, no LiDAR (deliberate, not a gap) - -Worth stating explicitly rather than leaving implicit: this proposal, like -the rest of `auto_e2e` today, uses only camera tiles and the rasterized -map image — `AutoE2E.forward()` has no LiDAR input anywhere in its -signature, and nothing here changes that. - -It is worth separating two things that both get called "BEV" in this -discussion, since GoalFlow conflates them in a way that could cause -confusion later. GoalFlow's BEV comes from fusing camera features with a -*live LiDAR point cloud* (their camera backbone plus a separate LiDAR -encoder) — a runtime perception sensor that gives direct range -measurement. Our map-BEV is different in kind: it is a rendering of a -*pre-surveyed HD map* (KITScenes' vectorized map data), built offline, not -measured live. Skipping LiDAR does not cost us the map signal at all — -that signal was never LiDAR-dependent in the first place, and `map_input` -already reaches the planner today through `MapBEVFusion`. - -What skipping LiDAR does cost us is direct range measurement for dynamic -obstacles (other vehicles, pedestrians, cyclists). Without LiDAR, that has -to come entirely from camera-based depth and 3D understanding, which is -the harder and less metrically reliable half of the perception problem. -That trade-off is consistent with the project's existing all-camera -architecture and the Renesas R-Car embedded deployment target, so it is -not a new decision this proposal is introducing — but the WG's own scope -for the Driving Model Team lists "cameras, LIDAR/RADAR" as the intended -sensor suite, so this should be treated as a deliberate, visible -simplification for now rather than something that quietly becomes -permanent because nobody revisited it. - -Genuinely open to other framings here, or to being told this trade-off -analysis is missing something — flagging it explicitly so the group can -weigh in rather than letting it default silently into "how things are." - -## Files in the attached PR - -| File | Status | -|------|--------| -| `Model/model_components/trajectory_planning/trajectory_scorer.py` | New | -| `Model/model_components/trajectory_planning/flow_matching_planner.py` | Modified (additive, see patch notes) | - -Zero changes to `auto_e2e.py`, `base.py`, `gru_planner.py`, or any existing -public call site. `TrajectoryComplianceScorer` is opt-in — nothing wires it -into the default forward pass automatically, since that's a separate -design decision (does the scorer live inside `AutoE2E.forward()` behind a -config flag, or stay a standalone post-processing step callers opt into?) -that probably deserves its own discussion once Phase 1 lands and we have -real numbers from it. - -## Testing status - -**Smoke test (CPU, zero-GPU, no KITScenes data required):** Passed locally -against a fake `BasePlanner` that satisfies the `BasePlanner` contract. -Covers shape correctness, out-of-bounds waypoint handling, both selection -modes (`nearest` / `mean`), and the invalid-selection-mode error path. -This test does not require a GPU or the actual trained model and can be -reproduced on any machine with PyTorch installed. - -**Quality / integration test (does it actually improve ADE?):** Not yet run. -This requires a trained `FlowMatchingPlanner` checkpoint and parsed -KITScenes map+camera data feeding real `map_input` tensors — both of -which are currently inaccessible due to hardware constraints on the -contributor's machine for the next few days (GPU unavailable). This test -is also contingent on the map-pixel calibration review flagged above, since -a miscalibrated `ego_row` / `ego_col` would make the DAC score meaningless -and render any ADE comparison uninformative. - -**What this means for the PR:** the code is structurally sound and the -`BasePlanner` contract is fully satisfied, but the PR should be treated as -ready for code review and calibration review, not ready to merge until -quality validation on real data is confirmed. Will update the PR once -hardware is available. - -## Open questions for the group - -1. Where should `TrajectoryComplianceScorer` actually plug in — - `AutoE2E.forward()` behind a flag, or kept as a standalone utility - callers use explicitly (e.g. only at eval/inference time, never during - training)? -2. Is `num_samples` (K) something we want exposed as a runtime knob for - the Renesas target, where compute budget is tight, vs always running - at a fixed K? -3. Does the comfort-bound scoring belong here at all, or should it instead - become an auxiliary *training* loss term on `FlowMatchingPlanner` - directly (closer to how GoalFlow's own ablation table treats each - signal as a separate, addable loss)? -4. Is camera+map-only the right scope for this phase, or is there a - lighter-weight way to bring RADAR or LiDAR range data in earlier than - planned, given the Driving Model Team's stated longer-term sensor - suite? Open to being told there's a better way to sequence this than - what's proposed here. - -This is a first pass, not a final design — alternative approaches, -different scoring signals, or a different Phase 1/Phase 2 split are all -welcome. Posting it now mainly to get the scoping question (what needs -goal-point/LiDAR infrastructure we don't have yet, vs what doesn't) in -front of the group before writing more code against it. diff --git a/tests/test_trajectory_scorer.py b/tests/test_trajectory_scorer.py deleted file mode 100644 index 0deb4214f..000000000 --- a/tests/test_trajectory_scorer.py +++ /dev/null @@ -1,169 +0,0 @@ -"""Smoke test for TrajectoryComplianceScorer. - -Runs entirely on CPU with a fake BasePlanner — no trained model, -no KITScenes data, no GPU required. Safe to run locally even when -hardware is constrained. - -Usage from repo root: - pytest tests/test_trajectory_scorer.py -v -or: - python tests/test_trajectory_scorer.py -""" - -import torch -import torch.nn as nn -import pytest - -from Model.model_components.trajectory_planning.trajectory_scorer import ( - ScorerConfig, - TrajectoryComplianceScorer, - decode_trajectory_to_xy, - drivable_area_compliance, - kinematic_comfort_score, - project_xy_to_bev_pixel, -) - -NUM_TIMESTEPS = 4 -BATCH = 2 -BEV_H, BEV_W = 450, 300 - - -class FakePlanner(nn.Module): - def __init__(self, num_timesteps=4, num_signals=2, embed_dim=8): - super().__init__() - self.trajectory_dim = num_timesteps * num_signals - self.embed_dim = embed_dim - - def forward(self, bev_features, visual_history, egomotion_history, - generator=None, **kwargs): - B = bev_features.shape[0] - traj = torch.randn(B, self.trajectory_dim, generator=generator) - ego_hidden = bev_features.flatten(1)[:, :self.embed_dim] - return traj, ego_hidden - - -@pytest.fixture() -def bev_features(): return torch.randn(BATCH, 8, 6, 6) -@pytest.fixture() -def visual_history(): return torch.randn(BATCH, 16) -@pytest.fixture() -def egomotion_history(): return torch.randn(BATCH, 12) -@pytest.fixture() -def map_input(): - m = torch.zeros(BATCH, 3, BEV_H, BEV_W) - m[:, :, 100:300, 100:200] = 255.0 - return m -@pytest.fixture() -def planner(): return FakePlanner(num_timesteps=NUM_TIMESTEPS) -@pytest.fixture() -def scorer(planner): return TrajectoryComplianceScorer(planner, num_timesteps=NUM_TIMESTEPS) - - -class TestDecodeTrajectoryToXY: - def test_output_shape(self): - xy = decode_trajectory_to_xy(torch.zeros(BATCH, NUM_TIMESTEPS * 2), NUM_TIMESTEPS) - assert xy.shape == (BATCH, NUM_TIMESTEPS, 2) - - def test_zero_curvature_stays_straight(self): - xy = decode_trajectory_to_xy(torch.zeros(1, NUM_TIMESTEPS * 2), NUM_TIMESTEPS) - assert torch.allclose(xy[0, :, 1], torch.zeros(NUM_TIMESTEPS), atol=1e-5) - assert (xy[0, 1:, 0] > xy[0, :-1, 0]).all() - - def test_extreme_deceleration_does_not_crash(self): - traj = torch.full((1, NUM_TIMESTEPS * 2), -1000.0) - xy = decode_trajectory_to_xy(traj, NUM_TIMESTEPS) - assert not torch.isnan(xy).any() and not torch.isinf(xy).any() - - -class TestProjectXYToBEVPixel: - def test_ego_origin_maps_correctly(self): - cfg = ScorerConfig() - px = project_xy_to_bev_pixel(torch.zeros(1, 2), cfg) - assert px[0, 0].item() == cfg.ego_row - assert px[0, 1].item() == cfg.ego_col - - def test_forward_motion_reduces_row(self): - cfg = ScorerConfig(forward_is_negative_row=True) - px = project_xy_to_bev_pixel(torch.tensor([[10.0, 0.0]]), cfg) - assert px[0, 0].item() < cfg.ego_row - - -class TestDrivableAreaCompliance: - def test_oob_reduces_compliance(self, map_input): - cfg = ScorerConfig() - traj = torch.zeros(BATCH, 1, 2) - traj[0, 0] = torch.tensor([10000.0, 10000.0]) - dac = drivable_area_compliance(traj, map_input, cfg) - assert dac[0].item() < 1.0 - assert dac[1].item() == 1.0 - - def test_range_zero_to_one(self, map_input): - cfg = ScorerConfig() - traj = torch.randn(BATCH, NUM_TIMESTEPS, 2) * 10 - dac = drivable_area_compliance(traj, map_input, cfg) - assert ((dac >= 0.0) & (dac <= 1.0)).all() - - -class TestKinematicComfortScore: - def test_no_violations_scores_one(self): - cfg = ScorerConfig(max_comfortable_accel=100.0, max_comfortable_lateral_accel=100.0) - score = kinematic_comfort_score(torch.zeros(BATCH, NUM_TIMESTEPS * 2), NUM_TIMESTEPS, cfg) - assert torch.allclose(score, torch.ones(BATCH)) - - def test_all_violations_scores_zero(self): - cfg = ScorerConfig(max_comfortable_accel=0.0, max_comfortable_lateral_accel=0.0) - score = kinematic_comfort_score(torch.ones(BATCH, NUM_TIMESTEPS * 2), NUM_TIMESTEPS, cfg) - assert torch.allclose(score, torch.zeros(BATCH)) - - -class TestTrajectoryComplianceScorer: - def test_output_shapes(self, scorer, bev_features, visual_history, egomotion_history, map_input): - traj, ego_hidden, scores = scorer.sample_and_score( - bev_features, visual_history, egomotion_history, map_input, num_samples=5, seed=42) - assert traj.shape == (BATCH, NUM_TIMESTEPS * 2) - assert ego_hidden.shape == (BATCH, 8) - assert scores.shape == (BATCH, 5) - - def test_mean_selection(self, planner, bev_features, visual_history, egomotion_history, map_input): - cfg = ScorerConfig(selection="mean") - s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) - traj, _, _ = s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4) - assert traj.shape == (BATCH, NUM_TIMESTEPS * 2) - - def test_invalid_selection_raises(self, planner, bev_features, visual_history, egomotion_history, map_input): - cfg = ScorerConfig(selection="bogus") - s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) - with pytest.raises(ValueError, match="config.selection"): - s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=3) - - def test_seed_reproducibility(self, scorer, bev_features, visual_history, egomotion_history, map_input): - t1, _, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) - t2, _, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) - assert torch.allclose(t1, t2) - - def test_different_seeds_differ(self, scorer, bev_features, visual_history, egomotion_history, map_input): - t1, _, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) - t2, _, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=99) - assert not torch.allclose(t1, t2) - - def test_scores_vary_across_samples(self, planner, bev_features, visual_history, egomotion_history, map_input): - cfg = ScorerConfig(dac_weight=1.0, comfort_weight=1.0) - s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) - _, _, scores = s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=8, seed=7) - assert scores.std(dim=1).sum() > 0 - - -if __name__ == "__main__": - print("Running smoke test (CPU, no GPU required)...") - bev = torch.randn(BATCH, 8, 6, 6) - vh = torch.randn(BATCH, 16) - eh = torch.randn(BATCH, 12) - mp = torch.zeros(BATCH, 3, BEV_H, BEV_W) - mp[:, :, 100:300, 100:200] = 255.0 - sc = TrajectoryComplianceScorer(FakePlanner(NUM_TIMESTEPS), NUM_TIMESTEPS) - traj, ego_h, scores = sc.sample_and_score(bev, vh, eh, mp, num_samples=6, seed=42) - print(f" trajectory: {tuple(traj.shape)}") - print(f" ego_hidden: {tuple(ego_h.shape)}") - print(f" scores: {tuple(scores.shape)}") - print(f" score range: {scores.min().item():.3f} – {scores.max().item():.3f}") - print("PASSED.") From df6160a4faca76509bbf7d645b0e66d1943d28d6 Mon Sep 17 00:00:00 2001 From: FLagbusted Date: Wed, 29 Jul 2026 02:43:09 +0000 Subject: [PATCH 4/4] fix(trajectory_scorer): rebuild drivable-area check on real geometry, not guessed calibration (#161) #149/#148 landed via #161 (route-conditioned navigation inputs) while this PR was waiting on calibration. That changes what needs fixing here from 'verify a guessed calibration' to 'the input format itself changed': map_input (a rendered RGB image) is now map_context, a 14-channel semantic raster (Model.navigation.geometry.MapChannel) -- there is no pixel colour to fuzzy-match against any more. Per riita10069's comment on #17 (BEV segmentation labels are being redirected to a separate PandaSet pretraining track, not landing on KITScenes route-training soon), this can't wait for learned segmentation labels either -- rebuilt directly on what #161 already provides: - project_xy_to_bev_pixel now reimplements NavigationRasterGeometry.ego_to_pixel's exact formula (same signs, same -0.5 pixel-center offset) in torch, instead of a hand-derived formula against guessed constants. Cross-checked against the real geometry object directly in tests, not just self-consistency. - ScorerConfig's calibration fields are now sourced directly from Model.navigation.geometry.DEFAULT_NAVIGATION_GEOMETRY (geometry_id=kitscenes-v3-bev-1m-v1) -- the actual values map_context is rasterized with, not a placeholder. - drivable_area_compliance reads map_context's DRIVABLE_AREA channel (a real binary 0/1 mask, one of BINARY_MAP_CHANNELS) directly, replacing the RGB-tolerance colour-distance heuristic entirely. - Renamed the scorer's own map_input param to map_context throughout, matching ReactiveE2E.forward()'s naming post-#161. Rebased onto main post-#161 -- clean, no conflicts (this branch's only overlap with #161's changes was via flow_matching_planner.py, and sample_k_trajectories doesn't touch anything #161 changed). Tests rewritten against the new 14-channel shape and real geometry: added a direct cross-check against DEFAULT_NAVIGATION_GEOMETRY.ego_to_pixel on both the origin and random points (not just internal self-consistency), and a test proving compliance now reads the binary channel rather than any color-adjacent heuristic. 22/22 passing, full test_trajectory_planning.py still 38 passed/1 skipped. ruff clean. Signed-off-by: FLagbusted --- .../trajectory_planning/trajectory_scorer.py | 130 +++++++++--------- Model/tests/test_trajectory_scorer.py | 111 ++++++++++----- 2 files changed, 140 insertions(+), 101 deletions(-) diff --git a/Model/model_components/trajectory_planning/trajectory_scorer.py b/Model/model_components/trajectory_planning/trajectory_scorer.py index 03cd49e47..c7f7dcbb1 100644 --- a/Model/model_components/trajectory_planning/trajectory_scorer.py +++ b/Model/model_components/trajectory_planning/trajectory_scorer.py @@ -27,22 +27,14 @@ or planner stack — it only changes *how many* samples are drawn from an already-trained stochastic planner and *how* the best one is picked. -Calibration note: `pixels_per_meter`, `ego_row`, and `ego_col` below MUST be -set to match whatever convention the KITScenes / L2D map renderer actually -uses to produce `map_input`. The defaults here follow the BEV geometry -discussed for this fork (120 m front / 60 m rear / 60 m each side at 0.4 m -resolution -> 450 x 300 px, issue #35) but have NOT been verified against -the renderer itself — confirm with whoever owns that code before relying -on the compliance score in any reported metric. - -DO NOT calibrate against Model/data_parsing/kit_scenes/map.py as it stands -today without checking #148 and #149 first: #149 proposes replacing the -current 640x360 non-square render with a 256x256 square tile at 120 m in -*all four* directions (symmetric), and #148 is reworking what the map -actually encodes (route direction from GPS traces, not just a static -drivable-area raster). Both are open and assigned to riita10069 as of -2026-07-21 — the geometry below may need to change again once those land, -not just be verified against today's renderer. +Calibration: `meters_per_pixel`, `ego_row`, `ego_col` below come directly +from `Model.navigation.geometry.DEFAULT_NAVIGATION_GEOMETRY` — the actual +geometry `map_context` is rasterized with (#161, merged, resolved #148/#149). +This is no longer a guess to verify; it's imported from the same source of +truth the renderer itself uses. Drivable-area compliance reads +`map_context`'s `MapChannel.DRIVABLE_AREA` channel directly — a real +binary semantic mask, not a pixel-colour heuristic on a human-viewable +image (map_context is 14 semantic channels now, not a rendered RGB image). """ from dataclasses import dataclass @@ -53,31 +45,35 @@ import torch.nn as nn from Model.evaluation.metrics import integrate_trajectory +from Model.navigation.geometry import DEFAULT_NAVIGATION_GEOMETRY, MapChannel @dataclass class ScorerConfig: - # --- BEV pixel-space calibration (see calibration note above) --- - pixels_per_meter: float = 2.5 # 1 / 0.4 m - bev_h: int = 450 # rows: forward axis - bev_w: int = 300 # cols: lateral axis - ego_row: int = 300 # row index where ego (x=0) sits - # (150m*2.5=375 if symmetric; the - # 120m-front/60m-rear split used - # in issue #35 gives 60*2.5=150 from - # the *bottom*, i.e. row 450-150=300 - # from the top if rendered front-up). - ego_col: int = 150 # col index where ego (y=0) sits - # (lateral center: 300 / 2) - forward_is_negative_row: bool = True # True if increasing x (forward) - # moves to smaller row indices - # (image rendered nose-up). - - # --- drivable-area colour lookup --- - # RGB tuple(s) considered "drivable" in the rendered map_input image. - # Confirm against the actual renderer palette before use. - drivable_rgb: tuple = (255, 255, 255) - drivable_rgb_tolerance: int = 10 # per-channel L1 tolerance + # --- BEV pixel-space calibration --- + # Sourced directly from Model.navigation.geometry.DEFAULT_NAVIGATION_GEOMETRY + # (geometry_id="kitscenes-v3-bev-1m-v1") — the same geometry map_context + # is actually rasterized with, not a guessed/reverse-engineered value. + # This resolves the #148/#149 calibration risk this module used to warn + # about: those issues are merged (see #161), and this is their real + # output geometry, not a placeholder. + meters_per_pixel: float = DEFAULT_NAVIGATION_GEOMETRY.meters_per_pixel + bev_h: int = DEFAULT_NAVIGATION_GEOMETRY.height_px + bev_w: int = DEFAULT_NAVIGATION_GEOMETRY.width_px + x_max_m: float = DEFAULT_NAVIGATION_GEOMETRY.x_max_m + y_max_m: float = DEFAULT_NAVIGATION_GEOMETRY.y_max_m + ego_row: float = DEFAULT_NAVIGATION_GEOMETRY.ego_anchor_row + ego_col: float = DEFAULT_NAVIGATION_GEOMETRY.ego_anchor_col + + # --- drivable-area lookup --- + # map_context's DRIVABLE_AREA channel (Model.navigation.geometry.MapChannel) + # is one of BINARY_MAP_CHANNELS — a real semantic 0/1 mask, not a pixel + # colour to fuzzy-match. Replaces the old RGB-tolerance heuristic + # entirely: map_context is 14 semantic channels now (#161), not a + # human-viewable rasterized image, so there is no colour to match + # against any more. + drivable_area_channel: int = int(MapChannel.DRIVABLE_AREA) + drivable_threshold: float = 0.5 # --- kinematic comfort bounds --- max_comfortable_accel: float = 3.0 # m/s^2 @@ -183,6 +179,14 @@ def decode_trajectory_to_xy(trajectory: torch.Tensor, num_timesteps: int, def project_xy_to_bev_pixel(xy: torch.Tensor, config: ScorerConfig) -> torch.Tensor: """Project ego-relative (x, y) meters into BEV pixel (row, col) indices. + Mirrors NavigationRasterGeometry.ego_to_pixel's formula exactly + (Model/navigation/geometry.py) — same sign conventions, same -0.5 + pixel-center offset — so a trajectory scored here and the same + trajectory rendered through the real geometry object land on the same + pixel. Reimplemented in torch (not called directly) only to stay + differentiable/GPU-resident for the batched xy tensor this receives; + the arithmetic is identical, not independently derived. + Args: xy: [..., 2] ego-relative coordinates in meters (x=forward, y=left). config: calibration parameters — see module docstring. @@ -193,33 +197,31 @@ def project_xy_to_bev_pixel(xy: torch.Tensor, config: ScorerConfig) -> torch.Ten (see `drivable_area_compliance`). """ x, y = xy[..., 0], xy[..., 1] - row_offset = -x if config.forward_is_negative_row else x - row = config.ego_row + row_offset * config.pixels_per_meter - col = config.ego_col - y * config.pixels_per_meter # +y = left = smaller col + row = (config.x_max_m - x) / config.meters_per_pixel - 0.5 + col = (config.y_max_m - y) / config.meters_per_pixel - 0.5 return torch.stack([row, col], dim=-1).round().long() -def drivable_area_compliance(xy: torch.Tensor, map_input: torch.Tensor, +def drivable_area_compliance(xy: torch.Tensor, map_context: torch.Tensor, config: ScorerConfig) -> torch.Tensor: - """Fraction of trajectory waypoints landing on a "drivable" map pixel. + """Fraction of trajectory waypoints landing on the drivable-area mask. Args: xy: [B, num_timesteps, 2] ego-relative waypoints in meters. - map_input: [B, 3, bev_h, bev_w] rasterized BEV map image, the same - tensor fed to RasterizedMapEncoder (channel order assumed RGB, - values in [0, 255] or normalized — see note below). + map_context: [B, 14, bev_h, bev_w] semantic navigation raster (#161) + — the same tensor ReactiveE2E.forward() takes as map_context. + Channel config.drivable_area_channel (MapChannel.DRIVABLE_AREA) + is a real binary 0/1 mask (Model.navigation.geometry. + BINARY_MAP_CHANNELS), not a pixel colour — no ImageNet + normalization or colour-space concern applies here the way it + did for the old rendered-RGB map_input. config: calibration parameters. Returns: - compliance: [B] fraction in [0, 1] of waypoints inside the - drivable-area colour band and within image bounds. - - Note: if `map_input` has already been ImageNet-normalized upstream of - this call, the colour lookup must run on a separate un-normalized copy - of the map image — wire this scorer to whichever stage in the data - pipeline still has raw pixel values. + compliance: [B] fraction in [0, 1] of waypoints on a drivable + pixel and within the raster's bounds. """ - B, _, H, W = map_input.shape + B, _, H, W = map_context.shape T = xy.shape[1] pixels = project_xy_to_bev_pixel(xy, config) # [B, T, 2] @@ -229,14 +231,11 @@ def drivable_area_compliance(xy: torch.Tensor, map_input: torch.Tensor, rows_c = rows.clamp(0, H - 1) cols_c = cols.clamp(0, W - 1) - target = torch.tensor(config.drivable_rgb, device=map_input.device, - dtype=map_input.dtype).view(1, 1, 3) + drivable = map_context[:, config.drivable_area_channel] # [B, H, W] - compliant = torch.zeros(B, T, dtype=torch.bool, device=map_input.device) + compliant = torch.zeros(B, T, dtype=torch.bool, device=map_context.device) for b in range(B): - sampled = map_input[b, :, rows_c[b], cols_c[b]].transpose(0, 1) # [T, 3] - diff = (sampled - target[0]).abs().sum(dim=-1) - compliant[b] = diff <= (3 * config.drivable_rgb_tolerance) + compliant[b] = drivable[b, rows_c[b], cols_c[b]] > config.drivable_threshold compliant = compliant & in_bounds return compliant.float().mean(dim=1) # [B] @@ -304,7 +303,7 @@ def __init__(self, planner: nn.Module, num_timesteps: int, def sample_and_score(self, bev_features: torch.Tensor, visual_history: torch.Tensor, egomotion_history: torch.Tensor, - map_input: torch.Tensor, + map_context: torch.Tensor, num_samples: int = 8, seed: Optional[int] = None): """Draw `num_samples` trajectories per batch element and re-rank. @@ -314,9 +313,12 @@ def sample_and_score(self, bev_features: torch.Tensor, already produced by AutoE2E before the planner call. visual_history: [B, visual_history_dim]. egomotion_history: [B, egomotion_dim]. - map_input: [B, 3, bev_h, bev_w] raw (un-normalized) rasterized - map image — see `drivable_area_compliance` note on - normalization. + map_context: [B, 14, bev_h, bev_w] semantic navigation raster + (#161) — see `drivable_area_compliance`. Renamed from + map_input to match ReactiveE2E.forward()'s own naming + after #161; this is no longer a human-viewable rendered + image, so the old name (implying a raw image to feed + a CNN) was misleading. num_samples: K, number of stochastic samples per batch element. seed: optional base seed for reproducible re-sampling. @@ -363,7 +365,7 @@ def sample_and_score(self, bev_features: torch.Tensor, ) # [B, K, T, 2] dac_scores = torch.stack([ - drivable_area_compliance(xy[:, k], map_input, self.config) + drivable_area_compliance(xy[:, k], map_context, self.config) for k in range(num_samples) ], dim=1) # [B, K] diff --git a/Model/tests/test_trajectory_scorer.py b/Model/tests/test_trajectory_scorer.py index f41d87b1b..2e3908eab 100644 --- a/Model/tests/test_trajectory_scorer.py +++ b/Model/tests/test_trajectory_scorer.py @@ -12,6 +12,7 @@ import torch import torch.nn as nn +import numpy as np import pytest from Model.model_components.trajectory_planning.trajectory_scorer import ( @@ -23,10 +24,14 @@ kinematic_comfort_score, project_xy_to_bev_pixel, ) +from Model.navigation.geometry import DEFAULT_NAVIGATION_GEOMETRY, MapChannel NUM_TIMESTEPS = 4 BATCH = 2 -BEV_H, BEV_W = 450, 300 +# Real geometry (#161), not a guessed shape — see ScorerConfig's own defaults. +BEV_H = DEFAULT_NAVIGATION_GEOMETRY.height_px +BEV_W = DEFAULT_NAVIGATION_GEOMETRY.width_px +NAV_CHANNELS = 14 # Model.navigation.geometry.MAP_CHANNEL_COUNT DEFAULT_SPEED = 5.0 # m/s, used to build realistic-looking egomotion fixtures @@ -69,16 +74,18 @@ def visual_history(): return torch.randn(BATCH, 16) @pytest.fixture() def egomotion_history(): return _egomotion() @pytest.fixture() -def map_input(): - # ScorerConfig defaults put ego_row at 300 — the drivable rectangle - # must include that row or the ego origin itself reads as - # non-drivable (this fixture originally stopped at row 300 exclusive, - # a one-pixel-short rectangle that made the "origin should always be - # compliant" assumption below false; never caught because this file - # lived at repo-root tests/, outside what `make test` / CI actually - # collects — see Model/pytest.ini + Makefile's `test:` target). - m = torch.zeros(BATCH, 3, BEV_H, BEV_W) - m[:, :, 100:301, 100:200] = 255.0 +def map_context(): + """[B, 14, H, W] semantic navigation raster (#161) — NOT a rendered + RGB image any more. Only the DRIVABLE_AREA channel is set here; the + other 13 channels are left zero since nothing under test reads them. + A generous drivable rectangle is used so the ego origin (which the + tests below rely on being compliant) is safely inside it — this file + used to ship a rectangle that was exactly one row short of the ego + row, an off-by-one that went uncaught for a while (see git history); + generous margins here are deliberate, not laziness. + """ + m = torch.zeros(BATCH, NAV_CHANNELS, BEV_H, BEV_W) + m[:, MapChannel.DRIVABLE_AREA, 50:BEV_H, 50:BEV_W - 50] = 1.0 return m @pytest.fixture() def planner(): return FakePlanner(num_timesteps=NUM_TIMESTEPS) @@ -134,32 +141,62 @@ def test_speed_row_count_mismatch_raises(self): class TestProjectXYToBEVPixel: def test_ego_origin_maps_correctly(self): + """Must match DEFAULT_NAVIGATION_GEOMETRY.ego_to_pixel exactly — + this is a reimplementation of that method for batched torch use, + not an independent formula, so cross-check against it directly.""" cfg = ScorerConfig() px = project_xy_to_bev_pixel(torch.zeros(1, 2), cfg) - assert px[0, 0].item() == cfg.ego_row - assert px[0, 1].item() == cfg.ego_col + expected = DEFAULT_NAVIGATION_GEOMETRY.ego_to_pixel(np.zeros((1, 2))) + assert px[0, 0].item() == round(expected[0, 0]) + assert px[0, 1].item() == round(expected[0, 1]) + assert px[0, 0].item() == round(cfg.ego_row) + assert px[0, 1].item() == round(cfg.ego_col) def test_forward_motion_reduces_row(self): - cfg = ScorerConfig(forward_is_negative_row=True) + cfg = ScorerConfig() px = project_xy_to_bev_pixel(torch.tensor([[10.0, 0.0]]), cfg) assert px[0, 0].item() < cfg.ego_row + def test_matches_real_geometry_object_on_random_points(self): + """Broader cross-check than the origin alone — random points + inside the raster bounds must match + NavigationRasterGeometry.ego_to_pixel to the pixel.""" + cfg = ScorerConfig() + pts = np.array([[10.0, -5.0], [-20.0, 30.0], [0.0, 50.0]]) + expected = DEFAULT_NAVIGATION_GEOMETRY.ego_to_pixel(pts) + got = project_xy_to_bev_pixel(torch.tensor(pts, dtype=torch.float64), cfg) + for i in range(len(pts)): + assert got[i, 0].item() == round(expected[i, 0]) + assert got[i, 1].item() == round(expected[i, 1]) + class TestDrivableAreaCompliance: - def test_oob_reduces_compliance(self, map_input): + def test_oob_reduces_compliance(self, map_context): cfg = ScorerConfig() traj = torch.zeros(BATCH, 1, 2) traj[0, 0] = torch.tensor([10000.0, 10000.0]) - dac = drivable_area_compliance(traj, map_input, cfg) + dac = drivable_area_compliance(traj, map_context, cfg) assert dac[0].item() < 1.0 assert dac[1].item() == 1.0 - def test_range_zero_to_one(self, map_input): + def test_range_zero_to_one(self, map_context): cfg = ScorerConfig() traj = torch.randn(BATCH, NUM_TIMESTEPS, 2) * 10 - dac = drivable_area_compliance(traj, map_input, cfg) + dac = drivable_area_compliance(traj, map_context, cfg) assert ((dac >= 0.0) & (dac <= 1.0)).all() + def test_reads_binary_channel_not_pixel_color(self, map_context): + """The whole point of the redesign: compliance is a channel + lookup, not a colour-distance heuristic. A trajectory sitting on + a pixel where DRIVABLE_AREA=0 must score non-compliant even + though every other channel (all zero in this fixture) would have + matched an old "close to black" heuristic.""" + cfg = ScorerConfig() + traj = torch.zeros(1, 1, 2) + traj[0, 0] = torch.tensor([-200.0, 0.0]) # well outside the fixture's drivable rect, but in-bounds + dac = drivable_area_compliance(traj, map_context[:1], cfg) + assert dac[0].item() == 0.0 + class TestKinematicComfortScore: def test_no_violations_scores_one(self): @@ -178,41 +215,41 @@ def test_all_violations_scores_zero(self): class TestTrajectoryComplianceScorer: - def test_output_shapes(self, scorer, bev_features, visual_history, egomotion_history, map_input): + def test_output_shapes(self, scorer, bev_features, visual_history, egomotion_history, map_context): traj, scores = scorer.sample_and_score( - bev_features, visual_history, egomotion_history, map_input, num_samples=5, seed=42) + bev_features, visual_history, egomotion_history, map_context, num_samples=5, seed=42) assert traj.shape == (BATCH, NUM_TIMESTEPS * 2) assert scores.shape == (BATCH, 5) - def test_mean_selection(self, planner, bev_features, visual_history, egomotion_history, map_input): + def test_mean_selection(self, planner, bev_features, visual_history, egomotion_history, map_context): cfg = ScorerConfig(selection="mean") s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) - traj, _ = s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4) + traj, _ = s.sample_and_score(bev_features, visual_history, egomotion_history, map_context, num_samples=4) assert traj.shape == (BATCH, NUM_TIMESTEPS * 2) - def test_invalid_selection_raises(self, planner, bev_features, visual_history, egomotion_history, map_input): + def test_invalid_selection_raises(self, planner, bev_features, visual_history, egomotion_history, map_context): cfg = ScorerConfig(selection="bogus") s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) with pytest.raises(ValueError, match="config.selection"): - s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=3) + s.sample_and_score(bev_features, visual_history, egomotion_history, map_context, num_samples=3) - def test_seed_reproducibility(self, scorer, bev_features, visual_history, egomotion_history, map_input): - t1, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) - t2, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) + def test_seed_reproducibility(self, scorer, bev_features, visual_history, egomotion_history, map_context): + t1, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_context, num_samples=4, seed=0) + t2, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_context, num_samples=4, seed=0) assert torch.allclose(t1, t2) - def test_different_seeds_differ(self, scorer, bev_features, visual_history, egomotion_history, map_input): - t1, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=0) - t2, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=4, seed=99) + def test_different_seeds_differ(self, scorer, bev_features, visual_history, egomotion_history, map_context): + t1, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_context, num_samples=4, seed=0) + t2, _ = scorer.sample_and_score(bev_features, visual_history, egomotion_history, map_context, num_samples=4, seed=99) assert not torch.allclose(t1, t2) - def test_scores_vary_across_samples(self, planner, bev_features, visual_history, egomotion_history, map_input): + def test_scores_vary_across_samples(self, planner, bev_features, visual_history, egomotion_history, map_context): cfg = ScorerConfig(dac_weight=1.0, comfort_weight=1.0) s = TrajectoryComplianceScorer(planner, NUM_TIMESTEPS, config=cfg) - _, scores = s.sample_and_score(bev_features, visual_history, egomotion_history, map_input, num_samples=8, seed=7) + _, scores = s.sample_and_score(bev_features, visual_history, egomotion_history, map_context, num_samples=8, seed=7) assert scores.std(dim=1).sum() > 0 - def test_different_initial_speeds_change_selection(self, planner, bev_features, visual_history, map_input): + def test_different_initial_speeds_change_selection(self, planner, bev_features, visual_history, map_context): """Regression guard for the original bug this file's fixtures used to hide: a scorer wired to a fixed initial_speed can't distinguish a scene where the ego starts at 1 m/s from one where it starts at @@ -223,9 +260,9 @@ def test_different_initial_speeds_change_selection(self, planner, bev_features, eh_slow = _egomotion(speed=1.0) eh_fast = _egomotion(speed=25.0) torch.manual_seed(0) - traj_slow, _ = s.sample_and_score(bev_features, visual_history, eh_slow, map_input, num_samples=4, seed=3) + traj_slow, _ = s.sample_and_score(bev_features, visual_history, eh_slow, map_context, num_samples=4, seed=3) torch.manual_seed(0) - traj_fast, _ = s.sample_and_score(bev_features, visual_history, eh_fast, map_input, num_samples=4, seed=3) + traj_fast, _ = s.sample_and_score(bev_features, visual_history, eh_fast, map_context, num_samples=4, seed=3) assert not torch.allclose(traj_slow, traj_fast) @@ -234,8 +271,8 @@ def test_different_initial_speeds_change_selection(self, planner, bev_features, bev = torch.randn(BATCH, 8, 6, 6) vh = torch.randn(BATCH, 16) eh = _egomotion() - mp = torch.zeros(BATCH, 3, BEV_H, BEV_W) - mp[:, :, 100:300, 100:200] = 255.0 + mp = torch.zeros(BATCH, NAV_CHANNELS, BEV_H, BEV_W) + mp[:, MapChannel.DRIVABLE_AREA, 50:BEV_H, 50:BEV_W - 50] = 1.0 sc = TrajectoryComplianceScorer(FakePlanner(NUM_TIMESTEPS), NUM_TIMESTEPS) traj, scores = sc.sample_and_score(bev, vh, eh, mp, num_samples=6, seed=42) print(f" trajectory: {tuple(traj.shape)}")