"""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/.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)