From 905fc60dbd102bd1d993b4673a33eab45dff1bae Mon Sep 17 00:00:00 2001 From: Meta Vens Date: Mon, 18 May 2026 21:31:09 +0100 Subject: [PATCH] Add common-space calibration and surface tracking --- docs/implementation-plan.md | 9 +- docs/sensor-fusion-architecture.md | 23 ++- localcast/sensor_fusion/__init__.py | 28 ++++ localcast/sensor_fusion/calibration_space.py | 167 +++++++++++++++++++ localcast/sensor_fusion/surface_features.py | 143 ++++++++++++++++ scripts/charuco_calibration.py | 4 + scripts/solve_common_camera_space.py | 96 +++++++++++ scripts/triangulate_surface_features.py | 68 ++++++++ state/map.yaml | 13 +- tests/test_sensor_fusion.py | 75 +++++++++ 10 files changed, 610 insertions(+), 16 deletions(-) create mode 100644 localcast/sensor_fusion/calibration_space.py create mode 100644 localcast/sensor_fusion/surface_features.py create mode 100644 scripts/solve_common_camera_space.py create mode 100644 scripts/triangulate_surface_features.py diff --git a/docs/implementation-plan.md b/docs/implementation-plan.md index fa60b2a..d7922d1 100644 --- a/docs/implementation-plan.md +++ b/docs/implementation-plan.md @@ -22,6 +22,8 @@ - `audio_field/` contains testable core modules for source buffering, bounded latency convergence, injectable capture/alignment/encoder/sink ports, and pipeline orchestration. - `docs/audio-field.md` maps the audio pipeline and its invariants. - Sensor-fusion render bridge: + - `localcast.sensor_fusion.calibration_space` solves fixed-board ChArUco observations into camera poses in one common world frame. + - `localcast.sensor_fusion.surface_features` matches cross-view features and triangulates calibrated surface tracks. - `localcast.sensor_fusion.render_bridge` emits render-frame packets with visual/audio timing metadata and Spout sender identity. - `localcast.sensor_fusion.cultcache_docs` stores live visual state as typed CultCache MessagePack documents. - `localcast.sensor_fusion.spout_output` renders render-frame packets into a GPU texture and publishes it as a named Spout sender for OBS. @@ -56,6 +58,7 @@ 9. Create local `config/audio-field.json`, confirm local Kiyo/PS Eye/Focusrite device matches, and confirm the neighbor Focusrite shotgun capture/transport path. 10. Replace placeholder mic/speaker geometry with measured world coordinates, then build the delay/SRO alignment stage that feeds the bounded field cache and emits aligned six-channel blocks before FOA encoding. 11. Move the render-frame consumer into Aquarium Engine so dense brush/splat rendering replaces the deadline OpenGL point sink behind the same OBS Spout boundary. -12. Replace synthetic live-fusion observations with PS3 Eye detector observations. -13. Keep the neighbor loopback leg running through the scheduled interactive WASAPI capture path and use it as the timing witness once actual neighbor app audio is present. -14. Let the co-streamer surface delay drive the shared presentation buffer horizon for audio stems, AmbiX, and remote video. +12. Capture fixed-board ChArUco observations for every camera, solve `config/sensor-fusion.json`, and verify cross-view feature triangulation with `scripts/triangulate_surface_features.py`. +13. Replace synthetic live-fusion observations with PS3 Eye detector observations and calibrated surface-feature tracks. +14. Keep the neighbor loopback leg running through the scheduled interactive WASAPI capture path and use it as the timing witness once actual neighbor app audio is present. +15. Let the co-streamer surface delay drive the shared presentation buffer horizon for audio stems, AmbiX, and remote video. diff --git a/docs/sensor-fusion-architecture.md b/docs/sensor-fusion-architecture.md index 6e9d01a..6bd1bbc 100644 --- a/docs/sensor-fusion-architecture.md +++ b/docs/sensor-fusion-architecture.md @@ -60,6 +60,8 @@ flowchart TD - `localcast.sensor_fusion.core` owns calibrated camera models, 2D observations, DLT triangulation, reprojection gating, confidence scoring, point-cloud export, and track cache expiry. - `localcast.sensor_fusion.adapters` owns driver-facing frame ingress. Its first concrete adapter is an FFmpeg raw BGR reader for the PS3 Eye DirectShow path. +- `localcast.sensor_fusion.calibration_space` owns fixed-board common-space calibration: ChArUco corner observations plus intrinsics become `CameraModel.world_from_sensor` transforms in one room frame. +- `localcast.sensor_fusion.surface_features` owns cross-view feature observations, descriptor matching, and triangulated surface tracks. It consumes calibrated cameras; it does not invent camera geometry. - `localcast.sensor_fusion.camera_control` owns measurement-quality policy: luminance, clipping, contrast, and sharpness in; normalized exposure/gain/focus commands out. Driver adapters own translating those commands to OpenCV, DirectShow, vendor tools, or no-op mocks. - `localcast.sensor_fusion.dense_stereo` owns a debug CPU reference for dense calibrated RGB surface claims. It is useful for tests and shader parity, but the live million-splat path belongs to Aquarium/GPU compute. - `localcast.sensor_fusion.active_illumination` owns deliberate light pulses as calibration telemetry. A Tuya bulb is an optional local-network actuator, not a rendering authority; its pulse timestamps are evidence for camera exposure/depth response. @@ -97,6 +99,8 @@ The core can be tested without cameras: - timestamp windows reject stale pairs - track cache expiry is deterministic - raw-frame adapter can be tested from an in-memory byte stream +- fixed-board ChArUco observations solve camera poses into the expected common frame +- matched surface descriptors triangulate to a known 3D point - adaptive camera control responds to overexposure without live hardware - dense stereo turns shifted textured camera pairs into calibrated RGB surface claims @@ -146,15 +150,16 @@ That lets the final stream compositor buffer point-cloud visuals until they alig ## Next Cut -1. Load real ChArUco intrinsics/extrinsics into `config/sensor-fusion.json`; the current dense Kiyo pair geometry is provisional and should not be mistaken for truth. -2. Move dense matching from CPU block search to GPU-resident stereo/flow so the million-sample target is not murdered by Python loops. -3. If a Tuya light is available on the local network, run `scripts/pulse_tuya_light.py` with its local key and align pulse timestamps against camera brightness response. -4. Add detector adapters that turn PS3 Eye frames into `Observation2D` marker detections. -5. Record an observation run from the two PS3 Eyes into typed CultCache docs. -6. Feed `RenderFramePacket` into Aquarium Engine. -7. Add Aquarium-side GPU point/brush rendering to a D3D render target. -8. Publish the render target as a Spout2 sender and receive it in OBS. -9. Replace compatibility JSON render-frame paths once no deadline script depends on them. +1. Capture ChArUco intrinsics for each camera with `scripts/charuco_calibration.py calibrate`. +2. Capture fixed-board observations from every camera and solve shared extrinsics with `scripts/solve_common_camera_space.py`. +3. Use `scripts/triangulate_surface_features.py` on calibrated image pairs to prove real cross-view surface tracks before promoting the matcher into the live producer. +4. Move dense matching from CPU block search to GPU-resident stereo/flow so the million-sample target is not murdered by Python loops. +5. If a Tuya light is available on the local network, run `scripts/pulse_tuya_light.py` with its local key and align pulse timestamps against camera brightness response. +6. Add detector adapters that turn PS3 Eye frames into `Observation2D` marker detections. +7. Record an observation run from the two PS3 Eyes into typed CultCache docs. +8. Feed `RenderFramePacket` into Aquarium Engine. +9. Publish the render target as a Spout2 sender and receive it in OBS. +10. Replace compatibility JSON render-frame paths once no deadline script depends on them. ## References diff --git a/localcast/sensor_fusion/__init__.py b/localcast/sensor_fusion/__init__.py index 9570f2f..d108d2f 100644 --- a/localcast/sensor_fusion/__init__.py +++ b/localcast/sensor_fusion/__init__.py @@ -21,6 +21,22 @@ from .camera_control import ( measure_frame_quality, ) from .dense_stereo import DenseStereoConfig, dense_stereo_points +from .calibration_space import ( + BoardObservation, + BoardSpec, + CameraIntrinsics, + CameraPoseSolve, + camera_models_to_config, + solve_camera_pose_from_fixed_board, + solve_common_space_from_fixed_board, +) +from .surface_features import ( + SurfaceFeatureObservation, + SurfaceFeatureTrack, + match_surface_features, + orb_surface_observations, + triangulate_surface_tracks, +) from .audio_overlay import AudioVisualSyncStatus, overlay_audio_events from .media_artifacts import RemoteVideoArtifact, remote_video_artifact_for_present_time from .cultcache_docs import ( @@ -35,6 +51,10 @@ from .spout_output import ScreenBrushPacket, SpoutOutputConfig, frame_to_vertex_ __all__ = [ "CameraModel", + "CameraIntrinsics", + "BoardObservation", + "BoardSpec", + "CameraPoseSolve", "FusionConfig", "FusionResult", "Observation2D", @@ -52,6 +72,8 @@ __all__ = [ "CameraQualityTarget", "DenseStereoConfig", "FrameQuality", + "SurfaceFeatureObservation", + "SurfaceFeatureTrack", "AudioVisualSyncStatus", "RemoteVideoArtifact", "LIVE_RENDER_FRAME_KEY", @@ -65,8 +87,14 @@ __all__ = [ "lower_frame_to_screen_brushes", "lower_points_to_render_frame", "dense_stereo_points", + "camera_models_to_config", + "match_surface_features", "measure_frame_quality", + "orb_surface_observations", "overlay_audio_events", "remote_video_artifact_for_present_time", "put_live_render_frame", + "solve_camera_pose_from_fixed_board", + "solve_common_space_from_fixed_board", + "triangulate_surface_tracks", ] diff --git a/localcast/sensor_fusion/calibration_space.py b/localcast/sensor_fusion/calibration_space.py new file mode 100644 index 0000000..8e25516 --- /dev/null +++ b/localcast/sensor_fusion/calibration_space.py @@ -0,0 +1,167 @@ +from __future__ import annotations + +from dataclasses import dataclass +from typing import Iterable + +import numpy as np + +from .core import CameraModel + + +@dataclass(frozen=True) +class CameraIntrinsics: + sensor_id: str + camera_matrix: np.ndarray + dist_coeffs: np.ndarray + width: int + height: int + role: str = "tracking" + latency_ms: float = 0.0 + + +@dataclass(frozen=True) +class BoardSpec: + squares_x: int + squares_y: int + square_length_m: float + + def object_point(self, corner_id: int) -> np.ndarray: + cols = self.squares_x - 1 + if corner_id < 0 or corner_id >= cols * (self.squares_y - 1): + raise ValueError(f"ChArUco corner id {corner_id} outside board") + row = corner_id // cols + col = corner_id % cols + return np.array( + [(col + 1) * self.square_length_m, (row + 1) * self.square_length_m, 0.0], + dtype=np.float64, + ) + + +@dataclass(frozen=True) +class BoardObservation: + sensor_id: str + timestamp_ns: int + corner_ids: np.ndarray + image_points: np.ndarray + confidence: float = 1.0 + + @property + def corner_count(self) -> int: + return int(len(self.corner_ids)) + + +@dataclass(frozen=True) +class CameraPoseSolve: + sensor_id: str + world_from_sensor: np.ndarray + reprojection_error_px: float + corner_count: int + timestamp_ns: int + + +def solve_camera_pose_from_fixed_board( + intrinsics: CameraIntrinsics, + board: BoardSpec, + observation: BoardObservation, + *, + min_corners: int = 6, +) -> CameraPoseSolve: + if observation.sensor_id != intrinsics.sensor_id: + raise ValueError("Observation sensor_id does not match intrinsics") + if observation.corner_count < min_corners: + raise ValueError(f"Need at least {min_corners} board corners; got {observation.corner_count}") + + object_points = np.vstack([board.object_point(int(corner_id)) for corner_id in observation.corner_ids]) + image_points = np.asarray(observation.image_points, dtype=np.float64).reshape((-1, 2)) + if len(object_points) != len(image_points): + raise ValueError("corner_ids and image_points must have matching length") + + import cv2 + + ok, rvec, tvec = cv2.solvePnP( + object_points, + image_points, + np.asarray(intrinsics.camera_matrix, dtype=np.float64), + np.asarray(intrinsics.dist_coeffs, dtype=np.float64), + flags=cv2.SOLVEPNP_ITERATIVE, + ) + if not ok: + raise ValueError(f"Could not solve pose for {intrinsics.sensor_id}") + + rotation, _ = cv2.Rodrigues(rvec) + sensor_from_world = np.eye(4, dtype=np.float64) + sensor_from_world[:3, :3] = rotation + sensor_from_world[:3, 3] = np.asarray(tvec, dtype=np.float64).reshape(3) + world_from_sensor = np.linalg.inv(sensor_from_world) + + projected, _ = cv2.projectPoints( + object_points, + rvec, + tvec, + np.asarray(intrinsics.camera_matrix, dtype=np.float64), + np.asarray(intrinsics.dist_coeffs, dtype=np.float64), + ) + error = float(np.mean(np.linalg.norm(projected.reshape((-1, 2)) - image_points, axis=1))) + return CameraPoseSolve( + sensor_id=intrinsics.sensor_id, + world_from_sensor=world_from_sensor, + reprojection_error_px=error, + corner_count=observation.corner_count, + timestamp_ns=observation.timestamp_ns, + ) + + +def solve_common_space_from_fixed_board( + intrinsics: Iterable[CameraIntrinsics], + board: BoardSpec, + observations: Iterable[BoardObservation], + *, + max_reprojection_error_px: float = 2.5, +) -> tuple[dict[str, CameraModel], tuple[CameraPoseSolve, ...]]: + intrinsics_by_id = {item.sensor_id: item for item in intrinsics} + best_solves: dict[str, CameraPoseSolve] = {} + for observation in observations: + camera_intrinsics = intrinsics_by_id.get(observation.sensor_id) + if camera_intrinsics is None: + continue + solve = solve_camera_pose_from_fixed_board(camera_intrinsics, board, observation) + if solve.reprojection_error_px > max_reprojection_error_px: + continue + current = best_solves.get(solve.sensor_id) + if current is None or (solve.corner_count, -solve.reprojection_error_px) > (current.corner_count, -current.reprojection_error_px): + best_solves[solve.sensor_id] = solve + + cameras: dict[str, CameraModel] = {} + for sensor_id, solve in best_solves.items(): + item = intrinsics_by_id[sensor_id] + cameras[sensor_id] = CameraModel( + sensor_id=sensor_id, + camera_matrix=np.asarray(item.camera_matrix, dtype=np.float64), + dist_coeffs=np.asarray(item.dist_coeffs, dtype=np.float64), + world_from_sensor=solve.world_from_sensor, + width=item.width, + height=item.height, + role=item.role, + latency_ms=item.latency_ms, + ) + return cameras, tuple(best_solves.values()) + + +def camera_models_to_config(cameras: Iterable[CameraModel]) -> list[dict]: + payload = [] + for camera in cameras: + payload.append( + { + "id": camera.sensor_id, + "role": camera.role, + "intrinsics": { + "width": camera.width, + "height": camera.height, + "camera_matrix": camera.camera_matrix.tolist(), + "dist_coeffs": camera.dist_coeffs.tolist(), + }, + "world_from_sensor": camera.world_from_sensor.tolist(), + "latency_ms": camera.latency_ms, + } + ) + return payload diff --git a/localcast/sensor_fusion/surface_features.py b/localcast/sensor_fusion/surface_features.py new file mode 100644 index 0000000..908b3ba --- /dev/null +++ b/localcast/sensor_fusion/surface_features.py @@ -0,0 +1,143 @@ +from __future__ import annotations + +from dataclasses import dataclass +from typing import Iterable + +import numpy as np + +from .core import CameraModel, TriangulatedPoint, triangulate_dlt + + +@dataclass(frozen=True) +class SurfaceFeatureObservation: + sensor_id: str + feature_id: str + timestamp_ns: int + uv: np.ndarray + descriptor: np.ndarray + color_bgr: tuple[int, int, int] = (255, 255, 255) + confidence: float = 1.0 + + +@dataclass(frozen=True) +class SurfaceFeatureTrack: + track_id: str + observations: tuple[SurfaceFeatureObservation, ...] + + +def match_surface_features( + left: Iterable[SurfaceFeatureObservation], + right: Iterable[SurfaceFeatureObservation], + *, + max_descriptor_distance: float, + max_dt_ns: int, +) -> tuple[SurfaceFeatureTrack, ...]: + tracks: list[SurfaceFeatureTrack] = [] + used_right: set[int] = set() + right_items = list(right) + for left_item in left: + best_index = -1 + best_distance = float("inf") + for index, right_item in enumerate(right_items): + if index in used_right or right_item.sensor_id == left_item.sensor_id: + continue + if abs(right_item.timestamp_ns - left_item.timestamp_ns) > max_dt_ns: + continue + distance = descriptor_distance(left_item.descriptor, right_item.descriptor) + if distance < best_distance: + best_index = index + best_distance = distance + if best_index < 0 or best_distance > max_descriptor_distance: + continue + used_right.add(best_index) + right_item = right_items[best_index] + tracks.append( + SurfaceFeatureTrack( + track_id=f"{left_item.sensor_id}:{right_item.sensor_id}:{left_item.feature_id}:{right_item.feature_id}", + observations=(left_item, right_item), + ) + ) + return tuple(tracks) + + +def triangulate_surface_tracks( + tracks: Iterable[SurfaceFeatureTrack], + cameras: dict[str, CameraModel], + *, + max_reprojection_error_px: float = 4.0, +) -> tuple[TriangulatedPoint, ...]: + points: list[TriangulatedPoint] = [] + for track in tracks: + if len(track.observations) < 2: + continue + left, right = track.observations[:2] + cam_a = cameras.get(left.sensor_id) + cam_b = cameras.get(right.sensor_id) + if cam_a is None or cam_b is None: + continue + try: + xyz = triangulate_dlt(cam_a.projection_matrix, left.uv, cam_b.projection_matrix, right.uv) + except ValueError: + continue + error_a = cam_a.reprojection_error(xyz, left.uv) + error_b = cam_b.reprojection_error(xyz, right.uv) + reprojection_error = float((error_a + error_b) * 0.5) + if not np.isfinite(reprojection_error) or reprojection_error > max_reprojection_error_px: + continue + confidence = min(left.confidence, right.confidence) * max(0.0, 1.0 - reprojection_error / max_reprojection_error_px) + points.append( + TriangulatedPoint( + marker_id=track.track_id, + timestamp_ns=max(left.timestamp_ns, right.timestamp_ns), + xyz=xyz, + confidence=float(confidence), + reprojection_error_px=reprojection_error, + sensors=(left.sensor_id, right.sensor_id), + ) + ) + return tuple(points) + + +def orb_surface_observations( + sensor_id: str, + frame_bgr: np.ndarray, + timestamp_ns: int, + *, + max_features: int = 2000, +) -> tuple[SurfaceFeatureObservation, ...]: + import cv2 + + gray = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2GRAY) + detector = cv2.ORB_create(nfeatures=max_features) + keypoints, descriptors = detector.detectAndCompute(gray, None) + if descriptors is None: + return () + observations: list[SurfaceFeatureObservation] = [] + height, width = frame_bgr.shape[:2] + for index, (keypoint, descriptor) in enumerate(zip(keypoints, descriptors)): + x = int(np.clip(round(keypoint.pt[0]), 0, width - 1)) + y = int(np.clip(round(keypoint.pt[1]), 0, height - 1)) + b, g, r = frame_bgr[y, x].tolist() + observations.append( + SurfaceFeatureObservation( + sensor_id=sensor_id, + feature_id=f"orb:{index}", + timestamp_ns=timestamp_ns, + uv=np.array([keypoint.pt[0], keypoint.pt[1]], dtype=np.float64), + descriptor=descriptor.astype(np.uint8), + color_bgr=(int(b), int(g), int(r)), + confidence=float(min(1.0, keypoint.response * 40.0 + 0.25)), + ) + ) + return tuple(observations) + + +def descriptor_distance(left: np.ndarray, right: np.ndarray) -> float: + a = np.asarray(left) + b = np.asarray(right) + if a.shape != b.shape: + return float("inf") + if a.dtype == np.uint8 and b.dtype == np.uint8: + return float(np.unpackbits(np.bitwise_xor(a, b)).sum()) + delta = a.astype(np.float32) - b.astype(np.float32) + return float(np.linalg.norm(delta)) diff --git a/scripts/charuco_calibration.py b/scripts/charuco_calibration.py index 92f9203..2984886 100644 --- a/scripts/charuco_calibration.py +++ b/scripts/charuco_calibration.py @@ -121,6 +121,10 @@ def capture(args): "file": filename, "timestamp_monotonic": now, "charuco_corners": corner_count, + "charuco_ids": [] if charuco_ids is None else [int(value) for value in charuco_ids.reshape(-1)], + "charuco_image_points": [] + if charuco_corners is None + else [[float(x), float(y)] for x, y in charuco_corners.reshape((-1, 2))], "marker_count": 0 if marker_ids is None else int(len(marker_ids)), } ) diff --git a/scripts/solve_common_camera_space.py b/scripts/solve_common_camera_space.py new file mode 100644 index 0000000..c598781 --- /dev/null +++ b/scripts/solve_common_camera_space.py @@ -0,0 +1,96 @@ +import argparse +import json +from pathlib import Path +import sys + +import numpy as np + +ROOT = Path(__file__).resolve().parents[1] +if str(ROOT) not in sys.path: + sys.path.insert(0, str(ROOT)) + +from localcast.sensor_fusion import ( + BoardObservation, + BoardSpec, + CameraIntrinsics, + camera_models_to_config, + solve_common_space_from_fixed_board, +) + + +def main() -> None: + parser = argparse.ArgumentParser(description="Solve camera extrinsics into one fixed-board world frame.") + parser.add_argument("--input", required=True, help="JSON with board, intrinsics, and observations.") + parser.add_argument("--output", default=str(ROOT / "config" / "sensor-fusion.json")) + parser.add_argument("--max-reprojection-error-px", type=float, default=2.5) + args = parser.parse_args() + + data = json.loads(Path(args.input).read_text(encoding="utf-8")) + board_data = data["board"] + board = BoardSpec( + squares_x=int(board_data["squares_x"]), + squares_y=int(board_data["squares_y"]), + square_length_m=float(board_data["square_length"]), + ) + intrinsics = [ + CameraIntrinsics( + sensor_id=item["id"], + camera_matrix=np.asarray(item["camera_matrix"], dtype=np.float64), + dist_coeffs=np.asarray(item.get("dist_coeffs", []), dtype=np.float64), + width=int(item["width"]), + height=int(item["height"]), + role=item.get("role", "tracking"), + latency_ms=float(item.get("latency_ms", 0.0)), + ) + for item in data.get("intrinsics", []) + ] + observations = [ + BoardObservation( + sensor_id=item["sensor_id"], + timestamp_ns=int(item.get("timestamp_ns", 0)), + corner_ids=np.asarray(item["corner_ids"], dtype=np.int32), + image_points=np.asarray(item["image_points"], dtype=np.float64), + confidence=float(item.get("confidence", 1.0)), + ) + for item in data.get("observations", []) + ] + cameras, solves = solve_common_space_from_fixed_board( + intrinsics, + board, + observations, + max_reprojection_error_px=args.max_reprojection_error_px, + ) + output = { + "fusion": data.get( + "fusion", + { + "max_pair_dt_ns": 25000000, + "max_reprojection_error_px": 4.0, + "min_confidence": 0.1, + "cache_ttl_ns": 500000000, + }, + ), + "render_bridge": data.get("render_bridge", {}), + "cameras": camera_models_to_config(cameras.values()), + "calibration_report": { + "world_frame": "fixed ChArUco board coordinates", + "solves": [ + { + "sensor_id": solve.sensor_id, + "timestamp_ns": solve.timestamp_ns, + "corner_count": solve.corner_count, + "reprojection_error_px": solve.reprojection_error_px, + } + for solve in solves + ], + }, + } + out = Path(args.output) + out.parent.mkdir(parents=True, exist_ok=True) + out.write_text(json.dumps(output, indent=2), encoding="utf-8") + print(out) + print(json.dumps(output["calibration_report"], indent=2)) + + +if __name__ == "__main__": + main() diff --git a/scripts/triangulate_surface_features.py b/scripts/triangulate_surface_features.py new file mode 100644 index 0000000..1b1dc11 --- /dev/null +++ b/scripts/triangulate_surface_features.py @@ -0,0 +1,68 @@ +import argparse +from pathlib import Path +import sys +import time + +import cv2 + +ROOT = Path(__file__).resolve().parents[1] +if str(ROOT) not in sys.path: + sys.path.insert(0, str(ROOT)) + +from localcast.sensor_fusion import ( + PointCloud, + load_fusion_config, + match_surface_features, + orb_surface_observations, + triangulate_surface_tracks, +) + + +def main() -> None: + parser = argparse.ArgumentParser(description="Match surface features between two calibrated views and write a PLY.") + parser.add_argument("--config", default=str(ROOT / "config" / "sensor-fusion.json")) + parser.add_argument("--left-id", required=True) + parser.add_argument("--right-id", required=True) + parser.add_argument("--left-image", required=True) + parser.add_argument("--right-image", required=True) + parser.add_argument("--output", required=True) + parser.add_argument("--max-features", type=int, default=2000) + parser.add_argument("--max-descriptor-distance", type=float, default=44.0) + parser.add_argument("--max-reprojection-error-px", type=float, default=4.0) + args = parser.parse_args() + + rig = load_fusion_config(Path(args.config)) + left = cv2.imread(args.left_image) + right = cv2.imread(args.right_image) + if left is None: + raise SystemExit(f"Could not read {args.left_image}") + if right is None: + raise SystemExit(f"Could not read {args.right_image}") + timestamp_ns = time.monotonic_ns() + left_features = orb_surface_observations(args.left_id, left, timestamp_ns, max_features=args.max_features) + right_features = orb_surface_observations(args.right_id, right, timestamp_ns, max_features=args.max_features) + tracks = match_surface_features( + left_features, + right_features, + max_descriptor_distance=args.max_descriptor_distance, + max_dt_ns=25_000_000, + ) + points = triangulate_surface_tracks( + tracks, + rig.cameras, + max_reprojection_error_px=args.max_reprojection_error_px, + ) + PointCloud(points).write_ply(Path(args.output)) + print( + { + "left_features": len(left_features), + "right_features": len(right_features), + "tracks": len(tracks), + "points": len(points), + "output": args.output, + } + ) + + +if __name__ == "__main__": + main() diff --git a/state/map.yaml b/state/map.yaml index b828a7e..58d2cd9 100644 --- a/state/map.yaml +++ b/state/map.yaml @@ -231,13 +231,18 @@ current_status: OpenCV capture adapter when the driver accepts them. Active illumination now has an optional Tuya local-network pulse hook for deliberately modulating room light as calibration telemetry; pulse timestamps can be aligned against - camera brightness/depth response. Unit tests inject + camera brightness/depth response. Common-space calibration now has a real + owner: fixed-board ChArUco observations plus intrinsics solve + `CameraModel.world_from_sensor` transforms, and cross-view surface-feature + observations can be descriptor-matched and triangulated into world-space + tracks. Unit tests inject synthetic cameras and frames so the core does not depend on live camera drivers. next_action: > - Capture real ChArUco intrinsics/extrinsics for the PS3 Eyes and Kiyos, - replace `config/sensor-fusion.example.json` placeholder transforms with a - local `config/sensor-fusion.json`, then move the dense RGB stereo matcher + Capture real ChArUco intrinsics for the PS3 Eyes and Kiyos, capture + fixed-board observations from every camera, solve a local + `config/sensor-fusion.json`, and prove calibrated cross-view surface tracks + with `scripts/triangulate_surface_features.py`. Then move the dense RGB stereo matcher to a GPU-resident stereo/flow pass so high-detail surface claims can scale toward the million-splat temporal accumulator without Python owning each point. If a Tuya light is available, run the local-key pulse hook and record diff --git a/tests/test_sensor_fusion.py b/tests/test_sensor_fusion.py index 7bea9e1..415d67c 100644 --- a/tests/test_sensor_fusion.py +++ b/tests/test_sensor_fusion.py @@ -7,7 +7,10 @@ import numpy as np from localcast.sensor_fusion import ( AdaptiveCameraController, + BoardObservation, + BoardSpec, CameraModel, + CameraIntrinsics, CameraQualityTarget, DenseStereoConfig, FusionConfig, @@ -24,9 +27,13 @@ from localcast.sensor_fusion import ( get_live_render_frame, lower_frame_to_screen_brushes, lower_points_to_render_frame, + match_surface_features, overlay_audio_events, put_live_render_frame, remote_video_artifact_for_present_time, + solve_common_space_from_fixed_board, + SurfaceFeatureObservation, + triangulate_surface_tracks, ) from localcast.sensor_fusion.spout_output import rasterize_frame_rgba from scripts.live_sensor_fusion import ( @@ -428,6 +435,74 @@ class SensorFusionTests(unittest.TestCase): self.assertGreater(len(points), 0) self.assertTrue(all(point.stable_key.startswith("dense-rgb") for point in points)) + def test_fixed_board_calibration_solves_camera_into_common_space(self): + board = BoardSpec(squares_x=5, squares_y=4, square_length_m=0.04) + intrinsics = CameraIntrinsics( + sensor_id="cam-a", + camera_matrix=np.array([[420.0, 0.0, 160.0], [0.0, 420.0, 120.0], [0.0, 0.0, 1.0]]), + dist_coeffs=np.zeros(5), + width=320, + height=240, + ) + expected = CameraModel( + sensor_id="cam-a", + camera_matrix=intrinsics.camera_matrix, + dist_coeffs=intrinsics.dist_coeffs, + world_from_sensor=np.array( + [ + [1.0, 0.0, 0.0, -0.12], + [0.0, 1.0, 0.0, 0.03], + [0.0, 0.0, 1.0, -0.80], + [0.0, 0.0, 0.0, 1.0], + ] + ), + width=320, + height=240, + ) + ids = np.array([0, 1, 2, 4, 5, 6, 8, 9, 10], dtype=np.int32) + image_points = np.vstack([expected.project_world(board.object_point(int(corner_id))) for corner_id in ids]) + + cameras, solves = solve_common_space_from_fixed_board( + [intrinsics], + board, + [BoardObservation("cam-a", 100, ids, image_points)], + max_reprojection_error_px=0.05, + ) + + self.assertEqual(1, len(solves)) + np.testing.assert_allclose(expected.world_from_sensor, cameras["cam-a"].world_from_sensor, atol=1e-5) + self.assertLess(solves[0].reprojection_error_px, 1e-4) + + def test_surface_features_match_and_triangulate_between_calibrated_views(self): + left = camera("left", -0.25) + right = camera("right", 0.25) + target = np.array([0.04, 0.02, 2.0]) + left_obs = SurfaceFeatureObservation( + sensor_id="left", + feature_id="a", + timestamp_ns=1_000, + uv=left.project_world(target), + descriptor=np.array([1, 2, 3, 4], dtype=np.uint8), + confidence=0.95, + ) + right_obs = SurfaceFeatureObservation( + sensor_id="right", + feature_id="b", + timestamp_ns=1_200, + uv=right.project_world(target), + descriptor=np.array([1, 2, 3, 4], dtype=np.uint8), + confidence=0.90, + ) + + tracks = match_surface_features([left_obs], [right_obs], max_descriptor_distance=0.0, max_dt_ns=1_000) + points = triangulate_surface_tracks(tracks, {"left": left, "right": right}, max_reprojection_error_px=0.01) + + self.assertEqual(1, len(tracks)) + self.assertEqual(1, len(points)) + np.testing.assert_allclose(target, points[0].xyz, atol=1e-9) + self.assertEqual(("left", "right"), points[0].sensors) + if __name__ == "__main__": unittest.main() + match_surface_features,