monster/festifun
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
bb542960c2
festifun/backend/tests/test_geometry.py
m3ultra bb542960c2 Lane B (M2 + M8 geometry): COLMAP reconstruction, geometry math, graceful degradation 
Implements spec M2 (frame sampling, COLMAP orchestration, TXT-model parsing, scene
normalization, pose interpolation, PLY + DB export) and the M8 geometry functions.

geometry.py (M8): slerp_pose (shortest-arc, double-cover), ray_from_pixel (COLMAP
+y-down back-projection), triangulate_rays (closest-point + parallel guard),
nearest_point_on_ray (in-front radius cylinder). Frozen colmap_to_threejs untouched.

frames.py (M2): variance-of-Laplacian sharpness + windowed sharpest-frame sampling.

sfm.py (M2): hand-written COLMAP images/cameras/points3D parsers; normalize_scene
(centroid->0, camera sphere r->10, up->+Y) as one similarity transform over points+poses;
interpolate_poses (slerp+lerp, no extrapolation); build-aware COLMAP CLI orchestration
(3.x/4.x option detection, CPU SIFT, single-camera-per-folder, undistort->TXT, largest
component by images.bin header count); run_reconstruct with full graceful degradation
(COLMAP absent / <60% / <2 videos / no frames / missing files -> poses left untouched).

Tests: 61 pass (24 foundation + 37 lane-B) with independent oracles (scipy Slerp,
projection-inverse, closed-form geometry) and every degradation/DB-safety branch covered.
Validated end-to-end against real COLMAP 4.1.0 (6 components, largest selected, weak ->
graceful degradation, exit 0).

Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
2026-07-16 08:40:46 +10:00

334 lines
14 KiB
Python
Raw Blame History

"""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)
Reference in New Issue View Git Blame Copy Permalink