"""Tests for geometry.py. The ``colmap_to_threejs`` tests below are a **FROZEN CONTRACT** (foundation). The embedded ``POSE_TEST_VECTORS`` are duplicated verbatim in ``frontend/src/lib/pose.js`` so both sides provably agree — if you touch the conversion math, update both files and both vector sets, via a change request. Lane B owns this file but must not modify these three cases; it adds tests for slerp_pose / ray_from_pixel / triangulate_rays / nearest_point_on_ray below them. """ from __future__ import annotations import numpy as np import pytest from scipy.spatial.transform import Rotation from festival4d.geometry import ( colmap_to_threejs, mat_to_quat, quat_to_mat, ) # Sqrt(2)/2, spelled out so it matches the JS constant character-for-character. SQRT1_2 = 0.7071067811865476 # --- FROZEN test vectors (must equal POSE_TEST_VECTORS in frontend/src/lib/pose.js) ---- # Each: COLMAP world->camera (q=[w,x,y,z], t) -> Three.js (position, rotation 3x3 row-major). POSE_TEST_VECTORS = [ { "name": "identity", "q": [1.0, 0.0, 0.0, 0.0], "t": [0.0, 0.0, -10.0], "position": [0.0, 0.0, 10.0], "rotation": [[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]], }, { "name": "yaw90", # +90 deg about world Y "q": [SQRT1_2, 0.0, SQRT1_2, 0.0], "t": [0.0, 0.0, 10.0], "position": [10.0, 0.0, 0.0], "rotation": [[0.0, 0.0, 1.0], [0.0, -1.0, 0.0], [1.0, 0.0, 0.0]], }, { "name": "lookat", # camera at +Z looking at origin -> Three.js rotation is identity "q": [0.0, 1.0, 0.0, 0.0], "t": [0.0, 0.0, 8.0], "position": [0.0, 0.0, 8.0], "rotation": [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]], }, ] @pytest.mark.parametrize("vec", POSE_TEST_VECTORS, ids=lambda v: v["name"]) def test_colmap_to_threejs_known_vectors(vec): position, rotation = colmap_to_threejs(vec["q"], vec["t"]) np.testing.assert_allclose(position, vec["position"], atol=1e-9) np.testing.assert_allclose(rotation, vec["rotation"], atol=1e-9) @pytest.mark.parametrize("vec", POSE_TEST_VECTORS, ids=lambda v: v["name"]) def test_threejs_rotation_is_proper(vec): """R_three must be a proper rotation (orthonormal, det = +1).""" _, R = colmap_to_threejs(vec["q"], vec["t"]) np.testing.assert_allclose(R @ R.T, np.eye(3), atol=1e-9) assert abs(np.linalg.det(R) - 1.0) < 1e-9 def _colmap_pose(R_w2c: np.ndarray, center: np.ndarray) -> tuple[np.ndarray, np.ndarray]: """Build a COLMAP (q, t) from a world->camera rotation and a world camera center.""" t = -R_w2c @ center q = mat_to_quat(R_w2c) return q, t def test_colmap_to_threejs_roundtrip_random_poses(): """Forward COLMAP->Three.js then invert it; recover the original (q, t).""" rng = np.random.default_rng(20260716) flip = np.diag([1.0, -1.0, -1.0]) for _ in range(50): R_w2c = Rotation.random(random_state=rng).as_matrix() center = rng.uniform(-10, 10, size=3) q, t = _colmap_pose(R_w2c, center) position, R_three = colmap_to_threejs(q, t) # position is the camera center np.testing.assert_allclose(position, center, atol=1e-9) # invert: R_c2w = R_three @ flip -> R_w2c = R_c2w^T -> t = -R_w2c @ C R_c2w = R_three @ flip R_back = R_c2w.T t_back = -R_back @ position q_back = mat_to_quat(R_back) np.testing.assert_allclose(t_back, t, atol=1e-9) # quaternions are double-cover; compare canonical (w>=0) forms q_canon = q if q[0] >= 0 else -q np.testing.assert_allclose(q_back, q_canon, atol=1e-9) def test_quat_to_mat_matches_scipy_oracle(): """quat_to_mat agrees with an independent implementation (scipy).""" rng = np.random.default_rng(11) for _ in range(50): # scipy quaternions are scalar-LAST [x,y,z,w]; ours are scalar-FIRST [w,x,y,z] q_xyzw = rng.normal(size=4) q_xyzw /= np.linalg.norm(q_xyzw) q_wxyz = np.array([q_xyzw[3], q_xyzw[0], q_xyzw[1], q_xyzw[2]]) np.testing.assert_allclose( quat_to_mat(q_wxyz), Rotation.from_quat(q_xyzw).as_matrix(), atol=1e-9 ) # =========================================================================== # Lane B (spec M2 + M8) tests — below the frozen block. These exercise the # stubs foundation left: slerp_pose, ray_from_pixel, triangulate_rays, # nearest_point_on_ray. They do NOT touch the frozen colmap_to_threejs cases above. # =========================================================================== from scipy.spatial.transform import Slerp # noqa: E402 from festival4d.geometry import ( # noqa: E402 colmap_to_threejs, nearest_point_on_ray, ray_from_pixel, slerp_pose, triangulate_rays, ) def _wxyz_to_xyzw(q): q = np.asarray(q, dtype=float) return np.array([q[1], q[2], q[3], q[0]]) def _rand_unit_quat(rng): q = rng.normal(size=4) return q / np.linalg.norm(q) # --- slerp_pose ------------------------------------------------------------ def test_slerp_pose_endpoints(): q0 = _rand_unit_quat(np.random.default_rng(1)) q1 = _rand_unit_quat(np.random.default_rng(2)) t0 = np.array([1.0, -2.0, 3.0]) t1 = np.array([-4.0, 5.0, 6.0]) q_a, t_a = slerp_pose(q0, t0, q1, t1, 0.0) q_b, t_b = slerp_pose(q0, t0, q1, t1, 1.0) # endpoints recover the endpoint *rotations* (quaternion up to sign) and translations np.testing.assert_allclose(quat_to_mat(q_a), quat_to_mat(q0), atol=1e-12) np.testing.assert_allclose(quat_to_mat(q_b), quat_to_mat(q1), atol=1e-12) np.testing.assert_allclose(t_a, t0, atol=1e-12) np.testing.assert_allclose(t_b, t1, atol=1e-12) def test_slerp_pose_matches_scipy_oracle(): rng = np.random.default_rng(20260716) for _ in range(30): q0 = _rand_unit_quat(rng) q1 = _rand_unit_quat(rng) oracle = Slerp([0.0, 1.0], Rotation.from_quat( [_wxyz_to_xyzw(q0), _wxyz_to_xyzw(q1)])) for alpha in (0.1, 0.25, 0.5, 0.73, 0.9): q, _ = slerp_pose(q0, [0, 0, 0], q1, [0, 0, 0], alpha) np.testing.assert_allclose( quat_to_mat(q), oracle(alpha).as_matrix(), atol=1e-9) def test_slerp_pose_translation_is_linear(): q = np.array([1.0, 0.0, 0.0, 0.0]) t0 = np.array([0.0, 0.0, 0.0]) t1 = np.array([10.0, -4.0, 2.0]) for alpha in (0.0, 0.3, 0.5, 1.0): _, t = slerp_pose(q, t0, q, t1, alpha) np.testing.assert_allclose(t, (1 - alpha) * t0 + alpha * t1, atol=1e-12) def test_slerp_pose_double_cover_takes_short_arc(): """q1 and -q1 are the same rotation; slerp must yield the same (short-arc) result.""" rng = np.random.default_rng(99) q0 = _rand_unit_quat(rng) q1 = _rand_unit_quat(rng) for alpha in (0.2, 0.5, 0.8): qa, _ = slerp_pose(q0, [0, 0, 0], q1, [0, 0, 0], alpha) qb, _ = slerp_pose(q0, [0, 0, 0], -q1, [0, 0, 0], alpha) np.testing.assert_allclose(quat_to_mat(qa), quat_to_mat(qb), atol=1e-12) def test_slerp_pose_midpoint_is_half_angle(): """A 180-degree-ish pair: the midpoint rotation angle is half the endpoint angle.""" q0 = np.array([1.0, 0.0, 0.0, 0.0]) # identity ang = np.radians(100.0) q1 = np.array([np.cos(ang / 2), 0.0, np.sin(ang / 2), 0.0]) # yaw 100 deg about Y qm, _ = slerp_pose(q0, [0, 0, 0], q1, [0, 0, 0], 0.5) # rotation angle of qm relative to identity should be ~50 deg angle = 2.0 * np.arccos(min(1.0, abs(qm[0]))) assert abs(np.degrees(angle) - 50.0) < 1e-6 # --- ray_from_pixel -------------------------------------------------------- def test_ray_from_pixel_origin_is_camera_center(): """The ray origin equals the Three.js camera center from the frozen contract.""" rng = np.random.default_rng(5) for _ in range(20): q = _rand_unit_quat(rng) t = rng.uniform(-5, 5, size=3) position, _ = colmap_to_threejs(q, t) origin, _ = ray_from_pixel(q, t, 600, 600, 320, 180, 320, 180) np.testing.assert_allclose(origin, position, atol=1e-9) def test_ray_from_pixel_center_is_forward_axis(): """The center pixel unprojects along the camera forward (+z) axis, in world coords.""" q = np.array([1.0, 0.0, 0.0, 0.0]) # identity world->cam t = np.array([0.0, 0.0, -10.0]) # camera center at (0,0,10) origin, direction = ray_from_pixel(q, t, 500, 500, 320, 180, 320, 180) np.testing.assert_allclose(origin, [0.0, 0.0, 10.0], atol=1e-9) np.testing.assert_allclose(direction, [0.0, 0.0, 1.0], atol=1e-9) def _project_colmap(q, t, fx, fy, cx, cy, X): """Forward COLMAP pinhole projection of a world point X -> pixel (px, py).""" R = quat_to_mat(q) xc = R @ np.asarray(X, float) + np.asarray(t, float) return fx * xc[0] / xc[2] + cx, fy * xc[1] / xc[2] + cy, xc[2] def test_ray_from_pixel_is_projection_inverse(): """A world point in front of the camera lies exactly on the ray through its pixel.""" rng = np.random.default_rng(7) for _ in range(50): q = _rand_unit_quat(rng) t = rng.uniform(-3, 3, size=3) fx = fy = rng.uniform(400, 800) cx, cy = 320.0, 180.0 C = -quat_to_mat(q).T @ t # camera center forward = quat_to_mat(q).T @ np.array([0, 0, 1.0]) X = C + rng.uniform(2, 8) * forward + rng.uniform(-1, 1, size=3) # in front px, py, zc = _project_colmap(q, t, fx, fy, cx, cy, X) if zc <= 0.1: continue origin, direction = ray_from_pixel(q, t, fx, fy, cx, cy, px, py) to_X = X - origin # X - origin must be parallel to direction and in front (positive projection) cross = np.cross(to_X, direction) assert np.linalg.norm(cross) < 1e-6 * (1 + np.linalg.norm(to_X)) assert np.dot(to_X, direction) > 0 # --- triangulate_rays ------------------------------------------------------ def test_triangulate_rays_intersecting(): P = np.array([1.0, 2.0, 3.0]) oa = np.array([0.0, 0.0, 0.0]) ob = np.array([4.0, 0.0, 0.0]) point, gap = triangulate_rays(oa, P - oa, ob, P - ob) np.testing.assert_allclose(point, P, atol=1e-9) assert gap < 1e-9 def test_triangulate_rays_skew_known_geometry(): """Ray A along +x at z=0; ray B along +y at z=1. Closest points (0,0,0),(0,0,1).""" oa = np.array([0.0, 0.0, 0.0]); da = np.array([1.0, 0.0, 0.0]) ob = np.array([0.0, 0.0, 1.0]); db = np.array([0.0, 1.0, 0.0]) point, gap = triangulate_rays(oa, da, ob, db) np.testing.assert_allclose(point, [0.0, 0.0, 0.5], atol=1e-9) assert abs(gap - 1.0) < 1e-9 def test_triangulate_rays_parallel_is_safe(): """Parallel rays must not divide-by-zero; gap ~ their separation.""" oa = np.array([0.0, 0.0, 0.0]); da = np.array([1.0, 0.0, 0.0]) ob = np.array([0.0, 2.0, 0.0]); db = np.array([1.0, 0.0, 0.0]) point, gap = triangulate_rays(oa, da, ob, db) assert np.all(np.isfinite(point)) assert abs(gap - 2.0) < 1e-6 def test_triangulate_rays_from_two_cameras_recovers_point(): """Two cameras looking at a world point; triangulating their pixel rays recovers it.""" rng = np.random.default_rng(123) P = np.array([0.5, 1.0, -0.5]) fx = fy = 600.0; cx, cy = 320.0, 180.0 rays = [] for center in ([6.0, 2.0, 6.0], [-6.0, 2.5, 6.0]): C = np.array(center, float) z = P - C; z /= np.linalg.norm(z) up = np.array([0.0, 1.0, 0.0]) up = up - np.dot(up, z) * z; up /= np.linalg.norm(up) x = np.cross(z, up); y = -up R_c2w = np.column_stack([x, y, z]); R = R_c2w.T t = -R @ C q = mat_to_quat(R) px, py, _ = _project_colmap(q, t, fx, fy, cx, cy, P) rays.append(ray_from_pixel(q, t, fx, fy, cx, cy, px, py)) point, gap = triangulate_rays(rays[0][0], rays[0][1], rays[1][0], rays[1][1]) assert np.linalg.norm(point - P) < 0.2 # spec M8 acceptance tolerance assert gap < 1e-6 # --- nearest_point_on_ray -------------------------------------------------- def test_nearest_point_on_ray_hits_within_radius(): o = np.array([0.0, 0.0, 0.0]); d = np.array([0.0, 0.0, 1.0]) points = np.array([ [0.1, 0.0, 5.0], # perp 0.1, in front -> candidate [0.05, 0.0, 3.0], # perp 0.05, in front -> closest [2.0, 0.0, 5.0], # perp 2.0 -> outside radius ]) got = nearest_point_on_ray(o, d, points, radius=0.3) np.testing.assert_allclose(got, [0.05, 0.0, 3.0], atol=1e-12) def test_nearest_point_on_ray_excludes_outside_and_behind(): o = np.array([0.0, 0.0, 0.0]); d = np.array([0.0, 0.0, 1.0]) behind = np.array([[0.05, 0.0, -3.0]]) # in cylinder but behind origin outside = np.array([[1.0, 0.0, 3.0]]) # in front but outside radius assert nearest_point_on_ray(o, d, behind, radius=0.3) is None assert nearest_point_on_ray(o, d, outside, radius=0.3) is None assert nearest_point_on_ray(o, d, np.zeros((0, 3)), radius=0.3) is None def test_nearest_point_on_ray_synthetic_stage_corner(): """M8 single-view fallback: a ray toward a stage corner selects that corner point.""" from festival4d import synthetic corner = np.array(synthetic.STAGE_CORNERS["Stage FL"], float) points, _ = synthetic.generate_point_cloud() # a camera looking straight at the corner; ray through the image center hits it q, t = synthetic.look_at_colmap((5.0, 3.0, 9.0), corner) intr = synthetic.intrinsics(640, 360) origin, direction = ray_from_pixel( q, t, intr["fx"], intr["fy"], intr["cx"], intr["cy"], intr["cx"], intr["cy"]) got = nearest_point_on_ray(origin, direction, points.astype(float), radius=0.3) assert got is not None np.testing.assert_allclose(got, corner, atol=1e-4)