diff --git a/docs/CHANGELOG.md b/docs/CHANGELOG.md index aa528d58..fd1a42f4 100644 --- a/docs/CHANGELOG.md +++ b/docs/CHANGELOG.md @@ -7,6 +7,13 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +### Added +- **Clubhead-pose geometry (`openflight.camera.clubpose`).** The club mesh and + its normalization tooling, the calibrated camera model with silhouette + projection, and sub-pixel location of the teed ball, which anchors the world + frame and the camera range. First stage of the clubface impact-location work; + not yet wired into the shot pipeline. + ### Added - **Automatic OV9281 exposure control.** High-speed camera capture now measures the impact area every five seconds, restores the last known-good setting at diff --git a/scripts/analysis/download_club_mesh.py b/scripts/analysis/download_club_mesh.py new file mode 100644 index 00000000..c306ca39 --- /dev/null +++ b/scripts/analysis/download_club_mesh.py @@ -0,0 +1,185 @@ +"""Acquire and normalize the club meshes without vendoring them. + +The 7-iron used by this research is a GrabCAD community model. It is not +redistributed, so you fetch your own copy under GrabCAD's terms and point this +script at it; see SOURCES.md for the link, the expected SHA-256 and the licence +position. Run from the repository root: + + uv run python scripts/analysis/download_club_mesh.py \n --local-iron "/path/to/690CB 7-iron.STL" + +""" + +from __future__ import annotations + +import argparse +import json +import sys +from pathlib import Path +from typing import Any + +ROOT = Path(__file__).resolve().parents[2] +sys.path.insert(0, str(ROOT / "src")) + +from openflight.camera.clubpose.mesh import ( # noqa: E402 + ACTIVE_MESH_SOURCES, + CATEGORY_DIMENSIONS_MM, + MESH_SOURCES, + MeshSource, + admit_mesh, + default_mesh_asset_root, + detect_face_plane, + face_detection_record, + load_binary_stl, + load_normalized_mesh, + normalize_clubhead, + save_normalized_mesh, +) + +_NORMALIZATION_VERSION = "geometric-face-anchor-v2" + + +def validate_source_metadata(source: MeshSource, metadata: dict[str, Any]) -> None: + """Fail closed if identity, author, download status, or license drifted.""" + if str(metadata.get("uid")) != source.uid or str(metadata.get("name")) != source.name: + raise ValueError(f"source identity changed for {source.club}") + if not bool(metadata.get("isDownloadable")): + raise ValueError(f"source is no longer downloadable for {source.club}") + author = str(metadata.get("user", {}).get("displayName", "")) + if author != source.author: + raise ValueError(f"source author changed for {source.club}: {author!r}") + license_payload = metadata.get("license", {}) + license_url = str(license_payload.get("url", "")).replace("http://", "https://") + if str(license_payload.get("label")) != "CC Attribution" or license_url.rstrip( + "/" + ) != source.license_url.rstrip("/"): + raise ValueError(f"source license changed for {source.club}") + + +def import_local_stl( + source_path: Path | str, + output_root: Path, + *, + expected_sha256: str | None = None, +) -> dict[str, Any]: + """Import the registered maintainer-local 7-iron without copying its STL.""" + source = MESH_SOURCES["poc_7iron"] + registered_hash = source.expected_source_sha256 + if expected_sha256 is not None and registered_hash is not None: + if expected_sha256.lower() != registered_hash.lower(): + raise ValueError("caller SHA-256 does not match the frozen local-source registration") + required_hash = expected_sha256 or registered_hash + loaded = load_binary_stl(source_path, source_uid=source.uid, expected_sha256=required_hash) + admission = admit_mesh( + loaded, + category_dimensions_mm=CATEGORY_DIMENSIONS_MM[source.club], + source_units_mm=True, + ) + if not admission.accepted: + raise ValueError(f"mesh admission failed for {source.club}: {admission.reasons}") + assert admission.face is not None + normalized = normalize_clubhead( + loaded, + CATEGORY_DIMENSIONS_MM[source.club], + source_units_mm=True, + ) + normalized_face = detect_face_plane(normalized) + asset_metadata = { + "source_uid": source.uid, + "source_name": source.name, + "author": source.author, + "page_url": source.page_url, + "license_spdx": source.license_spdx, + "license_url": source.license_url, + "source_file_sha256": loaded.source_sha256, + "download_format": "binary_stl_maintainer_local", + "redistribution": "prohibited; local research use only", + "normalization": _NORMALIZATION_VERSION, + "source_units_mm": True, + "category_dimensions_mm": CATEGORY_DIMENSIONS_MM[source.club], + "geometry_sha256": admission.geometry_sha256, + "component_count_after_weld": admission.component_count, + "boundary_edge_count_after_weld": admission.boundary_edge_count, + "boundary_edge_fraction_after_weld": admission.boundary_edge_fraction, + "dimensions_before_normalization_mm": admission.dimensions_mm, + "face_detection_source": face_detection_record(admission.face), + "face_detection_normalized": face_detection_record(normalized_face), + "source_vertex_count": int(len(loaded.vertices_local_mm)), + "source_triangle_count": int(len(loaded.faces)), + "clubhead_vertex_count": int(len(normalized.vertices_local_mm)), + "clubhead_triangle_count": int(len(normalized.faces)), + "trademark_note": "synthetic truth only; no Titleist endorsement implied", + } + asset_path = output_root / f"{source.club}.npz" + asset_sha256 = save_normalized_mesh(asset_path, normalized, asset_metadata) + record = {**asset_metadata, "asset_path": asset_path.name, "asset_sha256": asset_sha256} + print(json.dumps(record, indent=2, sort_keys=True)) + return record + + +def _existing_record(source: MeshSource, output_root: Path) -> dict[str, Any] | None: + asset_path = output_root / f"{source.club}.npz" + if not asset_path.is_file(): + return None + mesh, metadata, asset_sha256 = load_normalized_mesh(str(asset_path.resolve())) + if mesh.source_uid != source.uid: + raise ValueError(f"cached source identity mismatch for {source.club}") + if source.expected_source_sha256 is not None and ( + mesh.source_sha256 != source.expected_source_sha256 + ): + raise ValueError(f"cached source SHA-256 mismatch for {source.club}") + if source.expected_asset_sha256 is not None and (asset_sha256 != source.expected_asset_sha256): + raise ValueError(f"cached asset SHA-256 mismatch for {source.club}") + if metadata.get("normalization") != _NORMALIZATION_VERSION: + return None + return {**metadata, "asset_path": asset_path.name, "asset_sha256": asset_sha256} + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--output", type=Path, default=default_mesh_asset_root()) + parser.add_argument("--local-iron", type=Path) + args = parser.parse_args() + args.output.mkdir(parents=True, exist_ok=True) + iron = ACTIVE_MESH_SOURCES["poc_7iron"] + iron_record = _existing_record(iron, args.output) + if iron_record is None: + if args.local_iron is None: + parser.error("--local-iron is required to import the missing maintainer-local 690CB") + stl = Path(args.local_iron).expanduser() + if not stl.is_file(): + parser.error( + f"no STL at {stl}. Fetch the 690CB 7-iron from the GrabCAD page in " + "src/openflight/camera/clubpose/meshes/SOURCES.md (free account, " + "their terms) and point --local-iron at the downloaded file." + ) + iron_record = import_local_stl(stl, args.output) + records = [iron_record] + (args.output / "manifest.json").write_text( + json.dumps( + { + "sources": records, + "retired_sources": [ + { + "club": source.club, + "source_uid": source.uid, + "status": source.status, + "reason": source.status_reason, + } + for source in MESH_SOURCES.values() + if source.status != "active" + ], + }, + indent=2, + sort_keys=True, + ) + + "\n", + encoding="utf-8", + ) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) + (admit_mesh,) + (detect_face_plane,) + (face_detection_record,) diff --git a/src/openflight/camera/clubpose/__init__.py b/src/openflight/camera/clubpose/__init__.py new file mode 100644 index 00000000..41bbea13 --- /dev/null +++ b/src/openflight/camera/clubpose/__init__.py @@ -0,0 +1,9 @@ +"""Clubhead pose from the behind-ball camera and the radars. + +This package lands in stages. This stage carries the geometry: the club mesh +(mesh), the camera model and silhouette projection (projection), and sub-pixel +location of the teed ball (teed_ball), which anchors the world frame. The pose +fit that consumes them follows, and nothing here is wired into the shot +pipeline yet. See docs/clubface-impact-location-report.md for what is +validated and what is not. +""" diff --git a/src/openflight/camera/clubpose/mesh.py b/src/openflight/camera/clubpose/mesh.py new file mode 100644 index 00000000..e6d188c6 --- /dev/null +++ b/src/openflight/camera/clubpose/mesh.py @@ -0,0 +1,563 @@ +"""License-pinned mesh acquisition helpers and a NumPy silhouette renderer.""" + +from __future__ import annotations + +import base64 +import hashlib +import json +import math +import struct +import zipfile +from dataclasses import dataclass +from functools import lru_cache +from pathlib import Path +from typing import Any + +import numpy as np + + +@dataclass(frozen=True) +class MeshSource: + club: str + uid: str + name: str + author: str + page_url: str + license_spdx: str + license_url: str + downloadable: bool + published_triangles: int + source_kind: str = "maintainer_local_binary_stl" + expected_source_sha256: str | None = None + expected_asset_sha256: str | None = None + status: str = "active" + status_reason: str = "" + + +MESH_SOURCES = { + "poc_7iron": MeshSource( + club="poc_7iron", + uid="grabcad:titleist-7-iron-golf-club-1:690cb-right-handed", + name="Titleist 690CB 7-iron golf club", + author="GrabCAD Community contributor", + page_url="https://grabcad.com/library/titleist-7-iron-golf-club-1", + license_spdx="LicenseRef-GrabCAD-Local-Research-Only", + license_url=("https://help.grabcad.com/article/246-how-can-models-be-used-and-shared"), + downloadable=False, + published_triangles=26_238, + source_kind="maintainer_local_binary_stl", + expected_source_sha256=("f35936799295e6ce344279e557f0265ccbb8acef69c4508daff80d219d03cb85"), + ), +} + +ACTIVE_MESH_SOURCES = { + club: source for club, source in MESH_SOURCES.items() if source.status == "active" +} +CATEGORY_DIMENSIONS_MM = { + "poc_7iron": {"width": 80.0, "height": 50.0, "depth": 38.0}, +} + + +@dataclass(frozen=True) +class TriangleMesh: + """Triangle mesh in club-local coordinates: +x depth, +y width, +z height.""" + + vertices_local_mm: np.ndarray + faces: np.ndarray + source_uid: str + source_sha256: str + + def __post_init__(self) -> None: + vertices = np.asarray(self.vertices_local_mm, dtype=float) + faces = np.asarray(self.faces, dtype=np.int32) + if vertices.ndim != 2 or vertices.shape[1] != 3: + raise ValueError("mesh vertices must have shape [N,3]") + if faces.ndim != 2 or faces.shape[1] != 3: + raise ValueError("mesh faces must have shape [M,3]") + if faces.size and (int(faces.min()) < 0 or int(faces.max()) >= len(vertices)): + raise ValueError("mesh face index is outside the vertex array") + if not np.all(np.isfinite(vertices)): + raise ValueError("mesh vertices must be finite") + object.__setattr__(self, "vertices_local_mm", vertices) + object.__setattr__(self, "faces", faces) + + +@dataclass(frozen=True) +class FacePlaneDetection: + normal_source: np.ndarray + width_axis_source: np.ndarray + height_axis_source: np.ndarray + centroid_source: np.ndarray + coherent_area_mm2: float + face_span_mm: np.ndarray + triangle_indices: np.ndarray + + +@dataclass(frozen=True) +class MeshAdmission: + accepted: bool + reasons: tuple[str, ...] + component_count: int + boundary_edge_count: int + boundary_edge_fraction: float + geometry_sha256: str + dimensions_mm: dict[str, float] + face: FacePlaneDetection | None + + +def face_detection_record(face: FacePlaneDetection) -> dict[str, Any]: + """Serialize the required face-plane provenance without embedding geometry.""" + return { + "normal": face.normal_source.tolist(), + "coherent_area_mm2": face.coherent_area_mm2, + "face_span_mm": face.face_span_mm.tolist(), + "triangle_count": int(len(face.triangle_indices)), + } + + +_COMPONENT_DTYPES = { + 5120: np.dtype("i1"), + 5121: np.dtype("u1"), + 5122: np.dtype(" np.ndarray: + accessor = document["accessors"][index] + if "sparse" in accessor: + raise ValueError("sparse glTF accessors are not supported") + view = document["bufferViews"][accessor["bufferView"]] + dtype = _COMPONENT_DTYPES[int(accessor["componentType"])] + width = _TYPE_WIDTHS[str(accessor["type"])] + count = int(accessor["count"]) + offset = int(view.get("byteOffset", 0)) + int(accessor.get("byteOffset", 0)) + stride = int(view.get("byteStride", dtype.itemsize * width)) + raw = buffers[int(view["buffer"])] + if stride == dtype.itemsize * width: + values = np.frombuffer(raw, dtype=dtype, count=count * width, offset=offset) + return values.reshape(count, width).copy() + output = np.empty((count, width), dtype=dtype) + for row in range(count): + output[row] = np.frombuffer(raw, dtype=dtype, count=width, offset=offset + row * stride) + return output + + +def _quaternion_matrix(value: list[float]) -> np.ndarray: + x, y, z, w = (float(item) for item in value) + norm = math.sqrt(x * x + y * y + z * z + w * w) + if norm == 0.0: + return np.eye(4) + x, y, z, w = x / norm, y / norm, z / norm, w / norm + return np.array( + [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w), 0], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w), 0], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y), 0], + [0, 0, 0, 1], + ], + dtype=float, + ) + + +def _node_matrix(node: dict[str, Any]) -> np.ndarray: + if "matrix" in node: + return np.asarray(node["matrix"], dtype=float).reshape(4, 4, order="F") + translation = np.eye(4) + translation[:3, 3] = np.asarray(node.get("translation", [0, 0, 0]), dtype=float) + scale = np.eye(4) + scale[np.arange(3), np.arange(3)] = np.asarray(node.get("scale", [1, 1, 1]), dtype=float) + return translation @ _quaternion_matrix(node.get("rotation", [0, 0, 0, 1])) @ scale + + +def _archive_buffer(bundle: zipfile.ZipFile, gltf_dir: str, uri: str) -> bytes: + if uri.startswith("data:"): + return base64.b64decode(uri.split(",", 1)[1]) + return bundle.read(str(Path(gltf_dir, uri)).replace("\\", "/")) + + +def load_gltf_archive( + archive_path: Path | str, *, source_uid: str, source_sha256: str +) -> TriangleMesh: + """Load triangle primitives and scene-node transforms from a Sketchfab glTF ZIP.""" + with zipfile.ZipFile(archive_path) as bundle: + gltf_names = sorted(name for name in bundle.namelist() if name.lower().endswith(".gltf")) + if len(gltf_names) != 1: + raise ValueError("download archive must contain exactly one .gltf scene") + gltf_name = gltf_names[0] + document = json.loads(bundle.read(gltf_name)) + gltf_dir = str(Path(gltf_name).parent) + buffers = [ + _archive_buffer(bundle, gltf_dir, str(item["uri"])) for item in document["buffers"] + ] + + vertices: list[np.ndarray] = [] + faces: list[np.ndarray] = [] + + def visit(node_index: int, parent: np.ndarray) -> None: + node = document["nodes"][node_index] + world = parent @ _node_matrix(node) + if "mesh" in node: + mesh = document["meshes"][int(node["mesh"])] + for primitive in mesh["primitives"]: + if int(primitive.get("mode", 4)) != 4: + raise ValueError("only glTF TRIANGLES primitives are supported") + position = _read_accessor( + document, buffers, int(primitive["attributes"]["POSITION"]) + ).astype(float) + transformed = np.column_stack([position, np.ones(len(position))]) @ world.T + transformed = transformed[:, :3] / transformed[:, 3, None] + if "indices" in primitive: + triangle = _read_accessor(document, buffers, int(primitive["indices"])).reshape( + -1 + ) + else: + triangle = np.arange(len(position), dtype=np.int32) + if len(triangle) % 3: + raise ValueError("triangle index count is not divisible by three") + offset = sum(len(item) for item in vertices) + vertices.append(transformed) + faces.append(triangle.reshape(-1, 3).astype(np.int32) + offset) + for child in node.get("children", []): + visit(int(child), world) + + scene_index = int(document.get("scene", 0)) + for root in document["scenes"][scene_index].get("nodes", []): + visit(int(root), np.eye(4)) + if not vertices or not faces: + raise ValueError("glTF scene contains no triangle geometry") + return TriangleMesh(np.vstack(vertices), np.vstack(faces), source_uid, source_sha256) + + +def load_binary_stl( + path: Path | str, *, source_uid: str, expected_sha256: str | None +) -> TriangleMesh: + """Decode a binary STL after an optional fail-closed source-hash check.""" + payload = Path(path).read_bytes() + digest = hashlib.sha256(payload).hexdigest() + if expected_sha256 is not None and digest.lower() != expected_sha256.lower(): + raise ValueError( + f"binary STL SHA-256 mismatch: expected {expected_sha256.lower()}, got {digest}" + ) + if len(payload) < 84: + raise ValueError("binary STL is shorter than its 84-byte header") + triangle_count = struct.unpack_from(" list[np.ndarray]: + # Weld identical seam vertices before topology traversal. glTF material + # primitives commonly duplicate vertices along an otherwise connected shell. + _, welded = np.unique(np.round(vertices, decimals=9), axis=0, return_inverse=True) + welded_faces = welded[faces] + parent = np.arange(int(welded.max()) + 1, dtype=np.int32) + + def root(index: int) -> int: + while parent[index] != index: + parent[index] = parent[parent[index]] + index = int(parent[index]) + return index + + for a, b, c in welded_faces: + anchor = root(int(a)) + for other in (int(b), int(c)): + other_root = root(other) + if anchor != other_root: + parent[other_root] = anchor + labels = np.array([root(int(face[0])) for face in welded_faces], dtype=np.int32) + return [np.flatnonzero(labels == label) for label in np.unique(labels)] + + +def _welded_faces(vertices: np.ndarray, faces: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + welded_vertices, welded = np.unique( + np.round(np.asarray(vertices, dtype=float), decimals=8), axis=0, return_inverse=True + ) + return welded_vertices, welded[np.asarray(faces, dtype=np.int32)] + + +def geometry_hash(mesh: TriangleMesh) -> str: + """Hash triangle geometry independent of source vertex/face ordering.""" + triangles = np.round(mesh.vertices_local_mm[mesh.faces], decimals=8) + canonical_triangles = [] + for triangle in triangles: + order = np.lexsort((triangle[:, 2], triangle[:, 1], triangle[:, 0])) + canonical_triangles.append(triangle[order].reshape(-1)) + canonical = np.asarray(canonical_triangles) + order = np.lexsort(tuple(canonical[:, column] for column in reversed(range(9)))) + return hashlib.sha256(np.ascontiguousarray(canonical[order]).tobytes()).hexdigest() + + +def _triangle_adjacency(welded_faces: np.ndarray) -> list[set[int]]: + edge_owners: dict[tuple[int, int], list[int]] = {} + for face_index, face in enumerate(welded_faces): + for first, second in ((face[0], face[1]), (face[1], face[2]), (face[2], face[0])): + edge = tuple(sorted((int(first), int(second)))) + edge_owners.setdefault(edge, []).append(face_index) + adjacency = [set() for _ in welded_faces] + for owners in edge_owners.values(): + for owner in owners: + adjacency[owner].update(other for other in owners if other != owner) + return adjacency + + +def detect_face_plane( + mesh: TriangleMesh, + *, + normal_tolerance_deg: float = 15.0, + aspect_bounds: tuple[float, float] = (0.35, 0.65), +) -> FacePlaneDetection: + """Find the largest coherent extremity plane with a clubface-like lens aspect.""" + vertices = mesh.vertices_local_mm + triangles = vertices[mesh.faces] + cross = np.cross(triangles[:, 1] - triangles[:, 0], triangles[:, 2] - triangles[:, 0]) + double_area = np.linalg.norm(cross, axis=1) + valid = double_area > 1e-10 + normals = np.zeros_like(cross) + normals[valid] = cross[valid] / double_area[valid, None] + areas = double_area / 2.0 + _, welded_faces = _welded_faces(vertices, mesh.faces) + adjacency = _triangle_adjacency(welded_faces) + cosine_limit = math.cos(math.radians(normal_tolerance_deg)) + unassigned = set(np.flatnonzero(valid).tolist()) + candidates: list[FacePlaneDetection] = [] + mesh_center = np.mean(vertices, axis=0) + while unassigned: + seed = max(unassigned, key=lambda index: float(areas[index])) + seed_normal = normals[seed] + region = set() + pending = [seed] + while pending: + current = pending.pop() + if current not in unassigned: + continue + if abs(float(normals[current] @ seed_normal)) < cosine_limit: + continue + unassigned.remove(current) + region.add(current) + pending.extend(adjacency[current] & unassigned) + if not region: + continue + indices = np.asarray(sorted(region), dtype=np.int32) + aligned = normals[indices] * np.sign(normals[indices] @ seed_normal)[:, None] + normal = np.sum(aligned * areas[indices, None], axis=0) + normal /= np.linalg.norm(normal) + vertex_indices = np.unique(mesh.faces[indices].reshape(-1)) + points = vertices[vertex_indices] + centroid = np.average(np.mean(triangles[indices], axis=1), weights=areas[indices], axis=0) + if float(normal @ (centroid - mesh_center)) < 0.0: + normal *= -1.0 + centered = points - centroid + planar = centered - np.outer(centered @ normal, normal) + _, _, axes = np.linalg.svd(planar, full_matrices=False) + width_axis = axes[0] - normal * float(axes[0] @ normal) + width_axis /= np.linalg.norm(width_axis) + if width_axis[int(np.argmax(np.abs(width_axis)))] < 0.0: + width_axis *= -1.0 + height_axis = np.cross(normal, width_axis) + spans = np.array([np.ptp(points @ width_axis), np.ptp(points @ height_axis)], dtype=float) + if spans[1] > spans[0]: + spans = spans[::-1] + width_axis, height_axis = height_axis, -width_axis + aspect = float(spans[1] / max(spans[0], 1e-12)) + projection = vertices @ normal + extremity_distance = min( + abs(float(centroid @ normal) - float(np.min(projection))), + abs(float(np.max(projection)) - float(centroid @ normal)), + ) + depth = float(np.ptp(projection)) + flatness = float(np.max(np.abs(centered @ normal))) + if ( + aspect_bounds[0] <= aspect <= aspect_bounds[1] + and extremity_distance <= max(1.0, 0.10 * depth) + and flatness <= max(1.0, 0.04 * spans[0]) + ): + candidates.append( + FacePlaneDetection( + normal_source=normal, + width_axis_source=width_axis, + height_axis_source=height_axis, + centroid_source=centroid, + coherent_area_mm2=float(np.sum(areas[indices])), + face_span_mm=spans, + triangle_indices=indices, + ) + ) + if not candidates: + raise ValueError("no coherent extremity plane has the registered clubface lens aspect") + return max(candidates, key=lambda item: item.coherent_area_mm2) + + +def _boundary_edge_count(mesh: TriangleMesh) -> int: + _, faces = _welded_faces(mesh.vertices_local_mm, mesh.faces) + counts: dict[tuple[int, int], int] = {} + for face in faces: + for first, second in ((face[0], face[1]), (face[1], face[2]), (face[2], face[0])): + edge = tuple(sorted((int(first), int(second)))) + counts[edge] = counts.get(edge, 0) + 1 + return sum(count != 2 for count in counts.values()) + + +def admit_mesh( + mesh: TriangleMesh, + *, + category_dimensions_mm: dict[str, float], + source_units_mm: bool, + tolerance_fraction: float = 0.15, +) -> MeshAdmission: + """Apply the frozen CAD-corpus admission checks before normalization.""" + components = _connected_face_components(mesh.vertices_local_mm, mesh.faces) + boundary_edges = _boundary_edge_count(mesh) + edge_denominator = max(1.0, 1.5 * len(mesh.faces)) + boundary_fraction = boundary_edges / edge_denominator + reasons = [] + if len(components) != 1: + reasons.append("component_count") + if boundary_fraction > 0.001: + reasons.append("open_boundary") + try: + face = detect_face_plane(mesh) + except ValueError: + face = None + reasons.append("face_plane_missing") + dimensions: dict[str, float] = {} + if face is not None: + dimensions = { + "width": float(face.face_span_mm[0]), + "height": float(face.face_span_mm[1]), + "depth": float(np.ptp(mesh.vertices_local_mm @ face.normal_source)), + } + if source_units_mm: + for name, nominal in category_dimensions_mm.items(): + relative = abs(dimensions[name] - float(nominal)) / float(nominal) + if relative > tolerance_fraction + 0.001: + reasons.append(f"dimension_{name}") + return MeshAdmission( + accepted=not reasons, + reasons=tuple(reasons), + component_count=len(components), + boundary_edge_count=boundary_edges, + boundary_edge_fraction=boundary_fraction, + geometry_sha256=geometry_hash(mesh), + dimensions_mm=dimensions, + face=face, + ) + + +def normalize_clubhead( + mesh: TriangleMesh, + dimensions_mm: dict[str, float], + *, + source_units_mm: bool = False, +) -> TriangleMesh: + """Anchor axes to the detected face plane; preserve trusted metric CAD scale.""" + components = _connected_face_components(mesh.vertices_local_mm, mesh.faces) + if len(components) != 1: + raise ValueError("mesh admission requires one welded connected component") + face = detect_face_plane(mesh) + axes = np.stack([face.normal_source, face.width_axis_source, face.height_axis_source]) + local = mesh.vertices_local_mm @ axes.T + local -= (np.min(local, axis=0) + np.max(local, axis=0)) / 2.0 + if not source_units_mm: + target = np.array( + [dimensions_mm["depth"], dimensions_mm["width"], dimensions_mm["height"]], + dtype=float, + ) + local *= target / np.ptp(local, axis=0) + normalized = TriangleMesh(local, mesh.faces, mesh.source_uid, mesh.source_sha256) + normalized_face = detect_face_plane(normalized) + angle = math.degrees( + math.acos(float(np.clip(normalized_face.normal_source @ np.array([1.0, 0.0, 0.0]), -1, 1))) + ) + if angle > 3.0: + raise ValueError(f"normalized face normal invariant failed: {angle:.3f} degrees") + return normalized + + +def rasterize_projected_triangles( + vertices_uv: np.ndarray, faces: np.ndarray, *, width: int, height: int +) -> np.ndarray: + """Rasterize the union of projected triangles with NumPy scanline intervals.""" + vertices = np.asarray(vertices_uv, dtype=float) + triangles = vertices[np.asarray(faces, dtype=np.int32)] + finite = np.all(np.isfinite(triangles), axis=(1, 2)) + triangles = triangles[finite] + mask = np.zeros((height, width), dtype=bool) + if not len(triangles): + return mask + min_y = np.min(triangles[:, :, 1], axis=1) + max_y = np.max(triangles[:, :, 1], axis=1) + edges_a = triangles[:, [0, 1, 2]] + edges_b = triangles[:, [1, 2, 0]] + for row in range(height): + y = row + 0.5 + active = (min_y <= y) & (max_y >= y) + if not np.any(active): + continue + a = edges_a[active] + b = edges_b[active] + dy = b[:, :, 1] - a[:, :, 1] + crosses = (np.abs(dy) > 1e-12) & ( + (y >= np.minimum(a[:, :, 1], b[:, :, 1])) & (y <= np.maximum(a[:, :, 1], b[:, :, 1])) + ) + safe_dy = np.where(crosses, dy, 1.0) + intersections = a[:, :, 0] + (y - a[:, :, 1]) * (b[:, :, 0] - a[:, :, 0]) / safe_dy + intersections = np.where(crosses, intersections, np.nan) + with np.errstate(all="ignore"): + left = np.nanmin(intersections, axis=1) + right = np.nanmax(intersections, axis=1) + valid = np.isfinite(left) & np.isfinite(right) + starts = np.maximum(0, np.ceil(left[valid] - 0.5).astype(int)) + ends = np.minimum(width - 1, np.floor(right[valid] - 0.5).astype(int)) + visible = starts <= ends + difference = np.zeros(width + 1, dtype=np.int32) + np.add.at(difference, starts[visible], 1) + np.add.at(difference, ends[visible] + 1, -1) + mask[row] = np.cumsum(difference[:-1]) > 0 + return mask + + +def save_normalized_mesh(path: Path | str, mesh: TriangleMesh, metadata: dict[str, Any]) -> str: + """Write a deterministic local cache and return its content SHA-256.""" + path = Path(path) + path.parent.mkdir(parents=True, exist_ok=True) + np.savez( + path, + vertices_local_mm=mesh.vertices_local_mm, + faces=mesh.faces, + source_uid=np.asarray(mesh.source_uid), + source_sha256=np.asarray(mesh.source_sha256), + metadata_json=np.asarray(json.dumps(metadata, sort_keys=True, separators=(",", ":"))), + ) + return hashlib.sha256(path.read_bytes()).hexdigest() + + +@lru_cache(maxsize=8) +def load_normalized_mesh(path: str) -> tuple[TriangleMesh, dict[str, Any], str]: + payload = np.load(path, allow_pickle=False) + mesh = TriangleMesh( + payload["vertices_local_mm"], + payload["faces"], + str(payload["source_uid"]), + str(payload["source_sha256"]), + ) + metadata = json.loads(str(payload["metadata_json"])) + return mesh, metadata, hashlib.sha256(Path(path).read_bytes()).hexdigest() + + +def default_mesh_asset_root() -> Path: + return Path(__file__).resolve().parent / "meshes" / "assets" diff --git a/src/openflight/camera/clubpose/meshes/SOURCES.md b/src/openflight/camera/clubpose/meshes/SOURCES.md new file mode 100644 index 00000000..b553a218 --- /dev/null +++ b/src/openflight/camera/clubpose/meshes/SOURCES.md @@ -0,0 +1,82 @@ +# Phase F1 club-mesh sources and licenses + +No third-party mesh is committed in this directory. Acquisition uses the +publisher's authenticated download endpoint, records the returned archive hash, +and leaves downloaded files ignored by Git. + +## Selected and retired sources + +| Club | Source | Model ID | License | Published geometry | +|---|---|---|---|---:| +| Driver (RETIRED) | [Callaway Maverik Golf Driver](https://sketchfab.com/3d-models/callaway-maverik-golf-driver-978d0740dc514c8695bbb02f4083f0e3), Paul Ekins | `978d0740dc514c8695bbb02f4083f0e3` | [CC BY 4.0](https://creativecommons.org/licenses/by/4.0/) | 41,855 triangles | +| 7-iron | [Titleist 7-iron golf club](https://grabcad.com/library/titleist-7-iron-golf-club-1), GrabCAD Community contributor; maintainer-downloaded 690CB right-handed STL | `grabcad:titleist-7-iron-golf-club-1:690cb-right-handed` | Local research use only; no redistribution | 26,238 triangles | + +The Sketchfab v3 metadata reports the driver as downloadable and labels its +license `CC Attribution`, with author credit required and commercial use allowed. +CC BY 4.0 permits sharing and adaptation with attribution. We still use a local +cache rather than vendoring the archive: it preserves source provenance, does not +add a third-party binary payload to OpenFlight, and follows the design +specification's existing no-redistribution boundary. + +The right-handed Titleist 690CB source is a maintainer-supplied, millimetre-scaled +binary STL from the named GrabCAD model. Its source SHA-256 is +`f35936799295e6ce344279e557f0265ccbb8acef69c4508daff80d219d03cb85`. +It is **local use only**: neither the STL nor its normalized NPZ may be committed. +Only its provenance, hash, attribution, and aggregate evaluation results enter +the repository. The local importer checks this hash before parsing the STL. + +The downloaded models are used only as synthetic truth. Their names or geometry +do not imply endorsement by Callaway Golf or Titleist. + +### Post-F1 source-quality correction + +The Maverik is retired and excluded from active manifests and evaluations. It is +a posed art scene containing grass, a ball, and tens of thousands of disconnected +shell components. Its face/sole geometry is ambiguous, and the former PCA +extent-order normalizer assigned its face normal to the height axis and then +anisotropically distorted the head using an incorrect 55 mm driver depth. It is +not salvageable as canonical driver truth. Driver arms are `HOLD_CAD_MESH` until +the maintainer supplies a locally admitted CAD driver; the corrected driver +category references are 118 mm width, 60 mm height, and 112 mm depth. + +The 690CB was re-imported from the same pinned STL using geometric face anchoring +and trusted source millimetres. Corrected normalized asset SHA-256 is +`d63bf7cf1224eb9ce0c7480967057201a4843f3cc2612e4f779ec48fd0839c8a`; +geometry hash is +`87cfacdf639f8c7203ffdfb7da2c9e7ba60a63ed302fc6dbb5db2aed2b9047e3`. +It has one welded component and 23 boundary edges out of approximately 39,357 +edges (0.058%, retained as a provenance diagnostic). Before normalization, the +detected coherent face patch is 79.739 x 42.497 mm, 863.296 mm2, with source +normal `(0.112969, -0.253749, -0.960650)`. After the rigid axis transform the +normal is `(1, 0, 0)` to numerical precision; no dimension scaling is applied. + +### Pre-outcome iron-source substitution + +The originally selected Sketchfab iron (`dc748ddd268c4acab25c54c4048b3912`) +failed the deliberately strict identity validator before any F1 outcome ran. Its +uploader display name changed from the pinned ASCII `real_slimshady` to +`ℜ𝔢𝔞𝔩 𝔖𝔩𝔦𝔪 𝔖𝔥𝔞𝔡𝔶`. The validator was not relaxed or Unicode-normalized. The +maintainer substituted the higher-resolution local 690CB source above. This is +an acquisition/provenance amendment, not a grid, solver, criterion, or gate +change. + +## Rejected candidates + +- GrabCAD's [library-use guidance](https://help.grabcad.com/article/246-how-can-models-be-used-and-shared) + permits public non-commercial rendering with attribution but does not clearly + grant redistribution of a raw CAD file in an AGPL repository. The maintainer's + Titleist 7-iron is therefore accepted only as an uncommitted local input. +- CGTrader's [Terms and Conditions](https://www.cgtrader.com/pages/terms-and-conditions) + prohibit making a purchased product available as a separate file. Its + Royalty Free License permits an incorporated product, not raw-mesh + redistribution. The candidate `Golf Club 7 Iron` therefore was not purchased, + downloaded, or committed. + +## Required attribution in generated results + +Every F1 result bundle records the model page, attribution, model ID, license or +use boundary, source SHA-256, and normalized mesh SHA-256. Any redistributed +driver render or derived dataset must retain the CC BY credit and indicate that +the geometry was normalized into OpenFlight's calibrated club-local coordinate +frame. The local-use-only iron mesh and normalized asset must never be +redistributed. diff --git a/src/openflight/camera/clubpose/meshes/assets/.gitignore b/src/openflight/camera/clubpose/meshes/assets/.gitignore new file mode 100644 index 00000000..d6b7ef32 --- /dev/null +++ b/src/openflight/camera/clubpose/meshes/assets/.gitignore @@ -0,0 +1,2 @@ +* +!.gitignore diff --git a/src/openflight/camera/clubpose/projection.py b/src/openflight/camera/clubpose/projection.py new file mode 100644 index 00000000..c22ad977 --- /dev/null +++ b/src/openflight/camera/clubpose/projection.py @@ -0,0 +1,330 @@ +"""Frozen Phase 1b club-state solver, promoted to the Phase 3 fusion package.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass + +import cv2 +import numpy as np + +NOMINAL_RANGE_MM = 1_575.0 +CAMERA_HEIGHT_MM = 209.55 +RADAR_STATIC_BIAS_MM = 66.0069821 +BALL_RADIUS_MM = 42.67 / 2.0 +FRAME_TO_IMPACT_S = 1.0e-3 +FRAME_PERIOD_S = 2.137e-3 +MAX_EXTRAPOLATION_S = 2.5e-3 +CENTROID_NOISE_PX = 0.5 +MOMENT_EDGE_NOISE_PX = 0.5 +RANGE_NOISE_MM = 3.0 +FIT_RESIDUAL_LIMIT_PX = 8.0 +AMBIGUITY_RATIO_MIN = 1.10 +MODEL_VERSION = "phase1b-v1-frozen" +RADAR_HEIGHT_MM = 152.4 + +_CAMERA_X_MM = math.sqrt(NOMINAL_RANGE_MM**2 - CAMERA_HEIGHT_MM**2) +CAMERA_CENTER_WORLD = np.array([-_CAMERA_X_MM, 0.0, CAMERA_HEIGHT_MM]) +TARGET_WORLD = np.zeros(3) +WORLD_RIGHT = np.array([0.0, 1.0, 0.0]) +WORLD_UP = np.array([0.0, 0.0, 1.0]) +FACE_NORMAL = np.array([1.0, 0.0, 0.0]) +_FORWARD = (TARGET_WORLD - CAMERA_CENTER_WORLD) / np.linalg.norm(TARGET_WORLD - CAMERA_CENTER_WORLD) +_DOWN = np.cross(WORLD_RIGHT, _FORWARD) +_DOWN /= np.linalg.norm(_DOWN) +_R_WC = np.stack([WORLD_RIGHT, _DOWN, _FORWARD]) +RADAR_CENTER_WORLD = np.array( + [ + -math.sqrt(NOMINAL_RANGE_MM**2 - RADAR_HEIGHT_MM**2), + 0.0, + RADAR_HEIGHT_MM, + ] +) + + +@dataclass(frozen=True) +class CameraPreset: + """One explicit camera/crop configuration from approved spec section 5.""" + + name: str + width: int + height: int + fx: float + fy: float + cx: float + cy: float + plate_scale_px_per_mm: float + sensor_crop: tuple[int, int, int, int] + sampling_increment: tuple[int, int] + isp_offset: tuple[int, int] + orientation: str + gate_b1_passed: bool + physical_status: str + + @property + def horizontal_fov_deg(self) -> float: + return math.degrees(2.0 * math.atan(self.width / (2.0 * self.fx))) + + +@dataclass(frozen=True) +class ClubTemplate: + """Named analytic rear-view silhouette and speed distribution.""" + + name: str + radius_u_mm: float + radius_v_mm: float + speed_mean_mm_s: float + speed_sd_mm_s: float + impact_u_limit_mm: float + impact_v_limit_mm: float + velocity_direction: tuple[float, float, float] + + +@dataclass(frozen=True) +class SilhouetteObservation: + centroid_uv: np.ndarray + covariance_px2: np.ndarray + + +@dataclass(frozen=True) +class ClubState: + ok: bool + reason: str | None + frame_center_world: np.ndarray | None + roll_rad: float | None + fit_residual_px: float | None + calibrated_range_mm: float | None + predicted_covariance_px2: np.ndarray | None + + +def camera_presets() -> dict[str, CameraPreset]: + """Return independent intrinsics; no fixed-FOV scaling is permitted.""" + return { + "A0": CameraPreset( + name="A0", + width=320, + height=200, + fx=1033.0, + fy=1033.0, + cx=160.0, + cy=100.0, + plate_scale_px_per_mm=0.656, + sensor_crop=(336, 150, 816, 516), + sampling_increment=(2, 2), + isp_offset=(4, 4), + orientation="landscape_register_window", + gate_b1_passed=False, + physical_status="existing_320x200_plus_10us_strobe", + ), + "A1": CameraPreset( + name="A1", + width=320, + height=200, + fx=2063.0, + fy=2063.0, + cx=160.0, + cy=100.0, + plate_scale_px_per_mm=1.31, + sensor_crop=(480, 150, 320, 200), + sampling_increment=(1, 1), + isp_offset=(0, 0), + orientation="landscape_crop_metadata_sensitivity", + gate_b1_passed=False, + physical_status="plate_scale_sensitivity_only", + ), + "B": CameraPreset( + name="B", + width=1280, + height=200, + fx=2095.0, + fy=2095.0, + cx=640.0, + cy=100.0, + plate_scale_px_per_mm=1.33, + sensor_crop=(0, 300, 1280, 200), + sampling_increment=(1, 1), + isp_offset=(0, 0), + orientation="portrait_experimental", + gate_b1_passed=False, + physical_status="experimental_gate_b1_not_run", + ), + } + + +def _project(points_world: np.ndarray, camera: CameraPreset) -> tuple[np.ndarray, np.ndarray]: + points = np.asarray(points_world, dtype=float).reshape(-1, 3) + cam = (points - CAMERA_CENTER_WORLD) @ _R_WC.T + in_front = cam[:, 2] > 1e-9 + safe_z = np.where(in_front, cam[:, 2], 1.0) + uv = np.column_stack( + [ + camera.fx * cam[:, 0] / safe_z + camera.cx, + camera.fy * cam[:, 1] / safe_z + camera.cy, + ] + ) + return uv, in_front + + +def _ray_world(uv: np.ndarray, camera: CameraPreset) -> np.ndarray: + xy = np.array([(uv[0] - camera.cx) / camera.fx, (uv[1] - camera.cy) / camera.fy, 1.0]) + ray = xy @ _R_WC + return ray / np.linalg.norm(ray) + + +def _backproject_range( + uv: np.ndarray, + range_mm: float, + camera: CameraPreset, + range_origin_world: np.ndarray = CAMERA_CENTER_WORLD, +) -> np.ndarray: + """Intersect a camera ray with a range sphere around the supplied sensor.""" + ray = _ray_world(uv, camera) + offset = CAMERA_CENTER_WORLD - np.asarray(range_origin_world, dtype=float) + projection = float(offset @ ray) + discriminant = projection**2 - float(offset @ offset) + float(range_mm) ** 2 + if discriminant < 0.0: + return np.full(3, np.nan) + distance = -projection + math.sqrt(discriminant) + return CAMERA_CENTER_WORLD + ray * distance + + +def _range_mm( + point_world: np.ndarray, range_origin_world: np.ndarray = CAMERA_CENTER_WORLD +) -> float: + return float(np.linalg.norm(np.asarray(point_world) - range_origin_world)) + + +def _face_axes(roll_rad: float) -> tuple[np.ndarray, np.ndarray]: + c = math.cos(float(roll_rad)) + s = math.sin(float(roll_rad)) + return c * WORLD_RIGHT + s * WORLD_UP, -s * WORLD_RIGHT + c * WORLD_UP + + +def _velocity(template: ClubTemplate, speed_mm_s: float, reverse: bool = False) -> np.ndarray: + direction = np.asarray(template.velocity_direction, dtype=float) + direction /= np.linalg.norm(direction) + return direction * float(speed_mm_s) * (-1.0 if reverse else 1.0) + + +def _projected_velocity( + center_world: np.ndarray, velocity_world: np.ndarray, camera: CameraPreset +) -> np.ndarray: + dt = 1.0e-5 + uv_pair, front = _project( + np.stack([center_world - velocity_world * dt / 2, center_world + velocity_world * dt / 2]), + camera, + ) + if not bool(np.all(front)): + return np.zeros(2) + return (uv_pair[1] - uv_pair[0]) / dt + + +def _projection_jacobian(center_world: np.ndarray, camera: CameraPreset) -> np.ndarray: + """Pixel derivative for one millimetre along world-right/world-up.""" + epsilon = 1.0e-3 + points = np.stack( + [ + center_world - WORLD_RIGHT * epsilon, + center_world + WORLD_RIGHT * epsilon, + center_world - WORLD_UP * epsilon, + center_world + WORLD_UP * epsilon, + ] + ) + uv, front = _project(points, camera) + if not bool(np.all(front)): + return np.full((2, 2), np.nan) + return np.column_stack([(uv[1] - uv[0]) / (2.0 * epsilon), (uv[3] - uv[2]) / (2.0 * epsilon)]) + + +def _silhouette_moments( + center_world: np.ndarray, + roll_rad: float, + velocity_world: np.ndarray, + exposure_us: float, + camera: CameraPreset, + template: ClubTemplate, +) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: + center_uv, front = _project(center_world[None, :], camera) + if not bool(front[0]): + nan = np.full(2, np.nan) + return nan, np.full((2, 2), np.nan), nan, nan, nan + center_uv = center_uv[0] + axis_u, axis_v = _face_axes(roll_rad) + jacobian = _projection_jacobian(center_world, camera) + if not bool(np.all(np.isfinite(jacobian))): + nan = np.full(2, np.nan) + return nan, np.full((2, 2), np.nan), nan, nan, nan + body_u = np.array([float(axis_u @ WORLD_RIGHT), float(axis_u @ WORLD_UP)]) + body_v = np.array([float(axis_v @ WORLD_RIGHT), float(axis_v @ WORLD_UP)]) + vector_u = jacobian @ body_u * template.radius_u_mm + vector_v = jacobian @ body_v * template.radius_v_mm + blur_vector = _projected_velocity(center_world, velocity_world, camera) * ( + float(exposure_us) * 1e-6 + ) + covariance = ( + np.outer(vector_u, vector_u) / 4.0 + + np.outer(vector_v, vector_v) / 4.0 + + np.outer(blur_vector, blur_vector) / 12.0 + ) + extents = np.sqrt(vector_u**2 + vector_v**2) + np.abs(blur_vector) / 2.0 + return center_uv, covariance, extents, vector_u, vector_v + + +def _visible(center_uv: np.ndarray, extents: np.ndarray, camera: CameraPreset) -> bool: + if not bool(np.all(np.isfinite(center_uv))) or not bool(np.all(np.isfinite(extents))): + return False + return bool( + center_uv[0] - extents[0] >= 0.0 + and center_uv[0] + extents[0] < camera.width + and center_uv[1] - extents[1] >= 0.0 + and center_uv[1] + extents[1] < camera.height + ) + + +def _ball_geometry( + ball_center_world: np.ndarray, camera: CameraPreset +) -> tuple[np.ndarray, np.ndarray]: + center_uv, front = _project(ball_center_world[None, :], camera) + if not bool(front[0]): + return np.full(2, np.nan), np.full(2, np.nan) + center_uv = center_uv[0] + endpoints, endpoint_front = _project( + np.stack( + [ + ball_center_world + WORLD_RIGHT * BALL_RADIUS_MM, + ball_center_world + WORLD_UP * BALL_RADIUS_MM, + ] + ), + camera, + ) + if not bool(np.all(endpoint_front)): + return np.full(2, np.nan), np.full(2, np.nan) + extents = np.abs(endpoints - center_uv).max(axis=0) + return center_uv, extents + + +def _silhouette_polygon( + center_uv: np.ndarray, vector_u: np.ndarray, vector_v: np.ndarray, blur_vector: np.ndarray +) -> np.ndarray: + theta = np.linspace(0.0, 2.0 * np.pi, 24, endpoint=False) + ellipse = ( + center_uv[None, :] + + np.cos(theta)[:, None] * vector_u[None, :] + + np.sin(theta)[:, None] * vector_v[None, :] + ) + points = np.vstack([ellipse - blur_vector / 2.0, ellipse + blur_vector / 2.0]) + return cv2.convexHull(points.astype(np.float32)).reshape(-1, 2) + + +def _polygon_iou(a: np.ndarray, b: np.ndarray) -> float: + area_a = abs(float(cv2.contourArea(a))) + area_b = abs(float(cv2.contourArea(b))) + if area_a <= 0.0 or area_b <= 0.0: + return 0.0 + intersection, _ = cv2.intersectConvexConvex(a.astype(np.float32), b.astype(np.float32)) + union = area_a + area_b - float(intersection) + return float(intersection / union) if union > 0.0 else 0.0 + + +def _normalize_roll(angle: float) -> float: + return (float(angle) + math.pi / 2.0) % math.pi - math.pi / 2.0 diff --git a/src/openflight/camera/clubpose/teed_ball.py b/src/openflight/camera/clubpose/teed_ball.py new file mode 100644 index 00000000..396bc6fb --- /dev/null +++ b/src/openflight/camera/clubpose/teed_ball.py @@ -0,0 +1,321 @@ +"""Polarity-agnostic ball detection with known-radius arc fitting. + +Replaces the fixed-polarity, fixed-magnitude rule used by the superseded +synthetic pipeline (on the fork's feat/silhouette-poc branch): + + ball_mask = frame >= percentile(frame, 10) + 210.0 + +That rule fails on real captures in both directions at once. Measured on the real +OpenFlight archive (`frames.npz`, 2026-08-24): + + at address ball 192 DN on a clipped 255 DN mat -> -62 DN WRONG SIGN + in flight ball 224 DN on a 102 DN dark wall -> +122 DN right sign, + still under +210 + +No constant fixes it, because the background swings 102..255 DN inside one shot. + +What this does instead: + 1. estimate the LOCAL background, so a clipped mat and a dark wall are both handled + 2. threshold the SIGNED residual in BOTH directions, scaled to measured noise + rather than an absolute DN offset + 3. confirm candidates by SHAPE, since the ball is the one object in frame with a + known fixed physical diameter + 4. fit a circle to the reliable part of the boundary, so a ball whose lit side has + blended into the background still yields an honest radius + +Rejections are named, matching the project's fail-closed convention. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field + +import cv2 +import numpy as np + +SENSOR_NOISE_DN = 5.6 # measured: 50 static frames of the real archive at gain 15.94 +SATURATION_DN = 253 # within noise of the 8-bit ceiling +BALL_DIAMETER_MM = 42.67 + + +@dataclass +class BallFit: + center: np.ndarray # (x, y) subpixel + radius_px: float + polarity: str # "dark_on_light" | "light_on_dark" + contrast_dn: float # signed, ball minus local background + arc_coverage_deg: float # angular span of boundary used for the fit + visible_area_px: int + circularity: float + mask: np.ndarray = field(repr=False) + + @property + def px_per_mm(self) -> float: + return 2.0 * self.radius_px / BALL_DIAMETER_MM + + +class BallNotFound(ValueError): + """Named rejection, so a failure says which gate it failed.""" + + +def _local_background(frame: np.ndarray, ball_px: int) -> np.ndarray: + """Median over a window several ball-widths across. + + A ball is small relative to the window, so it is smoothed away while real scene + structure (the mat, the wall, the boundary between them) survives. + """ + k = min(int(max(15, ball_px * 3)), 99) | 1 + return cv2.medianBlur(frame, k) + + +def _robust_sigma(residual: np.ndarray) -> float: + """MAD-based noise estimate, floored at the sensor's measured read noise.""" + mad = float(np.median(np.abs(residual - np.median(residual)))) + return max(1.4826 * mad, SENSOR_NOISE_DN) + + +def _fit_circle(points: np.ndarray) -> tuple[np.ndarray, float]: + """Circle fit: algebraic (Kasa) seed, then geometric Gauss-Newton refinement. + + The refinement minimises true perpendicular distance, which is the statistically + correct objective and — unlike a purely algebraic fit — stays unbiased on the + short arcs this detector actually sees when the ball's lit side has blended into + a saturated background. + """ + x, y = points[:, 0].astype(float), points[:, 1].astype(float) + if len(x) < 4: + raise BallNotFound("too_few_boundary_points") + # Kasa seed: x^2 + y^2 = 2a.x + 2b.y + c, linear in (a, b, c) + design = np.column_stack([x, y, np.ones_like(x)]) + try: + sol, *_ = np.linalg.lstsq(design, x * x + y * y, rcond=None) + except np.linalg.LinAlgError: + raise BallNotFound("circle_fit_degenerate") from None + cx, cy = sol[0] / 2.0, sol[1] / 2.0 + seed = sol[2] + cx * cx + cy * cy + if not np.isfinite(seed) or seed <= 0.0: + raise BallNotFound("circle_fit_degenerate") + r = float(np.sqrt(seed)) + + for _ in range(50): # geometric refinement + dx, dy = x - cx, y - cy + dist = np.hypot(dx, dy) + if np.any(dist < 1e-9): + break + residual = dist - r + jac = np.column_stack([-dx / dist, -dy / dist, -np.ones_like(dist)]) + try: + step, *_ = np.linalg.lstsq(jac, -residual, rcond=None) + except np.linalg.LinAlgError: + break + cx, cy, r = cx + step[0], cy + step[1], r + step[2] + if not np.isfinite([cx, cy, r]).all() or r <= 0.0: + raise BallNotFound("circle_fit_degenerate") + if np.linalg.norm(step) < 1e-9: + break + return np.array([cx, cy]), float(r) + + +def _reliable_boundary(mask: np.ndarray, frame: np.ndarray) -> np.ndarray: + """Boundary pixels whose edge is trustworthy. + + Discards boundary where the BALL'S OWN pixel is saturated: there its lit surface + has merged into a clipped background, so the apparent edge marks where clipping + began, not where the ball ends, and including it biases the radius low. + + It deliberately does NOT discard boundary merely because the background beside it + is saturated. A dark ball against a blown-out mat has a real, sharp edge all the + way round; rejecting it there threw away 17 of 28 boundary points and left a + 45-degree arc that no circle fit can use. + """ + contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE) + if not contours: + raise BallNotFound("no_boundary") + pts = max(contours, key=cv2.contourArea).reshape(-1, 2) + h, w = mask.shape + keep = [ + (px, py) + for px, py in pts + if 0 < px < w - 1 and 0 < py < h - 1 and frame[py, px] < SATURATION_DN + ] + return np.array(keep if len(keep) >= 8 else pts, dtype=float) + + +def _arc_coverage(points: np.ndarray, center: np.ndarray) -> float: + """Angular span covered by the fitted boundary, in degrees. + + A radius fitted to a 90 degree arc is far less trustworthy than one fitted to + 300 degrees, and the caller should be able to tell the difference. + """ + ang = np.degrees(np.arctan2(points[:, 1] - center[1], points[:, 0] - center[0])) + occupied = np.zeros(72, dtype=bool) # 5 degree bins + occupied[((ang + 180.0) / 5.0).astype(int) % 72] = True + return float(occupied.sum() * 5) + + +def candidates( + frame: np.ndarray, + *, + expected_radius_px: float | None = None, + radius_tolerance: float = 0.6, + sigma_k: float = 3.5, + min_area_px: int = 20, +) -> list[BallFit]: + """Every region that could be a ball, whether darker or brighter than background.""" + if frame.ndim != 2: + raise BallNotFound("frame_not_2d") + f = frame.astype(np.float32) + nominal = int(expected_radius_px * 2) if expected_radius_px else 14 + background = _local_background(frame, nominal).astype(np.float32) + residual = f - background + sigma = _robust_sigma(residual) + + found: list[BallFit] = [] + for polarity, signed in (("light_on_dark", residual), ("dark_on_light", -residual)): + mask = (signed > sigma_k * sigma).astype(np.uint8) + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, np.ones((2, 2), np.uint8)) + mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, np.ones((3, 3), np.uint8)) + count, labels, stats, _ = cv2.connectedComponentsWithStats(mask, 8) + for idx in range(1, count): + w, h, area = stats[idx, 2], stats[idx, 3], stats[idx, 4] + if area < min_area_px: + continue + if max(w, h) / max(min(w, h), 1) > 3.0: # a long smear is not a ball + continue + comp = (labels == idx).astype(np.uint8) + try: + pts = _reliable_boundary(comp, frame) + if len(pts) < 8: + continue + center, radius = _fit_circle(pts) + except BallNotFound: + continue + if not np.isfinite(radius) or radius <= 1.0 or radius > 0.25 * max(frame.shape): + continue + if expected_radius_px is not None: + lo = expected_radius_px * (1.0 - radius_tolerance) + hi = expected_radius_px * (1.0 + radius_tolerance) + if not lo <= radius <= hi: + continue + circularity = float(area) / (np.pi * radius * radius) + if not 0.30 <= circularity <= 1.60: + continue + contrast = float(np.median(f[comp > 0]) - np.median(background[comp > 0])) + found.append( + BallFit( + center=center, + radius_px=float(radius), + polarity=polarity, + contrast_dn=contrast, + arc_coverage_deg=_arc_coverage(pts, center), + visible_area_px=int(area), + circularity=circularity, + mask=comp, + ) + ) + + return found + + +def fit_teed_ball(frame: np.ndarray, **kwargs) -> BallFit: + """Single-frame best guess. Shape only — see find_ball_at_address for the real one.""" + found = candidates(frame, **kwargs) + if not found: + raise BallNotFound("no_candidate_passed_shape_gate") + return max(found, key=lambda c: (c.arc_coverage_deg / 360.0) * min(c.circularity, 1.0)) + + +def fit_teed_ball_sequence( + frames: np.ndarray, + impact_index: int, + tee_region: tuple[int, int, int, int], + *, + stride: int = 2, + lead: int = 60, + settle: int = 2, + look_after: int = 4, + cluster_px: float = 5.0, + min_persistence: float = 0.25, + **kwargs, +) -> BallFit: + """Locate the teed ball inside a known tee region. + + `tee_region` is (x0, y0, x1, y1). This is not a shortcut: the camera is fixed to + the unit and aimed at the ball, so where the ball can be is a property of the rig, + established once at install. Searching the whole frame is a harder problem than + the system actually poses, and on the real capture it is not reliably solvable — + a bay contains signage, shoes and knees that fit a circle better than a ball whose + lit side has blended into the mat. + + Within the region, two physical facts do the work: + * the ball is stationary for the whole address period + * the ball is GONE once the club has sent it away + + The second is the strong one. Static clutter scores 0.00 on departure; the ball + scores 1.00. + + Ambient captures can change illumination mid-sequence (the real archive drops + 58% six frames after the trigger), so the "after" window is checked to be in the + same illumination regime and truncated if not — otherwise everything looks like + it departed. + """ + x0, y0, x1, y1 = tee_region + inside = lambda p: x0 <= p[0] <= x1 and y0 <= p[1] <= y1 # noqa: E731 + + lo, hi = max(1, impact_index - lead), max(2, impact_index - settle) + clusters: list[dict] = [] + for index in range(lo, hi, stride): + for cand in candidates(frames[index], **kwargs): + if not inside(cand.center): + continue + for cluster in clusters: + if np.linalg.norm(cand.center - cluster["center"]) <= cluster_px: + cluster["fits"].append(cand) + cluster["frames"].add(index) + cluster["center"] = np.mean([f.center for f in cluster["fits"]], axis=0) + break + else: + clusters.append({"center": cand.center.copy(), "fits": [cand], "frames": {index}}) + if not clusters: + raise BallNotFound("no_candidate_in_tee_region") + + # keep the post-impact window inside the same illumination regime + level = float(frames[lo:hi].reshape(hi - lo, -1).mean()) + after = [ + j + for j in range(impact_index + settle, min(impact_index + settle + look_after, len(frames))) + if abs(float(frames[j].mean()) - level) < 12.0 + ] + if not after: + raise BallNotFound("no_comparable_post_impact_frame") + + sampled = len(range(lo, hi, stride)) + scored = [] + for cluster in clusters: + persistence = len(cluster["frames"]) / max(sampled, 1) + if persistence < min_persistence: + continue + survives = sum( + any( + np.linalg.norm(c.center - cluster["center"]) <= cluster_px * 2 + for c in candidates(frames[j], **kwargs) + if inside(c.center) + ) + for j in after + ) + departed = 1.0 - survives / len(after) + if departed < 0.75: # a ball that is still there is not the ball + continue + scored.append((departed, persistence, cluster)) + if not scored: + raise BallNotFound("no_departing_candidate_in_tee_region") + + # Departure ranks above persistence. A bay can hold several balls, and a spare + # that the club sweeps across is briefly hidden — that scrapes past the gate as a + # partial departure. The struck ball is gone completely, so ranking on departure + # first separates them instead of leaving it to a persistence tie-break. + _, _, best = max(scored, key=lambda s: (round(s[0], 3), s[1])) + fit = max(best["fits"], key=lambda f: f.arc_coverage_deg) + fit.center = best["center"] + fit.radius_px = float(np.median([f.radius_px for f in best["fits"]])) + return fit diff --git a/tests/test_clubpose_ball_detect.py b/tests/test_clubpose_ball_detect.py new file mode 100644 index 00000000..654d6338 --- /dev/null +++ b/tests/test_clubpose_ball_detect.py @@ -0,0 +1,144 @@ +"""Properties of the polarity-agnostic ball detector, pinned by test. + +Each test here corresponds to a defect found by pointing the existing detector at +the first real OpenFlight capture (`frames.npz`, 2026-08-24). They are written so a +regression re-breaks the specific thing that was broken. +""" + +from __future__ import annotations + +import numpy as np +import pytest + +from openflight.camera.clubpose.teed_ball import ( + BallNotFound, + _fit_circle, + candidates, + fit_teed_ball_sequence, +) + + +def _disc(shape, center, radius, fg, bg, *, cap=None): + """Render a disc. `cap` clips everything above that row to the background, + reproducing a ball whose lit upper surface has blended into a saturated mat.""" + image = np.full(shape, bg, dtype=np.uint8) + ys, xs = np.mgrid[0 : shape[0], 0 : shape[1]] + inside = (xs - center[0]) ** 2 + (ys - center[1]) ** 2 <= radius**2 + if cap is not None: + inside &= ys >= cap + image[inside] = fg + return image + + +@pytest.mark.parametrize("arc_deg", [360, 300, 240, 180, 120, 90]) +def test_circle_fit_is_exact_on_partial_arcs(arc_deg): + """The previous algebraic fit was biased -44% at 120 degrees of arc. + + Partial arcs are the normal case, not the exception: at address the ball's lit + side blends into the background and only part of its outline survives. + """ + angles = np.radians(np.linspace(0.0, arc_deg, max(12, arc_deg // 6), endpoint=False)) + center, radius = np.array([127.0, 154.0]), 7.3 + points = np.column_stack( + [center[0] + radius * np.cos(angles), center[1] + radius * np.sin(angles)] + ) + fitted_center, fitted_radius = _fit_circle(points) + assert fitted_radius == pytest.approx(radius, abs=1e-6) + assert fitted_center == pytest.approx(center, abs=1e-6) + + +def test_finds_ball_darker_than_its_background(): + """The real teed ball is 62 DN DARKER than the clipped mat it sits on. + + The rule it replaces asked for `>= background + 210 DN`, which cannot express a + negative contrast at any threshold value. + """ + frame = _disc((200, 320), (127, 154), 7, fg=192, bg=255) + found = candidates(frame) + assert found, "a dark ball on a light background must be detectable" + best = min(found, key=lambda c: np.linalg.norm(c.center - np.array([127.0, 154.0]))) + assert best.polarity == "dark_on_light" + assert best.contrast_dn < 0 + assert best.center == pytest.approx(np.array([127.0, 154.0]), abs=1.5) + + +def test_finds_ball_brighter_than_its_background_below_the_old_threshold(): + """The real airborne ball is +122 DN, which the old `+210 DN` bar also failed.""" + frame = _disc((200, 320), (128, 84), 7, fg=224, bg=102) + found = candidates(frame) + best = min(found, key=lambda c: np.linalg.norm(c.center - np.array([128.0, 84.0]))) + assert best.polarity == "light_on_dark" + assert 0 < best.contrast_dn < 210 + assert best.center == pytest.approx(np.array([128.0, 84.0]), abs=1.5) + + +def test_detector_is_not_fooled_by_a_static_round_distractor(): + """A bay contains signage and shoes that fit a circle better than a blended ball. + + Shape alone picked the golfer's leg in 10 real frames out of 10. Departure is + what separates them: the ball leaves, the clutter does not. + """ + frames = [] + for index in range(40): + frame = _disc((200, 320), (40, 30), 9, fg=230, bg=100) # static distractor + if index < 30: # ball, gone after 30 + ys, xs = np.mgrid[0:200, 0:320] + frame[(xs - 127) ** 2 + (ys - 154) ** 2 <= 49] = 60 + frames.append(frame) + result = fit_teed_ball_sequence( + np.array(frames), 30, (80, 120, 200, 190), lead=30, look_after=4 + ) + assert result.center == pytest.approx(np.array([127.0, 154.0]), abs=3.0) + + +def test_rejection_is_named_not_silent(): + frames = np.full((20, 200, 320), 128, dtype=np.uint8) + with pytest.raises(BallNotFound) as excinfo: + fit_teed_ball_sequence(frames, 12, (80, 120, 200, 190), lead=10, look_after=3) + assert str(excinfo.value) in { + "no_candidate_in_tee_region", + "no_departing_candidate_in_tee_region", + "no_comparable_post_impact_frame", + } + + +def _mat_scene(n_frames, balls, occluder=None): + """A blown-out mat carrying several balls. `balls` is (x, y, departs_at|None).""" + frames = [] + for index in range(n_frames): + frame = np.full((200, 320), 255, dtype=np.uint8) + ys, xs = np.mgrid[0:200, 0:320] + for bx, by, departs in balls: + if departs is not None and index >= departs: + continue + frame[(xs - bx) ** 2 + (ys - by) ** 2 <= 49] = 192 + if occluder is not None: + ox, oy, active = occluder + if index in active: + frame[max(0, oy - 14) : oy + 14, max(0, ox - 16) : ox + 16] = 120 + frames.append(frame) + return np.array(frames) + + +TEE = (80, 120, 220, 190) + + +def test_picks_the_struck_ball_when_several_are_on_the_mat(): + """A real bay holds spares. Only the struck ball leaves.""" + scene = _mat_scene(40, [(127, 154, 30), (160, 168, None), (100, 140, None)]) + result = fit_teed_ball_sequence(scene, 30, TEE, lead=30, look_after=4) + assert result.center == pytest.approx(np.array([127.0, 154.0]), abs=4.0) + + +def test_a_spare_ball_swept_over_by_the_club_is_not_mistaken_for_the_struck_one(): + """Brief occlusion looks like a partial departure; full departure must outrank it.""" + scene = _mat_scene(40, [(127, 154, 30), (175, 170, None)], occluder=(175, 170, {32, 33, 34})) + result = fit_teed_ball_sequence(scene, 30, TEE, lead=30, look_after=4) + assert result.center == pytest.approx(np.array([127.0, 154.0]), abs=4.0) + + +def test_selection_is_by_departure_not_by_position_in_the_tee_region(): + """The struck ball need not be the one nearest the middle of the region.""" + scene = _mat_scene(40, [(200, 180, None), (95, 130, 30)]) + result = fit_teed_ball_sequence(scene, 30, TEE, lead=30, look_after=4) + assert result.center == pytest.approx(np.array([95.0, 130.0]), abs=4.0)