cone-distance / src /depth /estimate.py
Aryan Sethi
Claude Opus 5 (1M context)
Drop barrel, add held-out stop-sign distance, add the GPU runbook
5d449ff
Raw History Blame Contribute Delete
15.7 kB
"""Turn one detection box into one distance.
The ground-plane estimator answers for a single pixel. A box is not a pixel, and
the obvious choice -- the midpoint of the bottom edge -- puts the whole answer on
one number that the detector is least confident about. Box edges are exactly
where detectors are sloppy, the bottom edge gets dragged down by shadows and up
by occlusion, and at 40 m a ten-pixel error is eight metres.
So instead: sample a band of candidate ground-contact rows, estimate a distance
at every one, and take a robust summary.
The band is centred **on** the bottom edge, not extended upward from it inside
the box. That matters more than it looks. The interior of a box is not the
ground -- a pixel halfway up a cone is half a metre in the air, and its ray
strikes the road well beyond the cone -- so a band running upward from the bottom
edge biases every estimate outward. Measured on a synthetic cone at 20 m, a band
covering the bottom 20% of the box reads 21.5 m, because the median of that band
sits about a tenth of a box-height above the contact point and at 20 m a pixel is
worth 21 cm. Centred on the edge, the same box reads 20.0 m.
Centring also makes the median exact rather than approximate. Distance is a
monotonic function of row, and the median commutes with monotonic maps, so the
median of the sampled distances is exactly the distance at the median row --
which is the bottom edge. The band costs nothing in accuracy and buys two things:
rays that escape over the horizon drop out as NaN instead of poisoning an
average, and the spread of what remains is a usable confidence signal.
MAD rather than standard deviation for that spread, because the failure mode is
not Gaussian noise -- it is a minority of samples landing on nothing sensible,
and those are arbitrarily far off. A median survives up to half the samples being
nonsense; a mean does not survive one.
Per class:
cone, barrier ground plane, cross-checked against the height prior
stop_sign known size only -- pole-mounted, so the box bottom is
not a ground contact and the ground plane would put it
far beyond where it is
"""
from __future__ import annotations
from dataclasses import dataclass
import numpy as np
from src.common.calib import Calibration
from src.depth import known_size
from src.depth.ground_plane import (
MIN_HORIZON_MARGIN_PX,
ground_depth_batch,
horizon_row,
)
# How far either side of the bottom edge to sample, as a fraction of the box
# height. This is a model of how wrong the detector's bottom edge might be, so it
# is symmetric: the true contact is as likely to be below the drawn edge (shadow,
# a low kerb) as above it (occlusion, a clipped box).
EDGE_BAND = 0.10
# Width sampled, as a fraction of the box. The left and right edges are where
# boxes are loosest, so stay off them.
WIDTH_BAND = 0.60
GRID_U, GRID_V = 5, 5
# Samples further than this many MADs from the median are dropped before the
# final median is taken. 3.0 is loose enough to keep a genuinely spread-out but
# valid patch and tight enough to remove a ray that escaped over the horizon.
OUTLIER_MADS = 3.0
# Classes whose box bottom is a ground contact.
GROUND_CONTACT = ("cone", "barrier")
# Above this relative disagreement between the two estimators, the result is
# reported but flagged: one of the two assumptions has broken.
DISAGREEMENT_LIMIT = 0.40
@dataclass
class DepthEstimate:
"""One answer, plus everything needed to decide whether to believe it.
A bare float would be a worse return type than it looks. A monocular
distance is a *conditional* answer -- conditional on the road being flat,
on the box bottom being the ground contact, and on the object matching its
class prior. When those hold it is good to a fraction of a metre; when they
do not it is quietly wrong, and nothing about the number itself says which
case you are in. So each estimate carries its own evidence.
Fields
------
distance_m
Forward distance along the optical axis, in metres. NaN when no
estimate could be made -- deliberately NaN rather than 0.0 or -1.0, so
that arithmetic on it propagates instead of silently producing a
plausible number, and so `notna()` is the filter everywhere.
Note "forward distance", not Euclidean range: this is the Z component
in the camera frame, matching `manifest.gt_distance_m`. An object 30 m
ahead and 10 m to the left is at distance 30, not 31.6.
method
Which estimator produced it: "ground_plane", "known_size", or "none".
Worth keeping because the two have different error characteristics, so
an aggregate over a mixture of methods is hard to interpret without it.
spread_m
Robust spread of the samples that went into the median, as
1.4826 * MAD, which puts it on the same scale as a standard deviation.
This is the estimate's sensitivity to where exactly the box bottom
landed, not its accuracy: a tight spread on a sloped road is confidently
wrong. NaN for known-size estimates, which sample nothing.
n_samples
How many sampled pixels survived outlier rejection. A low count means
most rays escaped over the horizon, so the few that did not are near
that edge and the estimate is fragile.
cross_check_m
What the *other* estimator said, carried along for comparison rather
than folded into `distance_m`. Averaging the two would hide exactly the
disagreement that is worth seeing.
disagreement
|a - b| / mean(a, b) between the two estimators. NaN when only one was
available. This is the single most useful field: the two fail for
unrelated reasons -- the ground plane on road grade, the size prior on
an unusual object -- so agreement is real evidence, and a large gap
means at least one assumption has broken even though both returned a
number.
reason
Why there is no estimate, when there is not. Empty on success. Kept as
free text because the useful thing at the end of an evaluation run is a
frequency table of these, which says what is actually limiting coverage.
"""
distance_m: float
method: str
spread_m: float
n_samples: int
cross_check_m: float
disagreement: float
reason: str = ""
@property
def usable(self) -> bool:
"""Did we get a number at all?
The weaker of the two checks, and the right one for coverage: it asks
only whether an estimate exists, not whether it is trustworthy.
"""
return bool(np.isfinite(self.distance_m))
@property
def confident(self) -> bool:
"""Did we get a number that two independent estimators agree on?
Note the `not (x > limit)` rather than `x <= limit`. NaN compares False
against everything, so `disagreement <= limit` would reject every
estimate that had no cross-check available -- including every stop sign,
which has only one estimator by design. Written this way, a missing
cross-check counts as "nothing contradicts it" rather than as a failure.
"""
return self.usable and not (self.disagreement > DISAGREEMENT_LIMIT)
def sample_points(box, edge_band: float = EDGE_BAND,
width_band: float = WIDTH_BAND) -> tuple[np.ndarray, np.ndarray]:
"""A grid of candidate ground-contact pixels, as two flat (u, v) arrays.
Rows straddle the bottom edge rather than sitting above it, so the median
row is the edge itself. Rows below the box are deliberate: they are
candidate contact points, not pixels of the object.
"""
x1, y1, x2, y2 = box
width, height = x2 - x1, y2 - y1
centre = (x1 + x2) / 2.0
us = np.linspace(centre - width * width_band / 2.0,
centre + width * width_band / 2.0, GRID_U)
vs = np.linspace(y2 - height * edge_band, y2 + height * edge_band, GRID_V)
grid_u, grid_v = np.meshgrid(us, vs)
return grid_u.ravel(), grid_v.ravel()
def robust_summary(values: np.ndarray) -> tuple[float, float, int]:
"""Median and 1.4826 * MAD, after dropping samples far from the median.
The 1.4826 makes MAD comparable to a standard deviation for normal data, so
the spread reads in the same units as the error.
"""
values = values[np.isfinite(values)]
if values.size == 0:
return float("nan"), float("nan"), 0
median = float(np.median(values))
mad = float(np.median(np.abs(values - median)))
if mad > 0:
keep = np.abs(values - median) <= OUTLIER_MADS * mad
if keep.any():
values = values[keep]
median = float(np.median(values))
mad = float(np.median(np.abs(values - median)))
return median, 1.4826 * mad, int(values.size)
def estimate_ground_plane(box, calibration: Calibration) -> tuple[float, float, int, str]:
"""Distance to a box by intersecting its bottom edge with the road.
The whole ground-plane path in one function: sample rows around the bottom
edge, refuse the box if that edge is too close to the horizon, turn every
sample into a distance, and summarise robustly.
The horizon check comes *before* any arithmetic, and rejects the box rather
than clamping it. That is a deliberate choice about what a caller should
receive. Sensitivity is Z^2 / (fy * h), so it grows without bound as the box
bottom approaches the horizon row: at 10 m one pixel is 3 cm, at 45 m it is
1.8 m, and a pixel or two above the horizon the ray tips over and strikes
the road behind the camera. Returning a number there would be returning a
number that says more about sub-pixel box placement than about the world.
Better to answer "not from this box" and let the caller fall back to the
size prior, which is exactly what `estimate` does with this.
Parameters
----------
box
(x1, y1, x2, y2) in pixels, absolute, with y increasing downward, so y2
is the bottom edge.
calibration
The camera this box came from. Its `cam_height_m` must be the height
above the *road*, and its `distortion` must describe the lens -- both
are handled by `Calibration`, and both are wrong if taken naively from
a dataset's extrinsics.
Returns
-------
(distance_m, spread_m, n_samples, reason)
`reason` is empty on success and names the refusal otherwise, so the
caller can report why coverage is what it is rather than just that it
is low. Distance and spread are NaN and n_samples is 0 on failure.
"""
u, v = sample_points(box)
# Reject the whole box if its bottom edge sits too near the horizon: past
# that point the answer is set by sub-pixel box placement, not by geometry.
margin = box[3] - horizon_row(calibration)
if margin < MIN_HORIZON_MARGIN_PX:
return float("nan"), float("nan"), 0, f"box bottom {margin:.0f}px from horizon"
distances = ground_depth_batch(u, v, calibration)
median, spread, count = robust_summary(distances)
if count == 0:
return float("nan"), float("nan"), 0, "no ray met the ground"
return median, spread, count, ""
def estimate(box, class_name: str, calibration: Calibration,
priors: dict) -> DepthEstimate:
"""Distance to one detection. The entry point -- everything else feeds this.
Routes by class, then cross-checks, then falls back. In order:
1. **Not a ground-contact class** (today: stop_sign). The bottom of the box
is a metre or two in the air, so a ray through it strikes the road well
beyond the object and the ground plane would confidently overestimate.
The size prior is not a cross-check here, it is the only route, so the
result carries no cross-check and no disagreement.
2. **A ground-contact class.** The ground plane is the primary estimate,
because near the camera it is far better: it does not care how large
this particular cone happens to be, while the size prior inherits the
class's spread -- nuScenes cones have a standard deviation of 0.27 m on
a 1.07 m mean, so a size-based estimate is roughly 25% uncertain at any
range. The height prior is computed anyway as the cross-check.
3. **Ground plane unavailable** -- box bottom at the horizon, or every ray
escaped. Rather than return nothing, fall back to the size prior and
record in `reason` that this happened. Half an answer beats none, but
the caller should know which it got, because this is precisely the
far-field regime where the size prior is also at its weakest.
Parameters
----------
box
(x1, y1, x2, y2) in pixels. From a detector at inference time; from the
manifest when evaluating the depth module in isolation.
class_name
One of `schema.CLASSES`. Decides the routing above.
calibration
The camera this box came from -- `calib/<sensor_id>.yaml`. Note that
Argoverse 2 has one of these per log, not one per camera model, because
its focal length and camera height vary measurably between logs.
priors
`class_priors.yaml` as a dict: {class: {height_m, width_m, ...}}.
Derived from the dataset's own 3D boxes rather than looked up, which is
what makes the cross-check meaningful -- see the note in the README
about fitting them on too small a sample.
Returns
-------
DepthEstimate
Always returned, never None. A failed estimate is a DepthEstimate with
NaN distance and a populated `reason`, so callers filter on
`.usable` rather than having to handle two different return types.
"""
prior = priors.get(class_name, {})
x1, y1, x2, y2 = box
box_width, box_height = x2 - x1, y2 - y1
# Pole-mounted: the box bottom is not on the road, so size is the only route.
if class_name not in GROUND_CONTACT:
width_m = prior.get("width_m", float("nan"))
distance = known_size.distance_from_width(box_width, width_m, calibration)
return DepthEstimate(
distance_m=distance,
method="known_size" if np.isfinite(distance) else "none",
spread_m=float("nan"),
n_samples=1,
cross_check_m=float("nan"),
disagreement=float("nan"),
reason="" if np.isfinite(distance) else f"no width prior for {class_name}",
)
ground, spread, count, reason = estimate_ground_plane(box, calibration)
cross_check = known_size.distance_from_height(
box_height, prior.get("height_m", float("nan")), calibration)
disagreement = known_size.relative_disagreement(ground, cross_check)
if np.isfinite(ground):
return DepthEstimate(ground, "ground_plane", spread, count,
cross_check, disagreement)
# The ground plane gave up -- too near the horizon, or every ray escaped.
# The size prior still has something to say, so fall back to it rather than
# returning nothing, and record that it is the weaker estimate.
if np.isfinite(cross_check):
return DepthEstimate(cross_check, "known_size", float("nan"), 1,
float("nan"), float("nan"),
reason=f"ground plane unavailable ({reason})")
return DepthEstimate(float("nan"), "none", float("nan"), 0,
float("nan"), float("nan"), reason=reason)