tests/test_depth.py

"""Depth estimation, checked against geometry we can construct exactly.

A synthetic camera with known height and a flat road: for any chosen distance the
exact pixel row of the ground contact is computable, so the estimator can be
scored against truth rather than against itself.

    python -m tests.test_depth
"""

from __future__ import annotations

import numpy as np

from src.common.calib import Calibration
from src.depth import known_size
from src.depth.estimate import estimate, robust_summary, sample_points
from src.depth.ground_plane import (
    ground_depth,
    ground_depth_batch,
    horizon_row,
    metres_per_pixel,
)

FX = FY = 1266.0
CX, CY = 800.0, 450.0
CAM_HEIGHT = 1.51
WIDTH, HEIGHT = 1600, 900


def make_calibration(pitch_rad: float = 0.0) -> Calibration:
    """Camera looking forward, optionally pitched down by `pitch_rad`."""
    # Camera +Z forward -> ego +X, camera +Y down -> ego -Z.
    base = np.array([[0.0, 0.0, 1.0], [-1.0, 0.0, 0.0], [0.0, -1.0, 0.0]])
    # Pitch is a rotation about the camera's X axis (right). Rotating about Y
    # would be yaw here, since +Y is down, and would leave the ground normal --
    # and therefore every distance -- completely unchanged.
    pitch = np.array([[1.0, 0.0, 0.0],
                      [0.0, np.cos(pitch_rad), -np.sin(pitch_rad)],
                      [0.0, np.sin(pitch_rad), np.cos(pitch_rad)]])
    return Calibration(
        sensor_id="synthetic", width=WIDTH, height=HEIGHT,
        K=np.array([[FX, 0.0, CX], [0.0, FY, CY], [0.0, 0.0, 1.0]]),
        cam_height_m=CAM_HEIGHT, R_ego_from_cam=base @ pitch,
    )


def ground_row(distance_m: float) -> float:
    """The exact image row of the ground contact, for a level camera."""
    return CY + FY * CAM_HEIGHT / distance_m


def main() -> None:
    calibration = make_calibration()

    # 1. Round trip: a known distance -> its row -> back to the distance.
    for truth in (5.0, 10.0, 25.0, 40.0, 70.0):
        result = ground_depth(CX, ground_row(truth), calibration.K,
                              calibration.R_ego_from_cam, CAM_HEIGHT)
        assert result is not None, truth
        assert abs(result[0] - truth) < 1e-6, (truth, result[0])

    # 2. The reference and the vectorised form agree.
    rows = np.array([ground_row(d) for d in (5.0, 12.0, 30.0, 60.0)])
    batch = ground_depth_batch(np.full_like(rows, CX), rows, calibration)
    for row, got in zip(rows, batch):
        ref = ground_depth(CX, row, calibration.K, calibration.R_ego_from_cam, CAM_HEIGHT)
        assert abs(ref[0] - got) < 1e-9, (ref, got)

    # 3. Lateral offset: a point one metre right at 10 m sits at a predictable
    #    column, and comes back as +1 m.
    u_right = CX + FX * 1.0 / 10.0
    _, lateral = ground_depth(u_right, ground_row(10.0), calibration.K,
                              calibration.R_ego_from_cam, CAM_HEIGHT)
    assert abs(lateral - 1.0) < 1e-6, lateral

    # 4. Above the horizon there is no answer, and the horizon is where expected.
    assert ground_depth(CX, CY - 1.0, calibration.K,
                        calibration.R_ego_from_cam, CAM_HEIGHT) is None
    assert abs(horizon_row(calibration) - CY) < 0.5, horizon_row(calibration)
    assert np.isnan(ground_depth_batch(np.array([CX]), np.array([CY - 5.0]),
                                       calibration)[0])

    # 5. Sensitivity grows as Z^2: ten times the range is a hundred times the
    #    metres per pixel.
    near, far = metres_per_pixel(10.0, calibration), metres_per_pixel(100.0, calibration)
    assert abs(far / near - 100.0) < 1e-6, (near, far)

    # 6. The robust summary survives a minority of nonsense samples.
    values = np.array([20.0, 20.5, 19.5, 20.2, 5000.0, np.nan])
    median, spread, count = robust_summary(values)
    assert abs(median - 20.1) < 0.6, median
    assert count == 4, count

    # 7. Sampling stays inside the box and hugs its bottom.
    box = (100.0, 200.0, 300.0, 400.0)
    u, v = sample_points(box)
    assert u.min() >= box[0] and u.max() <= box[2], (u.min(), u.max())
    # Rows straddle the bottom edge -- below the box on purpose -- and the
    # median row is the edge itself, which is what keeps the estimate unbiased.
    height = box[3] - box[1]
    assert abs(np.median(v) - box[3]) < 1e-9, np.median(v)
    assert v.min() < box[3] < v.max(), (v.min(), v.max())
    assert v.max() - v.min() <= 0.21 * height + 1e-9, (v.min(), v.max())

    # 8. End to end on a synthetic cone at 20 m: a 1.07 m cone standing on the
    #    road, boxed exactly.
    priors = {"cone": {"height_m": 1.07, "width_m": 0.40},
              "stop_sign": {"width_m": 0.75}}
    truth = 20.0
    bottom = ground_row(truth)
    top = CY + FY * (CAM_HEIGHT - 1.07) / truth
    half = FX * 0.20 / truth
    result = estimate((CX - half, top, CX + half, bottom), "cone", calibration, priors)
    assert result.method == "ground_plane", result
    assert abs(result.distance_m - truth) < 0.5, result.distance_m
    assert result.disagreement < 0.05, result      # both estimators agree here

    # 9. A stop sign is routed to the size prior, never to the ground plane --
    #    its box bottom is in the air.
    width_px = FX * 0.75 / 30.0
    sign = estimate((CX - width_px / 2, 100.0, CX + width_px / 2, 100.0 + width_px),
                    "stop_sign", calibration, priors)
    assert sign.method == "known_size", sign
    assert abs(sign.distance_m - 30.0) < 0.5, sign.distance_m

    # 10. A box bottom at the horizon is refused rather than answered with a
    #     number in the kilometres.
    far_box = (CX - 5, CY - 20, CX + 5, CY + 5)
    refused = estimate(far_box, "cone", calibration, priors)
    assert refused.method != "ground_plane", refused

    # 11. The known-size forms invert cleanly.
    assert abs(known_size.distance_from_width(FX * 0.75 / 25.0, 0.75, calibration)
               - 25.0) < 1e-6
    assert abs(known_size.distance_from_height(FY * 1.07 / 15.0, 1.07, calibration)
               - 15.0) < 1e-6
    assert np.isnan(known_size.distance_from_width(0.0, 0.75, calibration))

    # 12. A pitched camera shifts the answer, which is why a per-scene pitch
    #     error shows up as a distance error. One degree at 20 m is metres.
    pitched = make_calibration(np.radians(1.0))
    shifted = ground_depth_batch(np.array([CX]), np.array([ground_row(20.0)]), pitched)[0]
    assert abs(shifted - 20.0) > 1.0, shifted

    print("DEPTH TEST PASSED")


if __name__ == "__main__":
    main()