"""Tests for lift.py. Run: python -m pytest -q""" import numpy as np import pytest from lift import backproject, box_center, depth_from_width, pixel_ray, ray_plane, to_base K = np.array([[600.0, 0.0, 320.0], [0.0, 600.0, 240.0], [0.0, 0.0, 1.0]]) def project(p: np.ndarray) -> tuple[float, float]: uvw = K @ p return uvw[0] / uvw[2], uvw[1] / uvw[2] def test_backproject_inverts_projection() -> None: rng = np.random.default_rng(3) for _ in range(20): p = np.array([rng.uniform(-0.3, 0.3), rng.uniform(-0.2, 0.2), rng.uniform(0.3, 1.5)]) u, v = project(p) assert np.allclose(backproject(u, v, p[2], K), p) def test_worked_example_numbers() -> None: u, v = box_center([376, 276, 424, 324]) assert (u, v) == (400.0, 300.0) z = depth_from_width(48, 0.04, 600.0) assert z == pytest.approx(0.5) assert np.allclose(backproject(u, v, z, K), [80 * 0.5 / 600, 60 * 0.5 / 600, 0.5]) def test_ray_plane_hits_plane_and_projects_back() -> None: p = ray_plane(400, 300, K, n=[0, 0, 1], d=0.52) assert p[2] == pytest.approx(0.52) assert np.allclose(project(p), (400, 300)) def test_ray_plane_tilted_plane() -> None: n = np.array([0.0, -0.5, 1.0]) / np.linalg.norm([0.0, -0.5, 1.0]) p = ray_plane(250, 180, K, n=n, d=0.4) assert float(n @ p) == pytest.approx(0.4) assert np.allclose(project(p), (250, 180)) def test_ray_plane_errors() -> None: with pytest.raises(ValueError): ray_plane(320, 240, K, n=[1, 0, 0], d=0.5) # optical axis is parallel to the plane x = 0.5 with pytest.raises(ValueError): ray_plane(320, 240, K, n=[0, 0, 1], d=-0.5) # plane behind the camera def test_pixel_ray_principal_point() -> None: assert np.allclose(pixel_ray(320, 240, K), [0, 0, 1]) def test_to_base_example() -> None: T = np.eye(4) T[:3, :3] = [[0, -1, 0], [-1, 0, 0], [0, 0, -1]] T[:3, 3] = [0.30, 0.0, 0.54] p_cam = ray_plane(400, 300, K, n=[0, 0, 1], d=0.52) assert np.allclose(to_base(T, p_cam), [0.30 - 0.052, -0.52 * 80 / 600, 0.02])