Source-linked AI summary
Beyond Waypoint Regression: Query-Based Cost Learning over Reachable Ego Futures for End-to-End Driving
Ahmed Abouelazm, Rupert Polley, Qingyuan Zhang, Yin Wu, Philip Schörner, Carl Esselborn, J. Marius Zöllner
TL;DR
Waypoint regression primarily rewards expert-geometry similarity and does not explicitly distinguish safe actions from collision-causing negatives [26]. The paper learns bounded costs over reachable ego trajectories using joint scene tokens and contingency-aware refinement; it improves over prior cost-based planners while remaining competitive with regression approaches, with a safety-margin objective reducing nuScenes collision from 0.12% to 0.07% versus ST-P3-style loss.
Problem
Method
The planner estimates bounded costs for reachable ego trajectory queries using compact joint scene tokens, contingency-aware aggregation, and cost-guided intra-cluster mixing.
Source: §1 · p. 2, §3.3 · p. 5, Risk-aware Aggregation · p. 7Results
The approach improves over previous cost-based planners and remains competitive with regression-based approaches; its safety-margin objective reduces nuScenes collision from 0.12% to 0.07% versus ST-P3-style loss.
Source: §5.3 · p. 12, §6 · p. 14Takeaways & Limitations
The reachable-space cost representation remains interpretable and can be inspected, optimized, and repaired.
Source: §6 · p. 14
Abstract
from arXiv · showhide
End-to-end planners based on waypoint regression achieve strong open-loop accuracy, but they primarily learn to mimic expert geometry and remain difficult to adapt to deployment-time safety constraints. We propose a query-based cost-learning framework that estimates bounded costs for dynamically reachable ego trajectory queries, rather than dense BEV cells or a small regressed trajectory set. Compact joint scene tokens capture coherent multimodal agent futures, while contingency-aware cost aggregation and cost-guided intra-cluster MPPI mixing convert the learned cost topology into feasible ego plans. On nuScenes, our method improves over prior cost-estimation planners such as ST-P3 and NMP, outperforms most regression baselines in collision rate, while remaining competitive in L2, and retaining an interpretable cost interface. On real-world driving logs, the proposed planner reduces collision rates compared with SparseDrive and Alpamayo without fine-tuning, while maintaining a diverse set of candidate trajectories.
1 Introduction
Waypoint regression primarily rewards similarity to expert geometry, while dense cost maps supervise unreachable space and may miss coherent agent interactions [26]. The paper proposes cost learning over reachable ego trajectories, using joint scene tokens and safety-aware planning components.
1 Introduction
- Waypoint regression emphasizes expert-geometry similarity rather than distinguishing safe behavior from collision-causing negatives, complicating inspection, repair, and adaptation under distribution shift [26].Safety is often added through post-hoc rescoring of a limited top-k set.Source: §1 · pp. 1–2
- Dense cost planners such as NMP and ST-P3 supervise unreachable cells, while marginal agent modes can miss scene-consistent interactions.The paper motivates preserving cost-map optimization without dispersing supervision over irrelevant space.Source: §1 · p. 2
- The planner estimates costs over reachable ego trajectory queries rather than dense spatial grids, combining joint scene tokens, contingency-aware aggregation, and cost-guided intra-cluster mixing.This preserves cost optimization while focusing supervision on actions the vehicle can execute.Source: §1 · p. 2
- The proposed contributions include compact joint scene tokens for coherent multimodal futures and cost-guided intra-cluster mixing beyond top-k selection.A ranking-based objective uses expert trajectories, collision risk, and off-road violations to shape costs.Source: §1 · p. 2
2 Related Work
The related work connects sparse scene representations and structured planning interfaces, while noting that many systems still regress trajectories or learn costs over dense spatial regions. This paper instead scores reachable ego actions against coherent scene hypotheses.
2 Related Work
- The paper situates its approach at the intersection of sparse scene representations and structured planning, replacing direct regression with costs over reachable ego trajectories.Compact sparse tokens address scene encoding, while the cost interface provides an alternative planning formulation.Source: §2 · p. 2
2.1 Sparse and vectorized end-to-end planning.
- Sparse and vectorized planners such as VAD, SparseDrive, and SparseAD [33] improve encoding and efficiency, but commonly regress trajectories and may rely on marginal agent modes.The proposed planner conditions reachable-action costs on joint scene tokens to represent coherent multimodal hypotheses.Source: §2.1 · p. 3
2.2 Cost-based and safety-aware planning.
- Cost planners such as NMP and ST-P3 expose optimizable costs, but dense BEV supervision covers irrelevant regions and can weaken planning performance; prior IRL methods often require inference loops [30].Distance-to-expert regression can also fail to separate safe from collision-prone alternatives [26], motivating cost ordering informed by expert behavior and violations.Source: §2.2 · p. 3
3 Methodology
The planner replaces regression over a small set of ego futures with bounded cost estimation over dynamically reachable trajectory queries, conditioned on coherent joint scene futures. Risk-aware cost aggregation, cluster-based refinement, safety-aware ranking, and inference-time repair shape candidate selection and deployment-time adaptation.
3.1 Overview and Problem Formulation
- The planner formulates planning as cost estimation over dynamically reachable ego trajectories, then aggregates costs across time and scene modes to select a refined low-cost plan.The architecture uses sparse perception, joint scene prediction, a kinematic rollout sampler, and bounded query costs.Source: §3.1 · p. 4
3.2 Vectorized Map and Agent Encoding
- Sparse perception extracts compact map and agent tokens from multiview image features using instance anchors, without lifting image features into a dense BEV volume.The selected vectorized tokens provide context for joint prediction and cost estimation.Source: §3.2 · p. 4
3.3 Joint Scene Representation
- Joint scene modes represent coherent agent futures, with scene-level probabilities trained alongside mode-specific agent trajectory predictions.The joint prediction branch uses negative log-likelihood to encourage probability mass on hypotheses that explain observed joint futures.Source: §3.3 · p. 5
3.4 Reachable Ego Queries and Cost Estimation
- Intention-clustered reachable queries receive scene-conditioned costs, which are aggregated across future modes and used to refine controls into one candidate per intention.The sampler produces dynamically feasible trajectories; before a branching time, aggregation uses the worst scene-mode cost, then uses learned scene probabilities.Source: §3.4 · p. 6, Risk-aware Aggregation · p. 7
3.5 Cost Ranking and Candidate Supervision
- Safety-aware ranking raises the required cost margin for sampled trajectories that collide or leave drivable boundaries, while classification and regression supervise refined candidates.The ranking loss uses top-k violations to spread gradients across informative negatives; candidate losses preserve inter-cluster selection and pull selected candidates toward the expert.Source: Safety-aware cost ranking · p. 8
3.6 Inference-time Cost Repair
- At inference, external safety or rule signals can saturate unsafe query costs toward the maximum, and repaired costs feed the same cluster-wise mixing without retraining.Consistently penalized intention clusters become less likely to be selected.Source: §3.6 · p. 9
4 Experimental Setup
The experiments evaluate open-loop planning on nuScenes and zero-shot transfer on FZI-AURA driving logs, using a sparse perception backbone and metrics for displacement, collision, diversity, and cost separation.
Dataset and protocol
- Zero-shot transfer uses 465 FZI-AURA logs totaling 2.58 hours, collected in Germany across varied times, lighting, and traffic densities.The logs use six surround-view cameras and manually annotated 3D boxes and semantic segmentation; sensor-rig camera intrinsics and extrinsics differ slightly from nuScenes.Source: Dataset and protocol · p. 9
Implementation details
- The planner uses SparseDrive-S with 6 joint scene modes, 512 sampled ego trajectories, and 16 intention clusters of 32 samples each.Experiments train with AdamW on two NVIDIA RTX 6000 Ada GPUs; costs are bounded to [0, 100], and the safety-ranking loss averages the top 32 violations.Source: Implementation details · p. 9
Metrics and baselines
- Evaluation reports displacement error and collision rate at 1s, 2s, and 3s; real-world transfer additionally reports candidate diversity.Cost AUC and cost gap assess separation of unsafe candidates—those that collide or leave the drivable area—from safe candidates.Source: Metrics and baselines · p. 10
5 Evaluation
On nuScenes and zero-shot German driving logs, the planner improves on prior cost-based planners while balancing displacement error, collision rate, and candidate diversity. Ablations show that contingency-aware safety supervision and a combined expert-distance and safety margin shape the cost topology; qualitative cases demonstrate external-cost repair and locally road-compliant planning under a premature command.
5.1 Comparison to State of the Art
- On nuScenes open-loop planning, compared with ST-P3, average L2 error falls from 2.11m to 0.71m and average collision rate falls from 0.71% to 0.07%.Source: §5.1 · p. 10
5.2 Zero-shot Real-world Transfer
- 2.31m average L2 distance and 0.68% average collision rate versus SparseDrive-S at 4.22m and 2.73% on zero-shot German logs.Without fine-tuning, the 86.2M-parameter planner achieves the lowest collision rate, runs in real time, and maintains higher candidate diversity; Alpamayo has the lowest displacement error.Source: §5.2 · p. 10, Table 2 · p. 11
5.3 Safety Supervision and Cost Topology
- Predicted contingency risk raises L2 error slightly but reduces average collision rate from 0.12% to 0.07% compared with top-1 predicted collision supervision.The comparison holds the architecture and inference planner fixed; contingency risk includes multimodal futures, including lower-probability safety-critical ones.Source: §5.3 · pp. 11–12, Table 3 · p. 12
- The proposed safety-margin objective lowers nuScenes collision rate from 0.12% to 0.07% versus the ST-P3-style loss, while raising cost AUC from 0.79 to 0.86 and cost gap from 7.80 to 45.70.The same objective yields the lowest collision rate and largest cost gap under real-world transfer.Source: Table 4 · p. 12, §5.3 · p. 12
- Combining expert-distance, collision, and off-road terms gives the best planning tradeoff and the lowest collision rate in both nuScenes and real-world logs.Distance-only margins leave unsafe hard negatives weakly separated, while safety-only terms degrade displacement.Source: §5.3 · pp. 12–13, Table 5 · p. 13
5.4 Qualitative Results of External Cost Repair
- Applying the V2X red-light penalty before intra-cluster mixing changes the cost topology and produces safe low-cost candidates that respect the rule.SparseDrive’s proposals already enter the conflict area, so post-hoc rescoring cannot recover a rule-compliant trajectory.Source: §5.4 · p. 13
5.5 Qualitative Robustness to Misaligned Navigation Commands
- Under a premature right-turn command, the planner follows the locally safe left bend before later committing to the route intent, unlike SparseDrive, which collides with a divider.The example tests a command that is not semantically wrong but is premature for the current lane geometry.Source: §5.5 · p. 13, Figure 4 · p. 14
6 Conclusion
The work learns bounded costs over dynamically reachable ego trajectories rather than dense BEV grids, retaining an inspectable and repairable cost interface. Its evaluation reports improvements over previous cost-based planners on nuScenes and zero-shot effectiveness on FZI-AURA without fine-tuning.
6 Conclusion
- The planner learns bounded costs over dynamically reachable ego trajectories rather than dense BEV grids, shifting supervision toward motions the vehicle can execute.The cost representation can be inspected, optimized, and repaired.Source: §6 · p. 14
- Compact joint scene tokens, safety-aware ranking, and cost-guided cluster mixing support reasoning over coherent future contingencies and produce safe ego plans.Source: §6 · p. 14
- The method improves over previous cost-based planners on nuScenes, remains competitive with strong regression approaches, and transfers to FZI-AURA without fine-tuning.The cost interface also provides a mechanism for inference-time repair using external safety signals.Source: §6 · p. 14
A Additional Method Details
The appendix details how reachable ego queries are sampled, cost-guided mixing and inference turn them into plans, and how the real-world evaluation data and preprocessing are configured.
A.1 Reachable Query Sampling
- Command-conditioned intention clusters set reference speeds and heading changes, then temporally smooth control perturbations are rolled out with the kinematic model to generate reachable queries.An auxiliary slow-motion cluster improves low-speed coverage.Source: §A.1 · p. 18
A.2 Cost-Guided Intra-cluster Mixing
- Cost-guided intra-cluster mixing weights sampled control deviations around each cluster reference and rolls out the fused controls, producing one continuous candidate per intention while preserving maneuver diversity.The score can use a trajectory-level cost or timestep-level costs; the figure shows command-conditioned query sets for straight, left, and right commands.Source: §A.2 · pp. 19–21, Figure 5 · p. 20
A.3 Cost-Guided Inference and Repair Algorithm
- Inference aggregates scene-mode costs, optionally applies external penalties, mixes controls within clusters, and selects the candidate with the lowest cluster-desirability cost.External penalties can affect candidate construction and selection without retraining; fused trajectories are not re-evaluated by the learned cost head.Source: §A.3 · p. 21
A.4 Network Architecture Details.
- The shared two-layer cost head scores each trajectory, scene mode, and timestep, mapping its scalar output to bounded costs from 0 to 100.The cost-head input combines trajectory, ego-context, and scene-token embeddings.Source: §A.4 · p. 22
B.1 Real World Driving Logs Details.
- The real-world evaluation set comprises 465 FZI-AURA driving logs totaling 2.58 hours of raw data, collected across varied times, lighting conditions, and traffic densities.Each log lasts 20 seconds and contains 93,000 raw samples at 10 Hz across the full set.Source: §B.1 · p. 22
B.2 SparseDrive-format preprocessing.
- Preprocessing aligns the camera view and defines a virtual top-lidar frame; navigation commands are derived from three-second future ego displacement using lateral thresholds.The resulting commands are 8% left, 83.5% straight, and 8.5% right, with the same labels converted to text prompts for Alpamayo.Source: §B.2 · pp. 22–23
B.3 Alpamayo Inference Details.
- Alpamayo is evaluated as an imitation-oriented vision-language reference, using one front camera and the same command labels as the SparseDrive-format methods.Because the released model exposes no trajectory-level score, L2 and collision use its first decoded trajectory; 16 samples per frame are used for diversity.Source: §B.3 · p. 23
B.4 Evaluation Metric Details.
This section defines the open-loop and candidate-level evaluation measures, then reports ablations of branching time, plan selection, and planning-loss components.
B.4 Evaluation Metric Details.
- Open-loop evaluation reports L2 and cumulative collision at 1 s, 2 s, and 3 s on the final selected trajectory; real-world logs lack the same HD-map road-compliance protocol.Collision checks intersect predicted ego footprints with future annotated 3D boxes.Source: §B.4 · pp. 23–24
- Candidate diversity measures pairwise displacement across candidates over the horizon and at the endpoint, while cost AUC and cost gap assess unsafe-versus-safe cost separation.Unsafe candidates collide or leave the drivable area; higher costs should rank them above safe candidates.Source: §B.4 · pp. 24–25
C.1 Branching Time.
- At Tb = 1 s, average L2 error is 0.71 m and average collision rate is 0.07%, the lowest values among the ablated branching times.Longer worst-case intervals of two or three seconds degrade both metrics.Source: §C.1 · p. 25, Table 6 · p. 25
C.2 Plan Selection and Rescoring.
- Compared with global selection, MPPI reduces average L2 from 0.77 to 0.71 m and collision from 0.13% to 0.07%.Best-per-cluster selection reaches lower L2 (0.68 m) but a higher collision rate (0.11%); rescoring fused trajectories raises average L2 from 0.71 to 0.73 m and collision from 0.07% to 0.15%.Source: §C.2 · p. 26, Table 7 · p. 26
C.3 Planning Loss Components.
- The safety-margin ranking loss alone yields cost AUC 0.81 and cost gap 36.17, while combining ranking, classification, and regression gives the best overall behavior.Classification improves displacement error but alone does not improve collision; regression lowers collision versus ranking alone but without classification hurts displacement.Source: §C.3 · p. 26, Table 8 · p. 27
C.4 Hard-Negative Aggregation.
These ablations examine how hard negatives shape safety-ranking costs and how the cost profile guides within-cluster mixing.
C.4 Hard-Negative Aggregation.
- With ground-truth collision supervision, Top-k aggregation reduces L2 from 0.76 to 0.69 and raises cost gap from 41.67 to 46.66; with contingency collision risk, it lowers average collision from 0.13% to 0.07%.For top-1 predicted collision risk, max aggregation instead yields a lower collision rate than Top-k; contingency risk with Top-k is the reported best combination.Source: §C.4 · p. 27, Table 9 · p. 27
C.5 Cost Profile for Cluster Mixing.
- Mean trajectory cost reduces average L2 from 0.77 to 0.71 and average collision from 0.14% to 0.07% compared with per-timestep costs, despite equal cost AUC.The authors use mean trajectory cost as the default because per-timestep weighting can combine locally low-cost controls into an incoherent maneuver.Source: §C.5 · p. 28, Table 10 · p. 28
C.6 Scene-Cost Aggregation.
This ablation compares scene-mode cost aggregation strategies and reports that a temporal contingency strategy performs best across the reported metrics.
C.6 Scene-Cost Aggregation.
- The contingency strategy combines worst-case and probability-weighted aggregation over different parts of the planning horizon.The comparison includes Top-1, worst-case, mean, and probability-weighted alternatives.Source: §C.6 · p. 28
- Top-1 and probability-weighted aggregation can underemphasize low-probability safety-critical futures, whereas full-horizon worst-case aggregation can be overly conservative.Uniform mean reduces collision relative to Top-1 and worst-case aggregation but ignores the differing roles of early and late timesteps.Source: §C.6 · p. 28
- Contingency aggregation improves average L2 from 0.85–0.88 to 0.71 and reduces collision from 0.13–0.19% to 0.07%, while producing the strongest cost separation.It applies worst-case costs before the branching time and probability-weighted costs afterward.Source: §C.6 · pp. 28–29, Table 11 · p. 29
D Additional Qualitative Results
The qualitative examples show the reachable-query planner selecting low-cost ego futures in interactive nuScenes scenes and presenting meaningful cost visualizations on real-world logs under zero-shot transfer.
D Additional Qualitative Results
- nuScenes examples cover car following, yielding, lane changes, and overtaking, with selected low-cost reachable candidates consistent with road geometry and surrounding agents.Camera and BEV visualizations illustrate how the learned cost topology guides candidate selection.Source: §D.1 · p. 29, Figure 6 · p. 30
D.2 Zero-shot Real-World Examples
- Zero-shot real-world examples span stop-and-go traffic, vulnerable-road-user interactions, intersection turns, and highway following across different sensors, camera domains, and road layouts.Each row shows surround-camera inputs, BEV costs over top-k reachable candidates, and ground-truth trajectory with semantic map support; the selected trajectory is outlined in black.Source: §D.2 · p. 29, Figure 7 · p. 31