cone-distance / src /depth /ground_plane.py

Aryan Sethi

Claude Opus 5 (1M context)

Document src/depth for transcription

699e481 about 1 month ago

8.43 kB

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

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