diff --git a/docs/implementation-plan.md b/docs/implementation-plan.md index 05ecd7a..2cdd5e0 100644 --- a/docs/implementation-plan.md +++ b/docs/implementation-plan.md @@ -31,6 +31,7 @@ - The live visual producer writes a multi-LOD scene cache with source kind and priority. Real Leap frames are promoted as the highest-priority visual timing/spatial evidence; Leap fallback frames remain lower-priority diagnostics. - Live clap calibration is wired into the visual producer. It keeps a rolling frame window from the Kiyo pair plus Leap, reads the live spatial audio frame for transient candidates, publishes `clap-events.msgpack`, injects clap calibration markers into the render frame, and writes clap evidence into the LOD cache. Kiyo stereo owns the current rough 3D solve; Leap owns the best visual timing witness until its geometric model is calibrated. - Clap peaks now update a per-camera clock sync model. The point-cloud builder can sample camera history buffers at `oracle_time - camera_offset` instead of using whichever frame arrived last. + - `localcast.sensor_fusion.chirp_pose` converts live phase-field delay meaning for Kiyo/PS Eye microphone sources into camera-body range constraints. `scripts/live_sensor_fusion.py` injects those constraints as `camera-chirp:*` render points and `chirp-camera-pose` LOD evidence. - `scripts/stream_spout.py` runs the deadline Spout sender loop from the typed cache with typed and JSON heartbeat status. - The deadline Spout renderer has a named `kiyo-mid-deru` virtual camera preset: eye at the midpoint between the two Kiyo-class cameras and target on the co-streamer body volume. It also applies a renderer-owned point budget with stable high-confidence anchors plus frame-varying remainder samples so downstream TAA/supersampling can accumulate detail without forcing every source claim into every OBS frame. - `docs/obs-spout-streaming.md` documents OBS setup and the Aquarium replacement boundary. @@ -74,7 +75,7 @@ 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. 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. Feed ultrasonic chirp delay estimates from Kiyo/PS Eye microphones into the shared scene map as camera-body pose constraints. The mics do not replace camera calibration; they add range/timing evidence for the physical camera bodies. +14. Replace the placeholder speaker/camera-mic geometry used by `chirp_pose` with measured rig coordinates, then let the common-space optimizer consume range residuals instead of only visualizing them. 15. Calibrate Leap as a geometric camera in the shared rig so the timing authority can also contribute direct 3D rays instead of only visual timing peaks. 16. Fix the live Leap capture backend so actual Leap frames, not fallback frames, populate `leap-ground-truth` cells in the multi-LOD cache. 17. 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. diff --git a/localcast/sensor_fusion/__init__.py b/localcast/sensor_fusion/__init__.py index 8adcd7c..8655b6b 100644 --- a/localcast/sensor_fusion/__init__.py +++ b/localcast/sensor_fusion/__init__.py @@ -67,6 +67,14 @@ from .clap_calibration import ( detect_camera_motion_peaks, detect_clap_events, ) +from .chirp_pose import ( + CameraChirpPoseConstraint, + CameraMicGeometry, + SpeakerGeometry, + constraints_from_phase_sources, + default_camera_mic_geometry, + default_speaker_geometry, +) from .cultcache_docs import ( CLAP_EVENTS_KEY, LIVE_RENDER_FRAME_KEY, @@ -127,6 +135,9 @@ __all__ = [ "CameraMotionPeak", "CameraClockSyncModel", "CameraSyncEstimate", + "CameraChirpPoseConstraint", + "CameraMicGeometry", + "SpeakerGeometry", "ClapCalibrationEvent", "ClapDetectorConfig", "TimestampedFrame", @@ -164,6 +175,9 @@ __all__ = [ "detect_audio_transients", "detect_camera_motion_peaks", "detect_clap_events", + "constraints_from_phase_sources", + "default_camera_mic_geometry", + "default_speaker_geometry", "remote_video_artifact_for_present_time", "put_live_clap_events", "put_live_render_frame", diff --git a/localcast/sensor_fusion/chirp_pose.py b/localcast/sensor_fusion/chirp_pose.py new file mode 100644 index 0000000..5d51646 --- /dev/null +++ b/localcast/sensor_fusion/chirp_pose.py @@ -0,0 +1,116 @@ +from __future__ import annotations + +from dataclasses import dataclass +from typing import Mapping, Sequence + +import numpy as np + + +SPEED_OF_SOUND_MPS = 343.0 + + +@dataclass(frozen=True) +class CameraMicGeometry: + sensor_id: str + source_id: str + position_m: np.ndarray + + +@dataclass(frozen=True) +class SpeakerGeometry: + speaker_id: str + position_m: np.ndarray + + +@dataclass(frozen=True) +class CameraChirpPoseConstraint: + stable_key: str + sensor_id: str + source_id: str + speaker_id: str + camera_position_m: np.ndarray + speaker_position_m: np.ndarray + nominal_range_m: float + observed_range_m: float + range_residual_m: float + confidence: float + timestamp_ns: int + + +def default_camera_mic_geometry() -> tuple[CameraMicGeometry, ...]: + return ( + CameraMicGeometry("kiyo-primary", "kiyo-0", np.array([-0.18, -0.72, 1.28], dtype=np.float64)), + CameraMicGeometry("kiyo-secondary", "kiyo-1", np.array([0.18, -0.72, 1.28], dtype=np.float64)), + CameraMicGeometry("ps3eye-left", "ps-eye-0", np.array([-0.78, -0.82, 1.34], dtype=np.float64)), + CameraMicGeometry("ps3eye-right", "ps-eye-1", np.array([0.78, -0.82, 1.34], dtype=np.float64)), + ) + + +def default_speaker_geometry() -> tuple[SpeakerGeometry, ...]: + return ( + SpeakerGeometry("local-speaker-left", np.array([-0.32, -1.08, 1.18], dtype=np.float64)), + SpeakerGeometry("local-speaker-right", np.array([0.32, -1.08, 1.18], dtype=np.float64)), + ) + + +def constraints_from_phase_sources( + sources: Sequence[Mapping[str, object]], + *, + audio_time_ns: int, + camera_mics: Sequence[CameraMicGeometry] | None = None, + speakers: Sequence[SpeakerGeometry] | None = None, + max_abs_residual_m: float = 2.0, +) -> tuple[CameraChirpPoseConstraint, ...]: + """Convert phase-field delay meaning into camera-body range constraints.""" + + camera_mics = default_camera_mic_geometry() if camera_mics is None else camera_mics + speakers = default_speaker_geometry() if speakers is None else speakers + mic_by_source = {item.source_id: item for item in camera_mics} + if not speakers: + return () + rows: list[CameraChirpPoseConstraint] = [] + for source in sources: + source_id = str(source.get("sourceId", "")) + mic = mic_by_source.get(source_id) + if mic is None: + continue + confidence = max(0.0, min(1.0, float(source.get("confidence", 0.0)))) + if confidence <= 0.0: + continue + distance_delta_m = _source_distance_delta_m(source) + if not np.isfinite(distance_delta_m): + continue + speaker = _nearest_speaker(mic.position_m, speakers) + nominal = float(np.linalg.norm(mic.position_m - speaker.position_m)) + residual = float(max(-max_abs_residual_m, min(max_abs_residual_m, distance_delta_m))) + observed = max(0.0, nominal + residual) + rows.append( + CameraChirpPoseConstraint( + stable_key=f"camera-chirp:{mic.sensor_id}:{speaker.speaker_id}", + sensor_id=mic.sensor_id, + source_id=source_id, + speaker_id=speaker.speaker_id, + camera_position_m=mic.position_m.astype(np.float64), + speaker_position_m=speaker.position_m.astype(np.float64), + nominal_range_m=nominal, + observed_range_m=observed, + range_residual_m=residual, + confidence=confidence, + timestamp_ns=int(audio_time_ns), + ) + ) + return tuple(rows) + + +def _nearest_speaker(position_m: np.ndarray, speakers: Sequence[SpeakerGeometry]) -> SpeakerGeometry: + return min(speakers, key=lambda speaker: float(np.linalg.norm(np.asarray(position_m) - speaker.position_m))) + + +def _source_distance_delta_m(source: Mapping[str, object]) -> float: + if "distanceDeltaMeters" in source: + return float(source["distanceDeltaMeters"]) + if "delayMs" in source: + return float(source["delayMs"]) * 0.001 * SPEED_OF_SOUND_MPS + if "delaySamples" in source and "sampleRate" in source: + return float(source["delaySamples"]) / max(1.0, float(source["sampleRate"])) * SPEED_OF_SOUND_MPS + return float("nan") diff --git a/notes/current-system-map.md b/notes/current-system-map.md index cc4a06e..ca951a1 100644 --- a/notes/current-system-map.md +++ b/notes/current-system-map.md @@ -106,7 +106,7 @@ Ownership: - `scripts/live_sensor_fusion.py` currently owns the live producer and writes `localcast.visual.render_frame` into CultCache. - The live producer now keeps a rolling RGB/Leap frame window for clap calibration. Kiyo stereo motion gives the first rough 3D clap position; Leap participates as the high-precision visual timing witness until a calibrated Leap geometric model joins the rig. - Clap peaks update a per-camera clock sync model. Camera history sampling uses `oracle_time - camera_offset`, so the point builder can reconstruct a shared instant instead of mixing latest frames from different clock domains. -- Ultrasonic chirps from the Kiyo and PS Eye microphones should feed camera-body pose constraints into the same scene map. Audio range/timing evidence augments visual calibration; it does not own image geometry. +- `localcast.sensor_fusion.chirp_pose` now turns phase-field delay meaning for Kiyo/PS Eye microphone sources into camera-body range constraints. The live producer publishes them as `camera-chirp:*` render points and `chirp-camera-pose` LOD evidence. Audio range/timing evidence augments visual calibration; it does not own image geometry. - The live producer also writes a multi-LOD scene cache with source kind and priority. Real Leap frames are the highest-priority timing/spatial evidence; Leap fallback frames are tagged as fallback and do not become ground truth. - `scripts/stream_spout.py` consumes typed CultCache state and publishes Spout. Its presentation camera defaults to `kiyo-mid-deru`: the virtual eye sits halfway between the two Kiyo-class cameras and aims at the co-streamer body target. Its render budget is temporal: high-confidence anchors stay stable while the remaining samples rotate by frame id for TAA/supersampling reconciliation. - CultNet document replication is the intended API boundary for other producers/consumers. diff --git a/scripts/live_sensor_fusion.py b/scripts/live_sensor_fusion.py index 31128cc..39d13fd 100644 --- a/scripts/live_sensor_fusion.py +++ b/scripts/live_sensor_fusion.py @@ -21,11 +21,13 @@ from localcast.sensor_fusion import ( SensorRig, SurfaceFeatureObservation, CameraClockSyncModel, + CameraChirpPoseConstraint, CameraMotionPeak, ClapCalibrationEvent, TimestampedFrame, TimestampedFrameHistory, ClapDetectorConfig, + constraints_from_phase_sources, detect_clap_events, dense_stereo_points, evidence_from_fusion_items, @@ -40,7 +42,7 @@ from localcast.sensor_fusion import ( stochastic_transient_matches, ) from localcast.sensor_fusion.camera_control import AdaptiveCameraController, OpenCvCameraSettingPort -from audio_field.cultcache_audio import frame_to_numpy, get_live_spatial_audio_frame +from audio_field.cultcache_audio import frame_to_numpy, get_live_audio_phase_field, get_live_spatial_audio_frame def acquire_runtime_lock(path: Path) -> BinaryIO | None: @@ -655,6 +657,70 @@ def sync_status_position(sensor_id: str) -> np.ndarray: return positions.get(sensor_id, np.zeros(3, dtype=np.float64)) +class LiveChirpPoseMapper: + def __init__(self, *, phase_cache: Path) -> None: + self.phase_cache = phase_cache + self.last_frame_id: int | None = None + self.constraints: tuple[CameraChirpPoseConstraint, ...] = () + + def update(self, fallback_timestamp_ns: int) -> tuple[RenderPointPacket, ...]: + if not self.phase_cache.exists(): + return self._constraint_points(fallback_timestamp_ns) + field = get_live_audio_phase_field(self.phase_cache) + if field is None: + return self._constraint_points(fallback_timestamp_ns) + if field.frame_id != self.last_frame_id: + self.last_frame_id = field.frame_id + self.constraints = constraints_from_phase_sources( + field.sources, + audio_time_ns=field.audio_time_ns, + ) + return self._constraint_points(field.audio_time_ns) + + def _constraint_points(self, timestamp_ns: int) -> tuple[RenderPointPacket, ...]: + points: list[RenderPointPacket] = [] + for constraint in self.constraints: + points.extend(chirp_pose_constraint_points(constraint, timestamp_ns)) + return tuple(points) + + +def chirp_pose_constraint_points( + constraint: CameraChirpPoseConstraint, + timestamp_ns: int, +) -> tuple[RenderPointPacket, ...]: + residual = float(constraint.range_residual_m) + confidence = max(0.0, min(1.0, float(constraint.confidence))) + camera = np.asarray(constraint.camera_position_m, dtype=np.float64) + speaker = np.asarray(constraint.speaker_position_m, dtype=np.float64) + direction = camera - speaker + norm = float(np.linalg.norm(direction)) + if norm <= 1.0e-9: + direction = np.array([0.0, 0.0, 1.0], dtype=np.float64) + norm = 1.0 + unit = direction / norm + corrected = speaker + unit * float(constraint.observed_range_m) + midpoint = speaker + unit * (0.5 * (float(constraint.nominal_range_m) + float(constraint.observed_range_m))) + residual_strength = min(1.0, abs(residual) / 0.25) + return ( + render_point( + f"{constraint.stable_key}:body", + corrected, + 0.030 + 0.045 * residual_strength, + (0.95, 0.78, 0.24, 0.38 + 0.42 * confidence), + confidence, + timestamp_ns, + ), + render_point( + f"{constraint.stable_key}:range", + midpoint, + 0.012 + 0.030 * residual_strength, + (1.0, 0.55, 0.18, 0.22 + 0.36 * confidence), + confidence * 0.85, + timestamp_ns, + ), + ) + + def pixel_ray_point(camera: CameraModel, uv: np.ndarray, *, distance_m: float) -> np.ndarray: point = np.asarray([float(uv[0]), float(uv[1]), 1.0], dtype=np.float64) direction_sensor = np.linalg.inv(camera.camera_matrix) @ point @@ -1018,8 +1084,10 @@ def main() -> None: parser.add_argument("--leap-step", type=int, default=12) parser.add_argument("--lod-cache", default=str(ROOT / "calibration" / "runs" / "visual-lod-cache.json")) parser.add_argument("--audio-cache", default=str(ROOT / "calibration" / "runs" / "audio-state.msgpack")) + parser.add_argument("--phase-cache", default=str(ROOT / "calibration" / "runs" / "audio-phase-field.msgpack")) parser.add_argument("--clap-cache", default=str(ROOT / "calibration" / "runs" / "clap-events.msgpack")) parser.add_argument("--no-clap-calibration", action="store_true") + parser.add_argument("--no-chirp-pose", action="store_true") parser.add_argument("--no-stochastic-transients", action="store_true") args = parser.parse_args() @@ -1066,6 +1134,7 @@ def main() -> None: camera_width=args.rgb_width, camera_height=args.rgb_height, ) + chirp_pose_mapper = None if args.no_chirp_pose else LiveChirpPoseMapper(phase_cache=Path(args.phase_cache)) try: while True: now = time.monotonic() @@ -1116,10 +1185,12 @@ def main() -> None: support_points = dim_fusion_points(tuple(frame.points)) if not rgb_points else () stochastic_points = transient_match_render_points(transient_matches, timestamp_ns) clap_points = () if clap_calibrator is None else clap_calibrator.update(timestamp_ns) + chirp_pose_points = () if chirp_pose_mapper is None else chirp_pose_mapper.update(timestamp_ns) multilod_cache_from_evidence( evidence_from_render_points(leap_points, source_kind=leap_source_kind, source_priority=leap_source_priority) + evidence_from_render_points(rgb_points, source_kind="rgb-surface", source_priority=1.4) + evidence_from_render_points(clap_points, source_kind="clap-calibration", source_priority=2.4) + + evidence_from_render_points(chirp_pose_points, source_kind="chirp-camera-pose", source_priority=1.8) + evidence_from_fusion_items(transient_matches, source_kind="stochastic-transient", source_priority=1.2) + evidence_from_fusion_items(result.points, source_kind="synthetic-fusion", source_priority=0.8), levels=(0.01, 0.04, 0.16, 0.64), @@ -1136,7 +1207,7 @@ def main() -> None: spout_sender_name=frame.spout_sender_name, target_width=frame.target_width, target_height=frame.target_height, - points=clap_points + leap_points + rgb_points + stochastic_points + support_points, + points=clap_points + chirp_pose_points + leap_points + rgb_points + stochastic_points + support_points, ) put_live_render_frame(cache_path, frame) frame_id += 1 diff --git a/scripts/live_status.py b/scripts/live_status.py index 994a5f9..8dddc90 100644 --- a/scripts/live_status.py +++ b/scripts/live_status.py @@ -43,12 +43,18 @@ def msgpack_head(path: Path): return summary summary["schema"] = payload[0] if payload else None if summary["type"] == "localcast.visual.render_frame": + points = payload[10] if len(payload) > 10 else [] + point_prefix_counts = {} + for point in points: + prefix = str(point[0]).split(":", 1)[0] + point_prefix_counts[prefix] = point_prefix_counts.get(prefix, 0) + 1 summary.update( { "frameId": payload[1], "presentTimeNs": payload[5], "audioAlignmentTimeNs": payload[6], - "pointCount": len(payload[10]) if len(payload) > 10 else 0, + "pointCount": len(points), + "pointPrefixCounts": dict(sorted(point_prefix_counts.items())), } ) elif summary["type"] == "localcast.audio.spatial_frame": diff --git a/state/map.yaml b/state/map.yaml index 86c1c78..609cafa 100644 --- a/state/map.yaml +++ b/state/map.yaml @@ -171,9 +171,11 @@ research: camera authority after shared calibration. Clap peaks update per-camera clock offsets, allowing history buffers to be sampled at `oracle_time - camera_offset` for coherent point-cloud construction. - Ultrasonic chirp delay estimates from camera microphones are valid - camera-body pose evidence and should enter this same map as range/timing - constraints, not as a separate audio-only calibration story. + `localcast.sensor_fusion.chirp_pose` converts live phase-field delay + meaning for Kiyo/PS Eye microphone sources into camera-body range + constraints, which the live producer emits as `camera-chirp:*` render + points and `chirp-camera-pose` LOD evidence. These constraints augment + visual calibration; they do not own image geometry. typed_visual_state: summary: docs/typed-visual-state.md key_decision: > diff --git a/tests/test_sensor_fusion.py b/tests/test_sensor_fusion.py index 598b648..387e5fa 100644 --- a/tests/test_sensor_fusion.py +++ b/tests/test_sensor_fusion.py @@ -9,6 +9,7 @@ from localcast.sensor_fusion import ( AdaptiveCameraController, BoardObservation, BoardSpec, + CameraMicGeometry, CameraModel, CameraClockSyncModel, CameraIntrinsics, @@ -22,11 +23,13 @@ from localcast.sensor_fusion import ( RenderFramePacket, RenderPointPacket, SensorRig, + SpeakerGeometry, SpoutOutputConfig, TimestampedFrame, TimestampedFrameHistory, TrackCache, VirtualCamera, + constraints_from_phase_sources, detect_clap_events, frame_to_vertex_array, frame_with_point_budget, @@ -549,6 +552,31 @@ class SensorFusionTests(unittest.TestCase): self.assertEqual(900, selected.timestamp_ns) self.assertEqual(100, sync.offset_ns("cam-a")) + def test_chirp_pose_constraints_map_camera_mics_to_range_residuals(self): + constraints = constraints_from_phase_sources( + ( + {"sourceId": "kiyo-0", "distanceDeltaMeters": 0.12, "confidence": 0.8}, + {"sourceId": "host-focusrite", "distanceDeltaMeters": 0.40, "confidence": 1.0}, + ), + audio_time_ns=12_345, + camera_mics=( + CameraMicGeometry("kiyo-primary", "kiyo-0", np.array([1.0, 0.0, 0.0], dtype=np.float64)), + ), + speakers=( + SpeakerGeometry("speaker-left", np.array([0.0, 0.0, 0.0], dtype=np.float64)), + ), + ) + + self.assertEqual(1, len(constraints)) + constraint = constraints[0] + self.assertEqual("kiyo-primary", constraint.sensor_id) + self.assertEqual("kiyo-0", constraint.source_id) + self.assertEqual("speaker-left", constraint.speaker_id) + self.assertAlmostEqual(1.0, constraint.nominal_range_m) + self.assertAlmostEqual(1.12, constraint.observed_range_m) + self.assertAlmostEqual(0.12, constraint.range_residual_m) + self.assertEqual(12_345, constraint.timestamp_ns) + def test_audio_source_events_overlay_against_audio_alignment_time(self): frame = RenderFramePacket( schema="localcast.sensor_fusion.render_frame.v1",