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