Self-driving cars and mobile robots usually find other road users with a camera and then work out where those objects sit in 3D, a step called lifting. Distance estimated from a single image is least reliable along the camera's line of sight, and that error grows quickly with range. The stages that come after, which track agents, forecast where they will go, and plan a path around them, receive a single 3D point with no error bar attached. So they treat a distant, half-occluded pedestrian as exactly as well located as a nearby car. We propose to attach a per-object uncertainty to every lifted detection at the moment it is created, shaped by the geometry of the sensor. That uncertainty then travels through tracking and forecasting and sets one concrete planner quantity: how long the robot should hedge between an agent's possible futures before committing to one.
The second use is the heart of the note. Contingency planners follow one shared trajectory until a fixed branch time and then split into one plan per predicted intent of a nearby agent. They fix that branch time by hand. The honest branch time is the moment the robot's own observations will have told the intents apart, and that moment depends directly on how noisy the observations are. Noisy far-range detections should delay commitment. Clean near-range detections should allow early commitment. Our claim is that lifting uncertainty with the right shape explains a large part of the known range-dependent overconfidence of trajectory predictors, and that feeding it into the branch time lowers closed-loop collisions without making the robot freeze.
Where this sits in prior work
Four papers motivate the gap. MTP shows that feeding real tracker output instead of ground-truth histories inflates forecast error (minADE, the average distance of the best of several forecasts) by 28× on KITTI. It answers by keeping several association hypotheses, but each hypothesis is still a deterministic box track with no geometric covariance. CCTR shows that multi-modal predictors are badly overconfident, with a U-shaped miscalibration that is worst for very near and very far agents. It fixes this with a learned, context-aware temperature scaler. It never asks where the far-range error comes from, and it works purely on bird's-eye-view coordinates. ViP3D sidesteps boxes entirely by letting learned 3D queries attend to image features. As the research file notes, though, its latent queries cannot be handed to an optimization-based planner without first being turned back into geometric shapes. That conversion brings back the same boundary error ViP3D was designed to avoid. RACP supplies the planner we build on: a Frenet-frame contingency tree with a shared trunk up to a branch time $t_b$, and a Bayesian filter over agent intents. It treats $t_b$ as a manual parameter and assumes clean predictions from a mixture of Gaussians.
The lifting literature constrains the design. Kim et al. show that 3D recall is capped by 2D recall, so covariance cannot rescue objects that were missed entirely. We therefore treat missed detections as a separate failure mode and do not claim to solve it. Feng et al. show that dense multi-view mesh refinement is too slow for online use, capped at 642 vertices. That rules out uncertainty estimates that require rendering, and it is why our covariance must be closed-form or come from a single forward pass.
Scalable SLAM enters as a compute constraint rather than a contribution. LIO-SAM keeps a sliding-window factor graph running on iSAM2, an incremental smoothing solver. We only need the ego-pose marginal covariance from that window, not a global map covariance. This keeps us inside the constant-time regime that Sibley et al. and S-Graphs 2.0 argue is necessary.
Core Claim
Each lifted detection gets a position covariance built from three parts, each computed cheaply: the detector's own 2D and depth uncertainty, the geometry of the sensor, and the robot's own pose uncertainty:
$$\Sigma_{3D} = J\,\Sigma_{img}\,J^\top + \Sigma_{ego},$$
where $\Sigma_{img}$ is the covariance of the detected image center and depth, $J$ is the Jacobian of back-projection at that pixel and depth, and $\Sigma_{ego}$ is the marginal pose covariance from the sliding-window factor graph. Because $J$ scales with depth and aligns with the viewing ray, $\Sigma_{3D}$ is elongated along the ray and grows with range.
For stereo we can predict the growth rate. Depth error is $\sigma_z \approx z^2\sigma_d/(fb)$, where $z$ is depth, $\sigma_d$ is disparity error, $f$ is focal length and $b$ is the stereo baseline; this follows from differentiating $z = fb/d$ with respect to disparity $d$. For monocular detectors the growth is learned, but it should keep the same ray-aligned shape.
The tracker, a Kalman-style filter, carries $\Sigma_{3D}$ into the history covariance. The predictor receives that history covariance as an input feature, and its output variance is trained to reflect it.
The planner then derives its branch time from the same noise. Take two intent modes whose predicted positions for the agent differ by $d(s)$ at time $s$, and let $\sigma_s$ be the propagated observation noise projected onto the direction that separates them. Each observation then adds about $d(s)^2/(2\sigma_s^2)$ to the expected log-likelihood ratio between the modes; this is the KL divergence between two equal-variance Gaussians. We set
$$t_b = \min\Big\{t : \sum_{s \le t} \frac{d(s)^2}{2\sigma_s^2} \ge \log(1/\epsilon)\Big\},$$
capped by the latest time at which the vehicle can still safely diverge. Here $\epsilon$ is the tolerated probability of committing to the wrong mode. Before $t_b$ the planner keeps the RACP shared trunk. After $t_b$ it branches.
The new ingredient is the causal link: perception geometry sets the observation noise, the noise sets how fast the robot's intent belief can be resolved, and that rate sets the branch time. RACP, CCTR and MTP each handle one link in isolation.
Research Program
The first stage builds the uncertainty-carrying chain on nuScenes. This means a camera 3D detector with a heteroscedastic depth head, meaning it outputs a variance alongside each depth. We add the Jacobian propagation and the ego covariance from a LIO-SAM-style window on the nuScenes lidar, then a covariance-aware tracker. We will measure the cost of producing ego marginals, and a firm requirement is that it adds no more than a few milliseconds per frame.
The second stage tests the diagnostic hypothesis before building anything downstream. We will run the chain and bin forecast errors by range, occlusion and truncation. We then ask how much of CCTR's U-shaped miscalibration curve disappears once predictor variance is conditioned on the propagated input covariance, compared with conditioning on range alone.
The third stage retrains a Trajectron++-style predictor (Trajectron++) to use the history covariance as an input. It outputs a mixture of Gaussians, which RACP's intent filter can consume directly.
The fourth stage replaces RACP's fixed $t_b$ with the rule above. The intent filter's likelihood uses the propagated $\sigma_s$ rather than a constant.
Baselines
The naive reference is the standard cascade. Deterministic 3D boxes go to a tracker, then to an uncalibrated predictor, then to RACP with its default fixed $t_b$. A second naive reference adds a fixed-radius obstacle inflation tuned on validation, which is the usual engineering patch.
The strongest calibration competitor is CCTR. It reports an 11% to 22% reduction in expected calibration error and a variance-error correlation of 0.95 on Argoverse, with 17% to 46% lower motion-planning $\ell_2$ error. There is no reported result found on Lacuna for CCTR on nuScenes, or on inputs from a real detector and tracker, so we will retrain it on our nuScenes tracker outputs. To be fair, we will strengthen it by giving its scaler network range, detector score and occlusion as extra context features, with the same data and the same hyperparameter search budget as our model. Beating it means lower expected calibration error and higher variance-error correlation on the far-range and occluded bins, without losing ground on near-range bins.
The strongest perception-robustness competitor is MTP. It reports a 34.2% overall gain on nuScenes against a single-hypothesis cascade. We run it with $H$ chosen so that its GPU memory (about 1.8 GB per hypothesis) matches our total budget.
For planning, the competitor is RACP. It reports a 0% collision rate on the T-junction and highway scenarios and an average runtime of 174.98 ms on CommonRoad under clean inputs, against a 3.47% collision rate for the single-most-likely-path planner at intersections. We rerun RACP with the best fixed $t_b$ found by grid search on validation scenarios, fed by the same noisy predictions. Beating it means fewer collisions at equal or better progress, meaning less freezing, within a similar runtime.
Three ablations isolate the key ingredient. The first propagates covariance but keeps a fixed $t_b$. The second uses adaptive $t_b$ driven by the predictor's learned variance alone, with no perception covariance. The third replaces the ray-aligned covariance with an isotropic one of equal determinant, which tests whether the shape matters or only the overall size.
Why not just…?
Why not just give CCTR's temperature scaler range as a feature and skip explicit propagation? This is the real threat. On a fixed sensor rig, range alone may capture most of the effect, and if it does, our first stage will show it plainly. Our bet rests on two differences. First, propagated covariance varies with occlusion, truncation and ego-pose uncertainty at a fixed range. Second, it changes correctly when the sensor changes, without retraining. The decisive test is a sensor-shift evaluation. We train both methods on nuScenes front cameras, then test on side cameras, which have different intrinsics, and on a stereo configuration in CARLA. A learned range-to-temperature mapping should transfer poorly, while the propagated covariance should adjust automatically. If CCTR-with-range matches us under the shift as well, we drop the calibration claim and keep only the branch-time contribution.
Why not just train the predictor on noisy tracker outputs as data augmentation? Augmentation makes the mean forecast more robust, but it teaches the predictor an average noise level, not the noise of this particular detection. The adaptive $t_b$ rule needs per-instance $\sigma_s$. We include this baseline, augmentation with no covariance input, and expect it to match us on minADE but fall behind on calibration in the far and occluded bins, and on closed-loop collisions.
Why not just inflate obstacles or add a large fixed safety margin? A margin large enough for far, occluded agents freezes the robot around near, clean ones. The robot freezing because it plans conservatively across every mode is exactly the failure RACP was built to avoid. The tuned-inflation baseline measures this trade-off directly as collision rate against progress.
Why not go end-to-end from pixels to controls? The CARLA study (Dosovitskiy et al.) reports that reinforcement learning reaches 7% success against 77% for a modular pipeline, and that imitation learning drops from 95% to 40% in an unseen town. The end-to-end route is also incompatible with the corridor-based optimizers whose feasibility guarantees we want to keep.
Proposed Experiments
The open-loop study runs on nuScenes with the full camera-to-forecast chain. We report minADE and minFDE (the average and final distance of the best forecast), ViP3D's End-to-end Prediction Accuracy metric, which also penalizes false "ghost" detections, and calibration measured as expected calibration error and variance-error correlation. All metrics are stratified by range, occlusion and truncation. As the reference point for forecast accuracy we cite ViP3D's minFDE of 2.90 m and EPA of 0.236. We will note that we are not trying to beat ViP3D on accuracy; our goal is to match typical box-based accuracy while producing usable variance.
The closed-loop study runs RACP-style planning in CommonRoad at the T-junction, highway and intersection scenarios. We inject detection noise fitted from nuScenes errors, including the ray-aligned shape and its growth with range. We then repeat the study in CARLA with the actual camera chain in the loop. Metrics are collision rate, time-to-goal, rate of emergency stops and planner runtime.
The first decisive check is the sensor-shift experiment described above. The second is the collision-versus-progress curve. The proposal succeeds only if adaptive $t_b$ lowers collisions relative to the best fixed $t_b$ at equal or better progress, and the isotropic-covariance ablation does measurably worse, which would show that the geometry of the uncertainty, not just its size, is doing the work.
References
- MTP: Multi-hypothesis Tracking and Prediction for Reduced Error Propagation
- CCTR: Calibrating Trajectory Prediction for Uncertainty-Aware Motion Planning in Autonomous Driving
- ViP3D: End-to-end Visual Trajectory Prediction via 3D Agent Queries
- RACP: Risk-Aware Contingency Planning with Multi-Modal Predictions
- Accurate Localization of 3D Objects from RGB-D Data Using Segmentation Hypotheses
- Localization and Mapping using Instance-specific Mesh Models
- LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping
- Adaptive relative bundle adjustment
- Trajectron++: Dynamically-Feasible Trajectory Forecasting with Heterogeneous Data
- CARLA: An Open Urban Driving Simulator