"""Tests for camera.py on synthetic, seeded data. Run: python -m pytest -q""" from __future__ import annotations import numpy as np import pytest from camera import (apply_homography, backproject_to_plane, distort, estimate_homography, intrinsic_matrix, intrinsics_from_homographies, look_at, plane_homography, project, rigid_transform, transfer_error, undistort_normalized) K = intrinsic_matrix(800.0, 800.0, 320.0, 240.0) def rot_z(a: float) -> np.ndarray: c, s = np.cos(a), np.sin(a) return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]) def rot_x(a: float) -> np.ndarray: c, s = np.cos(a), np.sin(a) return np.array([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]]) def test_worked_projection() -> None: uv, z = project(K, np.eye(3), np.zeros(3), np.array([[0.10, -0.05, 0.50]])) np.testing.assert_allclose(uv, [[480.0, 160.0]]) assert z[0] == pytest.approx(0.5) def test_look_at_is_a_rotation_and_centres_target() -> None: R, t = look_at([0.2, -0.4, 0.5], [0.0, 0.2, 0.0]) np.testing.assert_allclose(R @ R.T, np.eye(3), atol=1e-12) assert np.linalg.det(R) == pytest.approx(1.0) uv, z = project(K, R, t, np.array([[0.0, 0.2, 0.0]])) np.testing.assert_allclose(uv, [[320.0, 240.0]], atol=1e-9) assert z[0] > 0 def test_point_behind_camera_has_negative_depth() -> None: _, z = project(K, np.eye(3), np.zeros(3), np.array([[0.0, 0.0, -1.0]])) assert z[0] < 0 def test_downward_camera_backprojection_numbers() -> None: R = np.diag([1.0, -1.0, -1.0]) t = -R @ np.array([0.0, 0.0, 0.6]) X0 = backproject_to_plane(K, R, t, np.array([[480.0, 160.0]])) X1 = backproject_to_plane(K, R, t, np.array([[480.0, 160.0]]), offset=0.025) np.testing.assert_allclose(X0, [[0.12, 0.06, 0.0]], atol=1e-12) np.testing.assert_allclose(X1, [[0.115, 0.0575, 0.025]], atol=1e-12) def test_project_then_backproject_round_trip_with_distortion() -> None: rng = np.random.default_rng(10) R, t = look_at([0.1, -0.35, 0.45], [0.0, 0.2, 0.0]) dist = (-0.12, 0.05, 0.001, -0.0005, 0.0) X = np.column_stack((rng.uniform(-0.15, 0.15, 50), rng.uniform(0.05, 0.35, 50), np.zeros(50))) uv, _ = project(K, R, t, X, dist=dist) X_back = backproject_to_plane(K, R, t, uv, dist=dist) np.testing.assert_allclose(X_back, X, atol=1e-9) def test_undistort_inverts_distort() -> None: rng = np.random.default_rng(11) xn = rng.uniform(-0.4, 0.4, size=(200, 2)) coeffs = (-0.25, 0.08, 0.001, -0.002, 0.0) np.testing.assert_allclose(undistort_normalized(distort(xn, coeffs), coeffs), xn, atol=1e-10) def test_barrel_distortion_pulls_points_inwards() -> None: xn = np.array([[0.3, 0.2]]) xd = distort(xn, (-0.2, 0.0, 0.0, 0.0, 0.0)) assert np.linalg.norm(xd) < np.linalg.norm(xn) def test_homography_exact_from_noiseless_points() -> None: R, t = look_at([0.0, -0.35, 0.45], [0.0, 0.2, 0.0]) H_true = plane_homography(K, R, t) rng = np.random.default_rng(12) xy = np.column_stack((rng.uniform(-0.2, 0.2, 8), rng.uniform(0.05, 0.35, 8))) uv, _ = project(K, R, t, np.column_stack((xy, np.zeros(8)))) np.testing.assert_allclose(apply_homography(H_true, xy), uv, atol=1e-9) H = estimate_homography(xy, uv) np.testing.assert_allclose(H, H_true, rtol=1e-8, atol=1e-8) def test_homography_with_noise_and_cube_location() -> None: rng = np.random.default_rng(13) R, t = look_at([0.0, -0.35, 0.45], [0.0, 0.2, 0.0]) xy = np.array([[x, y] for y in (0.10, 0.20, 0.30) for x in (-0.15, -0.05, 0.05, 0.15)]) uv, _ = project(K, R, t, np.column_stack((xy, np.zeros(len(xy))))) sigma = 0.5 uv_noisy = uv + rng.normal(0.0, sigma, uv.shape) H = estimate_homography(xy, uv_noisy) # residual close to the noise level (8 dof fitted to 24 measurements) assert 0.5 * sigma < transfer_error(H, xy, uv_noisy) < 1.2 * sigma cube_uv, _ = project(K, R, t, np.array([[0.07, 0.22, 0.0]])) cube_xy = apply_homography(np.linalg.inv(H), cube_uv) assert np.linalg.norm(cube_xy - [0.07, 0.22]) < 1e-3 # better than 1 mm def test_normalised_dlt_is_invariant_to_world_units_and_origin() -> None: rng = np.random.default_rng(14) R, t = look_at([0.0, -0.35, 0.45], [0.0, 0.2, 0.0]) xy = np.column_stack((rng.uniform(-0.2, 0.2, 10), rng.uniform(0.05, 0.35, 10))) uv, _ = project(K, R, t, np.column_stack((xy, np.zeros(10)))) uv += rng.normal(0.0, 1.0, uv.shape) xy_mm = xy * 1000.0 + np.array([500.0, -200.0]) # other units, other origin H_m = estimate_homography(xy, uv) H_mm = estimate_homography(xy_mm, uv) probe = np.array([[0.05, 0.15], [-0.1, 0.3]]) np.testing.assert_allclose(apply_homography(H_m, probe), apply_homography(H_mm, probe * 1000.0 + [500.0, -200.0]), atol=1e-6) def test_homography_needs_four_points() -> None: with pytest.raises(ValueError): estimate_homography(np.zeros((3, 2)), np.zeros((3, 2))) def test_zhang_linear_calibration_recovers_K() -> None: rng = np.random.default_rng(15) K_true = intrinsic_matrix(820.0, 810.0, 330.0, 235.0) board = np.array([[i * 0.025, j * 0.025] for i in range(9) for j in range(6)]) # 9 x 6 corners board -= board.mean(axis=0) Hs = [] for k in range(6): R = rot_x(np.pi + rng.uniform(-0.5, 0.5)) @ rot_z(rng.uniform(-0.6, 0.6)) R = rot_x(rng.uniform(-0.35, 0.35)) @ R t = np.array([rng.uniform(-0.03, 0.03), rng.uniform(-0.03, 0.03), rng.uniform(0.35, 0.5)]) uv, z = project(K_true, R, t, np.column_stack((board, np.zeros(len(board))))) assert np.all(z > 0) uv += rng.normal(0.0, 0.2, uv.shape) Hs.append(estimate_homography(board, uv)) K_est = intrinsics_from_homographies(Hs) np.testing.assert_allclose(K_est[[0, 1, 0, 1], [0, 1, 2, 2]], K_true[[0, 1, 0, 1], [0, 1, 2, 2]], rtol=0.03) def test_rigid_transform_recovers_camera_to_base() -> None: rng = np.random.default_rng(16) R_true = rot_z(0.4) @ rot_x(2.3) t_true = np.array([0.05, -0.30, 0.50]) P_cam = rng.uniform(-0.2, 0.2, size=(12, 3)) + [0.0, 0.0, 0.6] P_base = P_cam @ R_true.T + t_true + rng.normal(0.0, 0.001, size=(12, 3)) R, t = rigid_transform(P_cam, P_base) assert np.linalg.det(R) == pytest.approx(1.0) angle = np.degrees(np.arccos(np.clip((np.trace(R.T @ R_true) - 1.0) / 2.0, -1.0, 1.0))) assert angle < 0.5 assert np.linalg.norm(t - t_true) < 0.005