"""Distance from a pixel, by intersecting its ray with the ground plane. REFERENCE IMPLEMENTATION -- this is the file to transcribe. The maths is laid out a step at a time below; `ground_depth` is the whole idea in a dozen lines and everything else in this package is built on it. The picture ----------- A pinhole camera at height `h` above a flat road. A pixel is not a point, it is a *direction*: every 3D point along one ray lands on the same pixel. That ray leaves the camera centre and travels outward, and if it is pointed below the horizon it eventually strikes the road. That intersection is the only point on the ray we can name, and naming it is what turns a pixel into a distance. This is why the bottom edge of a detection matters so much. The bottom of a cone is where the cone touches the road, so its ray hits the ground at the cone. The top of the cone is a metre in the air and its ray hits the road somewhere well behind the object -- which is the single easiest way to get a plausible-looking answer that is completely wrong. The maths --------- Work in the camera frame: +X right, +Y down, +Z forward. 1. A pixel `(u, v)` back-projects to the direction d = [(u - cx) / fx, (v - cy) / fy, 1] Dividing by the focal length converts a pixel offset into a tangent of an angle. The third component is 1, which means `d` is scaled so that "one unit along the ray" is "one metre forward". That is a convenience with a payoff later: it makes `lambda` come out directly as the forward distance. 2. The road is the plane whose normal is straight down in the world, passing `h` below the camera. The world's down direction expressed in camera axes is n = -R_ego_from_cam.T @ [0, 0, 1] `R_ego_from_cam` rotates a camera-frame vector into the ego frame, so its transpose does the reverse. `[0, 0, 1]` is up in the ego frame, so negating gives down. For a camera mounted level this comes out near `[0, 1, 0]` -- +Y, which is down in camera axes, as it should. 3. A point `lambda * d` lies on the road when its component along `n` equals the camera height: n . (lambda * d) = h => lambda = h / (n . d) 4. Because `d[2]` was set to 1, `lambda` *is* the forward distance Z, and `lambda * d[0]` is the lateral offset X -- positive to the right. The failure mode ---------------- `n . d` is how steeply the ray points down. It goes to zero at the horizon, and `lambda` goes to infinity with it. Just above the horizon it goes negative, and the ray strikes the road *behind* the camera. Guarding with a small epsilon is not enough: a ray one pixel below the horizon returns a mathematically valid answer of several kilometres. Sensitivity is `Z^2 / (fy * h)`, so at 10 m one pixel of error is 3 cm and at 45 m it is 1.8 m. Reject by horizon margin in pixels, not by epsilon -- see `horizon_row` and `MIN_HORIZON_MARGIN_PX` below. Two traps worth naming ---------------------- * `h` is the camera height **above the road**, which is not always the sensor's z translation. nuScenes puts its ego origin on the ground, so there `t[2]` is correct. Argoverse 2 puts it at the rear axle centre, about 0.26 m up, and using `t[2]` there makes every distance read ~18% short. `Calibration. cam_height_m` is already corrected for this; take the height from there rather than from a translation vector. * Step 1 assumes an ideal pinhole. Argoverse 2 cameras have real radial distortion (k1 near -0.24) and their images are not rectified, so the pixel must be undistorted first. `Calibration.ray()` does that; nuScenes passes zeros, for which it is the identity. """ from __future__ import annotations import numpy as np from src.common.calib import Calibration # A box bottom closer than this to the horizon is not turned into a distance. # At 20 px on nuScenes CAM_FRONT this caps reported range near 90 m; below it # the estimate is dominated by where exactly the box edge landed. MIN_HORIZON_MARGIN_PX = 20.0 # --------------------------------------------------------------------------- # The reference implementation # --------------------------------------------------------------------------- def ground_depth(u: float, v: float, K: np.ndarray, R_ego_from_cam: np.ndarray, cam_height_m: float) -> tuple[float, float] | None: """Forward distance Z and lateral offset X of the ground point at pixel (u, v). Returns None when the ray does not meet the road ahead of the camera. Note the third argument is the camera height **above the road**, not a translation vector -- see "Two traps worth naming" above. """ # 2. World-down, expressed in camera axes. n = -R_ego_from_cam.T @ np.array([0.0, 0.0, 1.0]) # 1. Pixel -> ray direction. Dividing by focal length turns a pixel offset # into an angle; the trailing 1 makes lambda come out as metres forward. d = np.array([(u - K[0, 2]) / K[0, 0], (v - K[1, 2]) / K[1, 1], 1.0]) # 3. How steeply the ray points at the road. Zero at the horizon, negative # above it. denom = float(n @ d) if denom <= 1e-6: return None lam = cam_height_m / denom # 4. d[2] == 1, so lambda is the forward distance outright. return lam, lam * float(d[0]) # --------------------------------------------------------------------------- # The same thing, vectorised and distortion-aware, for the rest of the pipeline # --------------------------------------------------------------------------- def ground_depth_batch(u: np.ndarray, v: np.ndarray, calibration: Calibration) -> np.ndarray: """`ground_depth` over many pixels at once, returning forward distance. Differs from the reference above in two ways, both deliberate: * it undistorts the pixel first, via `Calibration.ray`, so it is correct for Argoverse 2 as well as nuScenes; * it takes the height from `calibration.cam_height_m`. Rays that do not meet the road ahead come back as NaN rather than None, so the result lines up element-wise with the input. """ u = np.asarray(u, dtype=float) v = np.asarray(v, dtype=float) normal = -calibration.ground_normal_cam() # world-down in camera axes rays = calibration.ray(u, v) # (3, N), lens removed denominator = normal @ rays with np.errstate(divide="ignore", invalid="ignore"): scale = calibration.cam_height_m / denominator distance = scale * rays[2] distance[~np.isfinite(distance)] = np.nan distance[denominator <= 1e-6] = np.nan distance[distance <= 0.0] = np.nan return distance def horizon_row(calibration: Calibration) -> float: """The image row whose ray runs parallel to the road. Found by walking down the centre column until the ray's alignment with the ground normal changes sign, rather than solved in closed form, because with lens distortion there is no closed form. """ normal = -calibration.ground_normal_cam() rows = np.linspace(0.0, float(calibration.height), 4001) columns = np.full_like(rows, calibration.cx) alignment = normal @ calibration.ray(columns, rows) crossings = np.flatnonzero(np.sign(alignment[:-1]) != np.sign(alignment[1:])) return float(rows[crossings[0]]) if len(crossings) else float(calibration.cy) def metres_per_pixel(distance_m: float, calibration: Calibration) -> float: """How many metres one row of pixels is worth at a given range. The error budget of this whole approach in one line. Differentiating `Z = fy * h / (v - v_horizon)` with respect to `v` gives `Z^2 / (fy * h)`: the sensitivity grows with the *square* of range. On nuScenes CAM_FRONT (fy 1266, h 1.51 m) that is 3 cm per pixel at 10 m, 21 cm at 25 m, and 1.8 m at 45 m. The same one-pixel error in the detector's bottom edge is negligible near the car and dominant far from it, which is why the far-field numbers in the README are what they are, and why no amount of care in the estimator fixes them -- the information is not in the pixel. It is also the honest way to quote a result: an MAE of 8 m at 40 m is four pixels of box-edge error, which is about as well as a detector localises. """ return distance_m ** 2 / (calibration.fy * calibration.cam_height_m)