diff --git a/docs/typed-visual-state.md b/docs/typed-visual-state.md index 3b969b4..04ce8f7 100644 --- a/docs/typed-visual-state.md +++ b/docs/typed-visual-state.md @@ -58,6 +58,19 @@ CultNetDocumentPut(localcast.visual.render_frame) For the deadline rig, both producer and renderer share the local CultCache file. The boundary is still the document contract: a future camera process, Aquarium process, or remote sensor node should publish the same document through CultNet `document_put` messages rather than inventing another transport shape. +The live visual frame may contain multiple claim families. `room-rgb:*` claims +are camera-resolved room/background surface samples, `host-rgb:*` and +`deru-rgb:*` claims are RGB body/object surface samples, and `leap-motion:*` +claims are high-rate LeapUVC packed-channel motion samples. Leap packed maps are +split into explicit green, magenta, red, and blue channels before publication so +the downstream accumulator can use Leap as the strongest timing/motion witness +without pretending the packed image is one ordinary RGB camera. + +LocalCastBridge does not own the final splat budget. It publishes dense typed +claims with stable keys and timestamps; Aquarium owns temporal accumulation, +renderer residency, and any GPU-side reduction needed to make million-splat +frames presentable. + ## Audio Synchronization `stream_spout.py` can now read `calibration/runs/audio-state.msgpack` and `calibration/runs/audio-events.msgpack` while it renders the visual frame. It selects source events against `RenderFramePacket.audio_alignment_time_ns`, adds synchronized audio-event points to the render packet, and writes `calibration/runs/av-sync-status.json` with the active visual frame id, audio frame id, audio delta, event count, overlay count, and remote video sync status. diff --git a/localcast/sensor_fusion/spout_output.py b/localcast/sensor_fusion/spout_output.py index d84e38f..d732d69 100644 --- a/localcast/sensor_fusion/spout_output.py +++ b/localcast/sensor_fusion/spout_output.py @@ -230,8 +230,6 @@ def lower_frame_to_screen_brushes( def rasterize_frame_rgba(frame: RenderFramePacket, width: int, height: int, point_scale: float = 1.0) -> np.ndarray: - import cv2 - image = np.zeros((height, width, 4), dtype=np.uint8) image[:, :, 0] = 7 image[:, :, 1] = 10 @@ -239,19 +237,51 @@ def rasterize_frame_rgba(frame: RenderFramePacket, width: int, height: int, poin image[:, :, 3] = 255 draw_reconstruction_guides(image) draw_floor_shadows(image, frame, point_scale) + canvas = image.astype(np.float32) / 255.0 for brush in lower_frame_to_screen_brushes(frame, width, height, point_scale): - cv2.ellipse( - image, - brush.center_px, - brush.radii_px, - brush.rotation_deg, - 0.0, - 360.0, - brush.color_rgba, - thickness=-1, - lineType=cv2.LINE_AA, - ) - return image + composite_gaussian_brush(canvas, brush) + return np.clip(canvas * 255.0, 0.0, 255.0).astype(np.uint8) + + +def composite_gaussian_brush(canvas: np.ndarray, brush: ScreenBrushPacket) -> None: + height, width = canvas.shape[:2] + cx, cy = brush.center_px + rx, ry = max(1, brush.radii_px[0]), max(1, brush.radii_px[1]) + bound = int(math.ceil(max(rx, ry) * 1.35)) + x0 = max(0, cx - bound) + x1 = min(width, cx + bound + 1) + y0 = max(0, cy - bound) + y1 = min(height, cy + bound + 1) + if x0 >= x1 or y0 >= y1: + return + + yy, xx = np.mgrid[y0:y1, x0:x1].astype(np.float32) + dx = xx - float(cx) + dy = yy - float(cy) + angle = math.radians(float(brush.rotation_deg)) + ca = math.cos(angle) + sa = math.sin(angle) + local_x = dx * ca + dy * sa + local_y = -dx * sa + dy * ca + q = (local_x / float(rx)) ** 2 + (local_y / float(ry)) ** 2 + mask = q < 1.0 + if not np.any(mask): + return + + # Compact Gaussian envelope: smooth center, finite edge, no solid paint bucket. + falloff = 4.4 + edge = math.exp(-falloff) + gaussian = np.exp(-falloff * q) + envelope = np.clip((gaussian - edge) / max(1.0 - edge, 1e-6), 0.0, 1.0) ** 1.35 + envelope *= mask + + src = np.asarray(brush.color_rgba, dtype=np.float32) / 255.0 + alpha = np.clip(envelope * src[3], 0.0, 0.96) + if float(alpha.max()) <= 0.0: + return + target = canvas[y0:y1, x0:x1, :] + target[:, :, :3] = src[:3] * alpha[:, :, None] + target[:, :, :3] * (1.0 - alpha[:, :, None]) + target[:, :, 3] = np.maximum(target[:, :, 3], alpha) def project_world_to_screen( diff --git a/scripts/live_sensor_fusion.py b/scripts/live_sensor_fusion.py index 8938957..0528a76 100644 --- a/scripts/live_sensor_fusion.py +++ b/scripts/live_sensor_fusion.py @@ -213,6 +213,7 @@ class RgbSplatSampler: width: int, height: int, sample_step: int, + room_step: int, fallback_dir: Path, ) -> None: self.api = api @@ -221,6 +222,7 @@ class RgbSplatSampler: self.width = width self.height = height self.sample_step = max(4, int(sample_step)) + self.room_step = max(self.sample_step, int(room_step)) self.fallback_dir = fallback_dir self._captures: dict[int, object] = {} self._frames: dict[int, np.ndarray] = {} @@ -238,8 +240,10 @@ class RgbSplatSampler: secondary = self._read_frame(self.secondary_index) points: list[RenderPointPacket] = [] if primary is not None: + points.extend(rgb_room_splats("room-rgb:primary", primary, np.array([-0.42, 0.05, 0.0]), timestamp_ns, step=self.room_step)) points.extend(rgb_body_splats("host-rgb", primary, np.array([-0.42, 0.05, 0.0]), now_s, timestamp_ns, side=-1.0, step=self.sample_step)) if secondary is not None: + points.extend(rgb_room_splats("room-rgb:secondary", secondary, np.array([0.48, 0.28, 0.0]), timestamp_ns, step=self.room_step)) points.extend(rgb_body_splats("deru-rgb", secondary, np.array([0.48, 0.28, 0.0]), now_s + 0.6, timestamp_ns, side=1.0, step=self.sample_step)) return tuple(points) @@ -345,6 +349,71 @@ def rgb_body_splats( return points +def rgb_room_splats( + prefix: str, + frame_bgr: np.ndarray, + camera_origin: np.ndarray, + timestamp_ns: int, + *, + step: int, +) -> list[RenderPointPacket]: + height, width = frame_bgr.shape[:2] + points: list[RenderPointPacket] = [] + body_crop = person_crop(frame_bgr, -1.0) + for row in range(0, height, step): + for col in range(0, width, step): + if inside_person_crop(row, col, body_crop): + continue + pixel = frame_bgr[row, col].astype(np.float32) / 255.0 + luminance = float(0.0722 * pixel[0] + 0.7152 * pixel[1] + 0.2126 * pixel[2]) + saturation = float(np.max(pixel) - np.min(pixel)) + if luminance < 0.030 and saturation < 0.06: + continue + u = col / max(1, width - 1) + v = row / max(1, height - 1) + xyz = room_surface_point(u, v, camera_origin) + b, g, r = np.clip(np.power(pixel, 0.78), 0.0, 1.0) + alpha = max(0.16, min(0.58, 0.12 + luminance * 0.48 + saturation * 0.16)) + confidence = max(0.30, min(0.82, 0.38 + luminance * 0.32 + saturation * 0.18)) + points.append( + render_point( + f"{prefix}:{row}:{col}", + xyz, + 0.024 + 0.018 * confidence, + (float(r), float(g), float(b), alpha), + confidence, + timestamp_ns, + ) + ) + return points + + +def inside_person_crop(row: int, col: int, crop: tuple[int, int, int, int]) -> bool: + y0, y1, x0, x1 = crop + pad_y = max(6, (y1 - y0) // 18) + pad_x = max(6, (x1 - x0) // 18) + return y0 - pad_y <= row <= y1 + pad_y and x0 - pad_x <= col <= x1 + pad_x + + +def room_surface_point(u: float, v: float, camera_origin: np.ndarray) -> np.ndarray: + # Coarse calibrated-room proxy until real depth/SLAM owns this surface. + x = -1.75 + 3.5 * u + if v > 0.70: + floor_v = (v - 0.70) / 0.30 + y = -0.95 + 1.85 * (1.0 - floor_v) + z = 0.018 + 0.05 * (1.0 - floor_v) + elif v < 0.26: + wall_v = v / 0.26 + y = 1.12 + z = 1.15 + 0.90 * (1.0 - wall_v) + else: + mid_v = (v - 0.26) / 0.44 + y = 1.05 - 0.55 * mid_v + z = 1.12 - 0.55 * mid_v + parallax = 0.10 * np.array([camera_origin[0], camera_origin[1], 0.0], dtype=np.float64) + return np.array([x, y, z], dtype=np.float64) + parallax + + def person_crop(frame_bgr: np.ndarray, side: float) -> tuple[int, int, int, int]: height, width = frame_bgr.shape[:2] gray = np.max(frame_bgr, axis=2) @@ -389,6 +458,153 @@ def body_surface_point(u: float, v: float, side: float, now_s: float) -> np.ndar return np.array([x, y, z], dtype=np.float64) +class LeapPackedMotionSampler: + def __init__( + self, + *, + api: str, + index: int, + width: int, + height: int, + fps: float, + step: int, + fallback_dir: Path, + ) -> None: + self.api = api + self.index = index + self.width = width + self.height = height + self.fps = fps + self.step = max(4, int(step)) + self.fallback_dir = fallback_dir + self._capture = None + self._previous_channels: dict[str, np.ndarray] = {} + self._last_timestamp_ns: int | None = None + + @property + def last_timestamp_ns(self) -> int | None: + return self._last_timestamp_ns + + def close(self) -> None: + if self._capture is not None: + try: + self._capture.release() + except Exception: + pass + self._capture = None + + def motion_splats(self, timestamp_ns: int) -> tuple[RenderPointPacket, ...]: + frame = self._read_frame() + if frame is None: + return () + self._last_timestamp_ns = time.monotonic_ns() + channels = unpack_leap_packed_channels(frame) + points: list[RenderPointPacket] = [] + for channel_name, channel in channels.items(): + previous = self._previous_channels.get(channel_name) + self._previous_channels[channel_name] = channel + if previous is None or previous.shape != channel.shape: + continue + points.extend(leap_channel_motion_points(channel_name, channel, previous, self._last_timestamp_ns or timestamp_ns, step=self.step)) + return tuple(points) + + def _read_frame(self) -> np.ndarray | None: + import cv2 + + if self._capture is None: + capture = cv2.VideoCapture(self.index, cv2_api(self.api)) + if capture.isOpened(): + capture.set(cv2.CAP_PROP_FRAME_WIDTH, self.width) + capture.set(cv2.CAP_PROP_FRAME_HEIGHT, self.height) + capture.set(cv2.CAP_PROP_FPS, self.fps) + capture.set(cv2.CAP_PROP_BUFFERSIZE, 1) + self._capture = capture + else: + try: + capture.release() + except Exception: + pass + return self._fallback_frame() + ok, frame = self._capture.read() + if ok and frame is not None: + return frame + return self._fallback_frame() + + def _fallback_frame(self) -> np.ndarray | None: + import cv2 + + for path in ( + self.fallback_dir / f"rgb-probe-{self.api}-{self.index}.png", + self.fallback_dir / f"rgb-probe-dshow-{self.index}.png", + self.fallback_dir / f"rgb-probe-msmf-{self.index}.png", + self.fallback_dir / "leap-probe.png", + ): + if path.exists(): + frame = cv2.imread(str(path)) + if frame is not None: + return frame + return None + + +def unpack_leap_packed_channels(frame_bgr: np.ndarray) -> dict[str, np.ndarray]: + frame = frame_bgr.astype(np.float32) / 255.0 + blue = frame[:, :, 0] + green = frame[:, :, 1] + red = frame[:, :, 2] + magenta = np.maximum(red, blue) + return { + "green": green, + "magenta": magenta, + "red": red, + "blue": blue, + } + + +def leap_channel_motion_points( + channel_name: str, + current: np.ndarray, + previous: np.ndarray, + timestamp_ns: int, + *, + step: int, +) -> list[RenderPointPacket]: + height, width = current.shape[:2] + motion = np.abs(current - previous) + threshold = max(0.035, float(np.percentile(motion, 94))) + points: list[RenderPointPacket] = [] + for row in range(0, height, step): + for col in range(0, width, step): + value = float(motion[row, col]) + intensity = float(current[row, col]) + if value < threshold and intensity < 0.10: + continue + u = col / max(1, width - 1) + v = row / max(1, height - 1) + x = -0.46 + 0.92 * u + y = -0.16 + 0.68 * (1.0 - v) + z = 0.74 + 0.58 * (1.0 - v) + 0.12 * intensity + confidence = max(0.28, min(1.0, value * 3.8 + intensity * 0.42)) + if channel_name == "green": + color = (0.28, 1.0, 0.62, 0.86) + elif channel_name == "magenta": + color = (1.0, 0.26, 0.92, 0.80) + elif channel_name == "red": + color = (1.0, 0.22, 0.18, 0.42) + else: + color = (0.20, 0.42, 1.0, 0.42) + points.append( + render_point( + f"leap-motion:{channel_name}:{row}:{col}", + np.array([x, y, z], dtype=np.float64), + 0.012 + 0.026 * confidence, + color, + confidence, + timestamp_ns, + ) + ) + return points + + def main() -> None: parser = argparse.ArgumentParser(description="Write live sensor-fusion render frames into typed CultCache state.") parser.add_argument("--cache", default=str(ROOT / "calibration" / "runs" / "visual-state.msgpack")) @@ -402,6 +618,14 @@ def main() -> None: parser.add_argument("--rgb-width", type=int, default=640) parser.add_argument("--rgb-height", type=int, default=480) parser.add_argument("--rgb-sample-step", type=int, default=10) + parser.add_argument("--rgb-room-step", type=int, default=28) + parser.add_argument("--no-leap", action="store_true") + parser.add_argument("--leap-api", choices=["any", "dshow", "msmf"], default="msmf") + parser.add_argument("--leap-index", type=int, default=0) + parser.add_argument("--leap-width", type=int, default=320) + parser.add_argument("--leap-height", type=int, default=240) + parser.add_argument("--leap-fps", type=float, default=120.0) + parser.add_argument("--leap-step", type=int, default=12) args = parser.parse_args() left = camera("ps3eye_left", -0.25) @@ -425,6 +649,16 @@ def main() -> None: width=args.rgb_width, height=args.rgb_height, sample_step=args.rgb_sample_step, + room_step=args.rgb_room_step, + fallback_dir=ROOT / "calibration" / "runs", + ) + leap_sampler = None if args.no_leap else LeapPackedMotionSampler( + api=args.leap_api, + index=args.leap_index, + width=args.leap_width, + height=args.leap_height, + fps=args.leap_fps, + step=args.leap_step, fallback_dir=ROOT / "calibration" / "runs", ) try: @@ -442,19 +676,22 @@ def main() -> None: ) timestamp_ns = frame.source_time_max_ns rgb_points = () if rgb_sampler is None else rgb_sampler.splats(now, timestamp_ns) + leap_points = () if leap_sampler is None else leap_sampler.motion_splats(timestamp_ns) + if leap_sampler is not None and leap_sampler.last_timestamp_ns is not None: + timestamp_ns = max(timestamp_ns, leap_sampler.last_timestamp_ns) support_points = dim_fusion_points(tuple(frame.points)) if not rgb_points else () frame = RenderFramePacket( schema=frame.schema, frame_id=frame.frame_id, created_monotonic_ns=frame.created_monotonic_ns, source_time_min_ns=frame.source_time_min_ns, - source_time_max_ns=frame.source_time_max_ns, - present_time_ns=frame.present_time_ns, - audio_alignment_time_ns=frame.audio_alignment_time_ns, + source_time_max_ns=timestamp_ns, + present_time_ns=timestamp_ns + render_config.visual_delay_ns, + audio_alignment_time_ns=timestamp_ns + render_config.audio_alignment_delay_ns, spout_sender_name=frame.spout_sender_name, target_width=frame.target_width, target_height=frame.target_height, - points=rgb_points + support_points, + points=leap_points + rgb_points + support_points, ) put_live_render_frame(cache_path, frame) frame_id += 1 @@ -464,6 +701,8 @@ def main() -> None: finally: if rgb_sampler is not None: rgb_sampler.close() + if leap_sampler is not None: + leap_sampler.close() if __name__ == "__main__": diff --git a/tests/test_sensor_fusion.py b/tests/test_sensor_fusion.py index 31e71f4..1ef925a 100644 --- a/tests/test_sensor_fusion.py +++ b/tests/test_sensor_fusion.py @@ -12,6 +12,7 @@ from localcast.sensor_fusion import ( PointCloud, RenderBridgeConfig, RenderFramePacket, + RenderPointPacket, SensorRig, SpoutOutputConfig, TrackCache, @@ -23,6 +24,12 @@ from localcast.sensor_fusion import ( put_live_render_frame, remote_video_artifact_for_present_time, ) +from localcast.sensor_fusion.spout_output import rasterize_frame_rgba +from scripts.live_sensor_fusion import ( + leap_channel_motion_points, + rgb_room_splats, + unpack_leap_packed_channels, +) from audio_field.cultcache_audio import make_audio_source_events, make_spatial_audio_frame from localcast.sensor_fusion.adapters import read_raw_bgr_frames @@ -171,6 +178,39 @@ class SensorFusionTests(unittest.TestCase): self.assertEqual("unit-test", config.sender_name) self.assertEqual(128, config.width) + def test_rasterizer_uses_gaussian_envelope_not_solid_ellipse(self): + frame = RenderFramePacket( + schema="localcast.sensor_fusion.render_frame.v1", + frame_id=1, + created_monotonic_ns=1, + source_time_min_ns=1, + source_time_max_ns=1, + present_time_ns=1, + audio_alignment_time_ns=1, + spout_sender_name="test", + target_width=320, + target_height=180, + points=( + RenderPointPacket( + stable_key="rgb-splat", + xyz=np.array([0.0, 0.0, 1.2]), + radius_m=0.08, + color_rgba=(1.0, 0.0, 0.0, 1.0), + confidence=1.0, + source_timestamp_ns=1, + ), + ), + ) + + image = rasterize_frame_rgba(frame, 320, 180, point_scale=1.0) + red = image[:, :, 0] + hot = red[red > 24] + + self.assertGreater(len(hot), 20) + self.assertGreater(int(hot.max()), 180) + self.assertGreater(len(np.unique(hot)), 8) + self.assertTrue(np.any((hot > 40) & (hot < 160))) + def test_render_frame_round_trips_through_typed_cultcache_doc(self): left = camera("left", -0.25) right = camera("right", 0.25) @@ -246,6 +286,47 @@ class SensorFusionTests(unittest.TestCase): self.assertEqual(30_000_000, artifact.delta_ns) self.assertTrue(artifact.synchronized) + def test_rgb_room_splats_emit_camera_resolved_background(self): + frame = np.zeros((48, 64, 3), dtype=np.uint8) + frame[:, :, :] = np.array([48, 72, 96], dtype=np.uint8) + frame[16:34, 24:40, :] = np.array([240, 240, 240], dtype=np.uint8) + + points = rgb_room_splats( + "room-rgb:test", + frame, + np.array([0.0, -1.0, 1.5]), + 123, + step=12, + ) + + self.assertGreater(len(points), 0) + self.assertTrue(all(point.stable_key.startswith("room-rgb:test") for point in points)) + self.assertTrue(any(point.xyz[2] < 0.2 for point in points)) + self.assertTrue(any(point.xyz[1] > 0.7 for point in points)) + + def test_leap_packed_map_channels_are_mapped_individually(self): + packed = np.zeros((12, 12, 3), dtype=np.uint8) + packed[:, :, 0] = 20 + packed[:, :, 1] = 80 + packed[:, :, 2] = 160 + + channels = unpack_leap_packed_channels(packed) + + self.assertAlmostEqual(80 / 255.0, float(channels["green"][0, 0]), places=6) + self.assertAlmostEqual(160 / 255.0, float(channels["magenta"][0, 0]), places=6) + self.assertAlmostEqual(20 / 255.0, float(channels["blue"][0, 0]), places=6) + + def test_leap_motion_points_use_channel_identity(self): + previous = np.zeros((24, 24), dtype=np.float32) + current = previous.copy() + current[12, 12] = 1.0 + + points = leap_channel_motion_points("green", current, previous, 456, step=12) + + self.assertGreater(len(points), 0) + self.assertTrue(all(point.stable_key.startswith("leap-motion:green") for point in points)) + self.assertTrue(all(point.source_timestamp_ns == 456 for point in points)) + if __name__ == "__main__": unittest.main()