1322 lines
53 KiB
Python
1322 lines
53 KiB
Python
import argparse
|
|
from pathlib import Path
|
|
import sys
|
|
import time
|
|
from typing import BinaryIO
|
|
|
|
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 (
|
|
CameraModel,
|
|
DenseStereoConfig,
|
|
FusionConfig,
|
|
Observation2D,
|
|
RenderBridgeConfig,
|
|
RenderFramePacket,
|
|
RenderPointPacket,
|
|
SensorRig,
|
|
SurfaceFeatureObservation,
|
|
CameraClockSyncModel,
|
|
CameraChirpPoseConstraint,
|
|
CameraMotionPeak,
|
|
ClapCalibrationEvent,
|
|
TimestampedFrame,
|
|
TimestampedFrameHistory,
|
|
ClapDetectorConfig,
|
|
camera_mic_geometry_from_audio_profile,
|
|
constraints_from_phase_sources,
|
|
detect_clap_events,
|
|
dense_stereo_points,
|
|
estimate_pose_corrections,
|
|
evidence_from_fusion_items,
|
|
evidence_from_render_points,
|
|
get_live_clap_events,
|
|
make_clap_events_frame,
|
|
measure_frame_quality,
|
|
multilod_cache_from_evidence,
|
|
put_live_clap_events,
|
|
put_live_render_frame,
|
|
lower_points_to_render_frame,
|
|
stochastic_transient_matches,
|
|
speaker_geometry_from_audio_profile,
|
|
)
|
|
from localcast.sensor_fusion.camera_control import AdaptiveCameraController, OpenCvCameraSettingPort
|
|
from localcast.sensor_fusion.adapters import LatestFramePump, OpenCvCaptureConfig, OpenCvFrameSource
|
|
from audio_field.cultcache_audio import frame_to_numpy, get_live_audio_phase_field, get_live_spatial_audio_frame
|
|
|
|
|
|
DEFAULT_RESERVOIR_NS = 5_000_000_000
|
|
|
|
|
|
def acquire_runtime_lock(path: Path) -> BinaryIO | None:
|
|
import msvcrt
|
|
|
|
path.parent.mkdir(parents=True, exist_ok=True)
|
|
handle = path.open("a+b")
|
|
handle.seek(0)
|
|
handle.write(b"\0")
|
|
handle.flush()
|
|
handle.seek(0)
|
|
try:
|
|
msvcrt.locking(handle.fileno(), msvcrt.LK_NBLCK, 1)
|
|
except OSError:
|
|
handle.close()
|
|
return None
|
|
return handle
|
|
|
|
|
|
def reservoir_window_ns(latest_timestamp_ns: int, reservoir_ns: int) -> tuple[int, int]:
|
|
end_ns = int(latest_timestamp_ns)
|
|
return end_ns - int(reservoir_ns), end_ns
|
|
|
|
|
|
def evidence_in_reservoir(evidence: tuple, *, latest_timestamp_ns: int, reservoir_ns: int) -> tuple:
|
|
start_ns, end_ns = reservoir_window_ns(latest_timestamp_ns, reservoir_ns)
|
|
return tuple(item for item in evidence if start_ns <= int(item.timestamp_ns) <= end_ns)
|
|
|
|
|
|
def render_points_in_reservoir(points: tuple[RenderPointPacket, ...], *, latest_timestamp_ns: int, reservoir_ns: int) -> tuple[RenderPointPacket, ...]:
|
|
start_ns, end_ns = reservoir_window_ns(latest_timestamp_ns, reservoir_ns)
|
|
return tuple(point for point in points if start_ns <= int(point.source_timestamp_ns) <= end_ns)
|
|
|
|
|
|
def latest_audio_time_ns(audio_cache: Path) -> int | None:
|
|
if not audio_cache.exists():
|
|
return None
|
|
frame = get_live_spatial_audio_frame(audio_cache)
|
|
if frame is None:
|
|
return None
|
|
return int(frame.audio_time_ns)
|
|
|
|
|
|
def camera(sensor_id: str, x: float) -> CameraModel:
|
|
return CameraModel(
|
|
sensor_id=sensor_id,
|
|
camera_matrix=np.array([[300.0, 0.0, 160.0], [0.0, 300.0, 120.0], [0.0, 0.0, 1.0]]),
|
|
dist_coeffs=np.zeros(5),
|
|
world_from_sensor=np.array(
|
|
[
|
|
[1.0, 0.0, 0.0, x],
|
|
[0.0, 1.0, 0.0, 0.0],
|
|
[0.0, 0.0, 1.0, 0.0],
|
|
[0.0, 0.0, 0.0, 1.0],
|
|
]
|
|
),
|
|
width=320,
|
|
height=240,
|
|
)
|
|
|
|
|
|
def synthetic_observations(rig: SensorRig, now_s: float, count: int) -> list[Observation2D]:
|
|
left = rig.cameras["ps3eye_left"]
|
|
right = rig.cameras["ps3eye_right"]
|
|
timestamp = time.monotonic_ns()
|
|
observations: list[Observation2D] = []
|
|
targets = reconstruction_targets(now_s, count)
|
|
for index, point in enumerate(targets):
|
|
marker = f"synthetic-fused-{index}"
|
|
observations.append(Observation2D("ps3eye_left", marker, timestamp, left.project_world(point), 0.95))
|
|
observations.append(Observation2D("ps3eye_right", marker, timestamp, right.project_world(point), 0.92))
|
|
return observations
|
|
|
|
|
|
def stochastic_surface_observations(observations: list[Observation2D]) -> list[SurfaceFeatureObservation]:
|
|
surface: list[SurfaceFeatureObservation] = []
|
|
for obs in observations:
|
|
marker_hash = abs(hash(obs.marker_id)) % 255
|
|
descriptor = np.full(32, marker_hash, dtype=np.uint8)
|
|
surface.append(
|
|
SurfaceFeatureObservation(
|
|
sensor_id=obs.sensor_id,
|
|
feature_id=obs.marker_id,
|
|
timestamp_ns=obs.timestamp_ns,
|
|
uv=obs.uv,
|
|
descriptor=descriptor,
|
|
confidence=obs.confidence,
|
|
)
|
|
)
|
|
return surface
|
|
|
|
|
|
def transient_match_render_points(matches, timestamp_ns: int) -> tuple[RenderPointPacket, ...]:
|
|
points: list[RenderPointPacket] = []
|
|
for match in matches:
|
|
confidence = max(0.0, min(1.0, float(match.confidence)))
|
|
points.append(
|
|
render_point(
|
|
f"stochastic:{match.stable_key}",
|
|
match.xyz,
|
|
0.018 + 0.020 * confidence,
|
|
(1.0, 0.88, 0.32, 0.35 + 0.55 * confidence),
|
|
confidence,
|
|
timestamp_ns,
|
|
)
|
|
)
|
|
return tuple(points)
|
|
|
|
|
|
def reconstruction_targets(now_s: float, count: int) -> list[np.ndarray]:
|
|
"""Deadline stand-in for real detections: two people plus tracked room support."""
|
|
body_count = max(32, int(count * 0.55))
|
|
room_count = max(24, int(count * 0.25))
|
|
marker_count = max(8, count - body_count - room_count)
|
|
targets: list[np.ndarray] = []
|
|
targets.extend(body_points("host", np.array([-0.42, 0.05, 0.0]), now_s, body_count // 2))
|
|
targets.extend(body_points("deru", np.array([0.48, 0.28, 0.0]), now_s + 0.7, body_count - body_count // 2))
|
|
targets.extend(room_support_points(now_s, room_count))
|
|
targets.extend(marker_points(now_s, marker_count))
|
|
return targets[: max(1, count)]
|
|
|
|
|
|
def body_points(name: str, origin: np.ndarray, now_s: float, count: int) -> list[np.ndarray]:
|
|
points: list[np.ndarray] = []
|
|
golden = 2.399963229728653
|
|
for index in range(max(1, count)):
|
|
u = (index + 0.5) / max(1, count)
|
|
theta = index * golden
|
|
if u < 0.22:
|
|
radius = 0.17 * np.sqrt(u / 0.22)
|
|
z = 1.55 + 0.16 * np.sin(theta)
|
|
local = np.array([radius * np.cos(theta), radius * np.sin(theta) * 0.72, z])
|
|
elif u < 0.72:
|
|
v = (u - 0.22) / 0.50
|
|
radius = 0.23 * (1.0 - abs(v - 0.52) * 0.55)
|
|
z = 0.72 + v * 0.72
|
|
sway = 0.035 * np.sin(now_s * 1.4 + (0.0 if name == "host" else 1.2))
|
|
local = np.array([radius * np.cos(theta) + sway, radius * np.sin(theta) * 0.52, z])
|
|
else:
|
|
v = (u - 0.72) / 0.28
|
|
side = -1.0 if index % 2 == 0 else 1.0
|
|
reach = 0.28 + 0.18 * np.sin(now_s * 1.7 + side)
|
|
z = 1.16 - 0.25 * v
|
|
local = np.array([side * reach * v, 0.06 * np.sin(theta), z])
|
|
points.append(origin + local)
|
|
return points
|
|
|
|
|
|
def room_support_points(now_s: float, count: int) -> list[np.ndarray]:
|
|
points: list[np.ndarray] = []
|
|
for index in range(max(1, count)):
|
|
u = (index + 0.5) / max(1, count)
|
|
if index % 3 == 0:
|
|
x = -1.65 + 3.3 * u
|
|
y = -0.85 + 1.75 * ((index * 7) % max(1, count)) / max(1, count)
|
|
z = 0.02
|
|
elif index % 3 == 1:
|
|
x = -1.65 + 3.3 * u
|
|
y = 1.05
|
|
z = 0.28 + 1.72 * ((index * 5) % max(1, count)) / max(1, count)
|
|
else:
|
|
x = -1.05 + 2.1 * ((index * 11) % max(1, count)) / max(1, count)
|
|
y = -0.65 + 0.12 * np.sin(now_s + index)
|
|
z = 0.85 + 0.95 * u
|
|
points.append(np.array([x, y, z], dtype=np.float64))
|
|
return points
|
|
|
|
|
|
def marker_points(now_s: float, count: int) -> list[np.ndarray]:
|
|
points: list[np.ndarray] = []
|
|
for index in range(max(1, count)):
|
|
theta = now_s * 0.9 + index * 2.399963229728653
|
|
ring = 0.28 + 0.52 * ((index % 5) / 4.0)
|
|
points.append(
|
|
np.array(
|
|
[
|
|
ring * np.cos(theta),
|
|
0.12 + ring * 0.42 * np.sin(theta * 0.7),
|
|
1.08 + 0.42 * np.sin(theta + index * 0.37),
|
|
],
|
|
dtype=np.float64,
|
|
)
|
|
)
|
|
return points
|
|
|
|
|
|
def reconstruction_context_points(now_s: float, timestamp_ns: int) -> tuple[RenderPointPacket, ...]:
|
|
points: list[RenderPointPacket] = []
|
|
cameras = [
|
|
("ps3eye-left", (-0.78, -0.82, 1.34), (0.10, 0.72, 1.05), (0.2, 0.7, 1.0, 0.95)),
|
|
("ps3eye-right", (0.78, -0.82, 1.34), (-0.10, 0.72, 1.05), (0.2, 0.7, 1.0, 0.95)),
|
|
("kiyo", (-0.28, -1.05, 1.62), (-0.15, 0.45, 1.15), (1.0, 0.62, 0.25, 0.95)),
|
|
("kiyo-pro", (0.38, -1.02, 1.66), (0.20, 0.48, 1.18), (1.0, 0.62, 0.25, 0.95)),
|
|
("leap", (0.0, -0.18, 0.76), (0.0, 0.20, 1.05), (0.55, 1.0, 0.72, 0.95)),
|
|
]
|
|
for sensor, origin, target, color in cameras:
|
|
origin_np = np.asarray(origin, dtype=np.float64)
|
|
target_np = np.asarray(target, dtype=np.float64)
|
|
points.append(render_point(f"sensor:{sensor}", origin_np, 0.055, color, 1.0, timestamp_ns))
|
|
for ray_index, t in enumerate((0.28, 0.48, 0.68, 0.88)):
|
|
p = origin_np + (target_np - origin_np) * t
|
|
jitter = np.array([0.04 * np.sin(now_s + ray_index), 0.0, 0.035 * np.cos(now_s * 0.7 + ray_index)])
|
|
points.append(render_point(f"frustum:{sensor}:{ray_index}", p + jitter, 0.014, color, 0.72, timestamp_ns))
|
|
return tuple(points)
|
|
|
|
|
|
def render_point(
|
|
key: str,
|
|
xyz: np.ndarray,
|
|
radius_m: float,
|
|
color: tuple[float, float, float, float],
|
|
confidence: float,
|
|
timestamp_ns: int,
|
|
) -> RenderPointPacket:
|
|
return RenderPointPacket(
|
|
stable_key=key,
|
|
xyz=np.asarray(xyz, dtype=np.float64),
|
|
radius_m=radius_m,
|
|
color_rgba=color,
|
|
confidence=confidence,
|
|
source_timestamp_ns=timestamp_ns,
|
|
)
|
|
|
|
|
|
def dim_fusion_points(points: tuple[RenderPointPacket, ...]) -> tuple[RenderPointPacket, ...]:
|
|
muted: list[RenderPointPacket] = []
|
|
for point in points:
|
|
confidence = max(0.0, min(1.0, float(point.confidence)))
|
|
muted.append(
|
|
RenderPointPacket(
|
|
stable_key=point.stable_key,
|
|
xyz=point.xyz,
|
|
radius_m=point.radius_m * 0.72,
|
|
color_rgba=(0.46, 0.58, 0.72, 0.18 * confidence),
|
|
confidence=confidence * 0.72,
|
|
source_timestamp_ns=point.source_timestamp_ns,
|
|
)
|
|
)
|
|
return tuple(muted)
|
|
|
|
|
|
class RgbSplatSampler:
|
|
def __init__(
|
|
self,
|
|
*,
|
|
api: str,
|
|
primary_index: int,
|
|
secondary_index: int,
|
|
width: int,
|
|
height: int,
|
|
sample_step: int,
|
|
room_step: int,
|
|
dense_step: int,
|
|
enable_cpu_dense_stereo: bool,
|
|
adaptive_controls: bool,
|
|
fallback_dir: Path,
|
|
reservoir_ns: int = DEFAULT_RESERVOIR_NS,
|
|
fallback_only: bool = False,
|
|
) -> None:
|
|
self.api = api
|
|
self.primary_index = primary_index
|
|
self.secondary_index = secondary_index
|
|
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.dense_step = max(1, int(dense_step))
|
|
self.enable_cpu_dense_stereo = enable_cpu_dense_stereo
|
|
self.adaptive_controls = adaptive_controls
|
|
self.fallback_dir = fallback_dir
|
|
self.fallback_only = bool(fallback_only)
|
|
self._captures: dict[int, object] = {}
|
|
self._pumps: dict[int, LatestFramePump] = {}
|
|
self._frames: dict[int, np.ndarray] = {}
|
|
self._timestamped_frames: dict[str, TimestampedFrame] = {}
|
|
self._history = TimestampedFrameHistory(max_frames=720, max_age_ns=reservoir_ns)
|
|
self._controllers: dict[int, AdaptiveCameraController] = {}
|
|
self._setting_ports: dict[int, OpenCvCameraSettingPort] = {}
|
|
|
|
def close(self) -> None:
|
|
for capture in self._captures.values():
|
|
try:
|
|
capture.release()
|
|
except Exception:
|
|
pass
|
|
self._captures.clear()
|
|
|
|
def splats(self, now_s: float, timestamp_ns: int) -> tuple[RenderPointPacket, ...]:
|
|
primary = self._read_frame(self.primary_index)
|
|
secondary = self._read_frame(self.secondary_index)
|
|
return self._splats_from_frames(primary, secondary, now_s, timestamp_ns)
|
|
|
|
def synced_splats(
|
|
self,
|
|
now_s: float,
|
|
timestamp_ns: int,
|
|
*,
|
|
sync_model: CameraClockSyncModel,
|
|
oracle_time_ns: int,
|
|
) -> tuple[RenderPointPacket, ...]:
|
|
self._read_frame(self.primary_index)
|
|
self._read_frame(self.secondary_index)
|
|
primary = self._history.nearest("kiyo-primary", sync_model.raw_time_for_oracle("kiyo-primary", oracle_time_ns))
|
|
secondary = self._history.nearest("kiyo-secondary", sync_model.raw_time_for_oracle("kiyo-secondary", oracle_time_ns))
|
|
return self._splats_from_frames(
|
|
None if primary is None else primary.image,
|
|
None if secondary is None else secondary.image,
|
|
now_s,
|
|
timestamp_ns,
|
|
)
|
|
|
|
def _splats_from_frames(
|
|
self,
|
|
primary: np.ndarray | None,
|
|
secondary: np.ndarray | None,
|
|
now_s: float,
|
|
timestamp_ns: int,
|
|
) -> tuple[RenderPointPacket, ...]:
|
|
points: list[RenderPointPacket] = []
|
|
if self.enable_cpu_dense_stereo and primary is not None and secondary is not None:
|
|
points.extend(rgb_dense_stereo_splats(primary, secondary, timestamp_ns, step=self.dense_step))
|
|
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))
|
|
if not points:
|
|
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))
|
|
if not points:
|
|
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)
|
|
|
|
def latest_timestamped_frames(self) -> dict[str, TimestampedFrame]:
|
|
return dict(self._timestamped_frames)
|
|
|
|
def _read_frame(self, index: int) -> np.ndarray | None:
|
|
if self.fallback_only:
|
|
return self._fallback_frame(index)
|
|
pump = self._pumps.get(index)
|
|
if pump is None:
|
|
sensor_id = self._sensor_id(index)
|
|
pump = LatestFramePump(
|
|
lambda sensor_id=sensor_id, index=index: OpenCvFrameSource(
|
|
OpenCvCaptureConfig(sensor_id=sensor_id, index=index, api=self.api, width=self.width, height=self.height)
|
|
)
|
|
)
|
|
pump.start()
|
|
self._pumps[index] = pump
|
|
packet = pump.latest(max_age_ns=DEFAULT_RESERVOIR_NS)
|
|
if packet is not None:
|
|
frame = packet.image_bgr
|
|
stamped = TimestampedFrame(packet.sensor_id, packet.timestamp_ns, frame.copy())
|
|
self._timestamped_frames[packet.sensor_id] = stamped
|
|
self._history.add(stamped)
|
|
self._tune_capture(index, frame)
|
|
self._frames[index] = frame
|
|
return frame
|
|
cached = self._frames.get(index)
|
|
if cached is not None:
|
|
return cached
|
|
return self._fallback_frame(index)
|
|
|
|
def _tune_capture(self, index: int, frame: np.ndarray) -> None:
|
|
controller = self._controllers.get(index)
|
|
port = self._setting_ports.get(index)
|
|
if controller is None or port is None:
|
|
return
|
|
command = controller.update(measure_frame_quality(frame))
|
|
port.apply(command)
|
|
|
|
def _fallback_frame(self, index: int) -> np.ndarray | None:
|
|
import cv2
|
|
|
|
candidates = [
|
|
self.fallback_dir / f"rgb-probe-{self.api}-{index}.png",
|
|
self.fallback_dir / f"rgb-probe-dshow-{index}.png",
|
|
self.fallback_dir / f"rgb-probe-any-{index}.png",
|
|
self.fallback_dir / f"rgb-probe-msmf-{index}.png",
|
|
]
|
|
for path in candidates:
|
|
if path.exists():
|
|
frame = cv2.imread(str(path))
|
|
if frame is not None:
|
|
sensor_id = self._sensor_id(index)
|
|
stamped = TimestampedFrame(sensor_id, time.monotonic_ns(), frame.copy())
|
|
self._timestamped_frames[sensor_id] = stamped
|
|
self._history.add(stamped)
|
|
return frame
|
|
return None
|
|
|
|
def _sensor_id(self, index: int) -> str:
|
|
if index == self.primary_index:
|
|
return "kiyo-primary"
|
|
if index == self.secondary_index:
|
|
return "kiyo-secondary"
|
|
return f"rgb-{index}"
|
|
|
|
|
|
def cv2_api(name: str) -> int:
|
|
import cv2
|
|
|
|
return {
|
|
"any": 0,
|
|
"dshow": cv2.CAP_DSHOW,
|
|
"msmf": cv2.CAP_MSMF,
|
|
}[name]
|
|
|
|
|
|
def rgb_dense_stereo_splats(
|
|
primary_bgr: np.ndarray,
|
|
secondary_bgr: np.ndarray,
|
|
timestamp_ns: int,
|
|
*,
|
|
step: int,
|
|
) -> tuple[RenderPointPacket, ...]:
|
|
height, width = primary_bgr.shape[:2]
|
|
secondary_height, secondary_width = secondary_bgr.shape[:2]
|
|
width = min(width, secondary_width)
|
|
height = min(height, secondary_height)
|
|
left = rgb_dense_camera("kiyo-primary", -0.18, width, height)
|
|
right = rgb_dense_camera("kiyo-secondary", 0.18, width, height)
|
|
return dense_stereo_points(
|
|
prefix="dense-rgb",
|
|
left_frame_bgr=primary_bgr[:height, :width],
|
|
right_frame_bgr=secondary_bgr[:height, :width],
|
|
left_camera=left,
|
|
right_camera=right,
|
|
timestamp_ns=timestamp_ns,
|
|
config=DenseStereoConfig(sample_step=step, block_radius=3, max_disparity_px=max(24, width // 3), radius_m=0.0045),
|
|
)
|
|
|
|
|
|
def rgb_dense_camera(sensor_id: str, x: float, width: int, height: int) -> CameraModel:
|
|
focal = 0.82 * float(width)
|
|
return CameraModel(
|
|
sensor_id=sensor_id,
|
|
camera_matrix=np.array(
|
|
[
|
|
[focal, 0.0, width * 0.5],
|
|
[0.0, focal, height * 0.5],
|
|
[0.0, 0.0, 1.0],
|
|
],
|
|
dtype=np.float64,
|
|
),
|
|
dist_coeffs=np.zeros(5),
|
|
world_from_sensor=np.array(
|
|
[
|
|
[1.0, 0.0, 0.0, x],
|
|
[0.0, 1.0, 0.0, -0.72],
|
|
[0.0, 0.0, 1.0, 1.28],
|
|
[0.0, 0.0, 0.0, 1.0],
|
|
],
|
|
dtype=np.float64,
|
|
),
|
|
width=width,
|
|
height=height,
|
|
role="rgb_dense_stereo",
|
|
latency_ms=45.0,
|
|
)
|
|
|
|
|
|
class LiveClapCalibrator:
|
|
def __init__(
|
|
self,
|
|
*,
|
|
audio_cache: Path,
|
|
clap_cache: Path,
|
|
camera_width: int,
|
|
camera_height: int,
|
|
max_frames_per_sensor: int = 720,
|
|
reservoir_ns: int = DEFAULT_RESERVOIR_NS,
|
|
) -> None:
|
|
self.audio_cache = audio_cache
|
|
self.clap_cache = clap_cache
|
|
self.max_frames_per_sensor = max(2, int(max_frames_per_sensor))
|
|
self.history = TimestampedFrameHistory(max_frames=self.max_frames_per_sensor, max_age_ns=reservoir_ns)
|
|
self.sync_model = CameraClockSyncModel()
|
|
self.last_audio_frame_id: int | None = None
|
|
self.last_audio_time_ns: int | None = None
|
|
self.last_event_key: str | None = None
|
|
left = rgb_dense_camera("kiyo-primary", -0.18, camera_width, camera_height)
|
|
right = rgb_dense_camera("kiyo-secondary", 0.18, camera_width, camera_height)
|
|
self.rig = SensorRig(
|
|
cameras={left.sensor_id: left, right.sensor_id: right},
|
|
config=FusionConfig(max_pair_dt_ns=80_000_000, max_reprojection_error_px=80.0, cache_ttl_ns=reservoir_ns),
|
|
)
|
|
self.config = ClapDetectorConfig(
|
|
audio_window_samples=96,
|
|
audio_hop_samples=24,
|
|
audio_onset_ratio=2.0,
|
|
min_audio_energy=1.0e-8,
|
|
min_camera_motion_score=0.010,
|
|
camera_window_ns=2_500_000_000,
|
|
min_camera_count=2,
|
|
min_event_spacing_ns=220_000_000,
|
|
)
|
|
self.events = ()
|
|
self._restore_existing_events()
|
|
|
|
def _restore_existing_events(self) -> None:
|
|
if not self.clap_cache.exists():
|
|
return
|
|
frame = get_live_clap_events(self.clap_cache)
|
|
if frame is None or not frame.events:
|
|
return
|
|
events: list[ClapCalibrationEvent] = []
|
|
for event in frame.events:
|
|
peaks = tuple(
|
|
CameraMotionPeak(
|
|
sensor_id=str(peak.get("sensorId", "")),
|
|
timestamp_ns=int(peak.get("timestampNs", 0)),
|
|
uv=np.asarray(peak.get("uv", [0.0, 0.0]), dtype=np.float64),
|
|
score=float(peak.get("score", 0.0)),
|
|
)
|
|
for peak in event.camera_peaks
|
|
)
|
|
restored = ClapCalibrationEvent(
|
|
stable_key=event.stable_key,
|
|
position_m=event.position_m,
|
|
acoustic_oracle_ns=event.acoustic_oracle_ns,
|
|
visual_observed_ns=event.visual_observed_ns,
|
|
timing_uncertainty_us=event.timing_uncertainty_us,
|
|
visual_confidence=event.visual_confidence,
|
|
acoustic_confidence=event.acoustic_confidence,
|
|
camera_peaks=peaks,
|
|
)
|
|
self.sync_model.observe_event(restored)
|
|
events.append(restored)
|
|
self.events = tuple(events)
|
|
|
|
def observe_frames(self, frames: dict[str, TimestampedFrame]) -> None:
|
|
self.history.add_many(frames)
|
|
|
|
def update(self, frame_id: int) -> tuple[RenderPointPacket, ...]:
|
|
audio = self._read_audio()
|
|
if audio is None or audio.frame_id == self.last_audio_frame_id:
|
|
return self._event_points(frame_id)
|
|
self.last_audio_frame_id = audio.frame_id
|
|
self.last_audio_time_ns = int(audio.audio_time_ns)
|
|
block = frame_to_numpy(audio)
|
|
events = detect_clap_events(
|
|
block,
|
|
audio.sample_rate,
|
|
self._frames_for_detection(),
|
|
audio_time_ns=audio.audio_time_ns,
|
|
start_sample=audio.start_sample,
|
|
rig=self.rig,
|
|
config=self.config,
|
|
)
|
|
if events:
|
|
self.events = tuple(events)
|
|
self.last_event_key = events[-1].stable_key
|
|
for event in events:
|
|
self.sync_model.observe_event(event)
|
|
put_live_clap_events(self.clap_cache, make_clap_events_frame(frame_id=frame_id, events=events))
|
|
elif not self.events:
|
|
put_live_clap_events(self.clap_cache, make_clap_events_frame(frame_id=frame_id, events=()))
|
|
return self._event_points(frame_id)
|
|
|
|
def _read_audio(self):
|
|
if not self.audio_cache.exists():
|
|
return None
|
|
return get_live_spatial_audio_frame(self.audio_cache)
|
|
|
|
def _frames_for_detection(self) -> dict[str, list[TimestampedFrame]]:
|
|
return self.history.frames_by_sensor()
|
|
|
|
def oracle_time_ns(self, fallback_ns: int) -> int:
|
|
return int(self.last_audio_time_ns if self.last_audio_time_ns is not None else fallback_ns)
|
|
|
|
def _event_points(self, timestamp_ns: int) -> tuple[RenderPointPacket, ...]:
|
|
points: list[RenderPointPacket] = []
|
|
for event in self.events[-4:]:
|
|
confidence = max(0.0, min(1.0, event.visual_confidence * event.acoustic_confidence))
|
|
points.append(
|
|
render_point(
|
|
f"clap-calibration:{event.stable_key}",
|
|
np.asarray(event.position_m, dtype=np.float64),
|
|
0.075,
|
|
(0.15, 0.95, 1.0, 0.72),
|
|
confidence,
|
|
timestamp_ns,
|
|
)
|
|
)
|
|
for peak in event.camera_peaks:
|
|
camera = self.rig.cameras.get(peak.sensor_id)
|
|
if peak.sensor_id == "leap":
|
|
points.append(
|
|
render_point(
|
|
f"clap-timing:{event.stable_key}:leap",
|
|
np.asarray(event.position_m, dtype=np.float64) + np.array([0.0, -0.035, 0.12], dtype=np.float64),
|
|
0.04,
|
|
(0.45, 1.0, 0.72, 0.82),
|
|
max(0.0, min(1.0, peak.score)),
|
|
int(peak.timestamp_ns),
|
|
)
|
|
)
|
|
continue
|
|
if camera is None:
|
|
continue
|
|
ray = pixel_ray_point(camera, peak.uv, distance_m=1.2)
|
|
points.append(
|
|
render_point(
|
|
f"clap-ray:{event.stable_key}:{peak.sensor_id}",
|
|
ray,
|
|
0.026,
|
|
(0.2, 0.85, 1.0, 0.42),
|
|
max(0.0, min(1.0, peak.score)),
|
|
int(peak.timestamp_ns),
|
|
)
|
|
)
|
|
for estimate in self.sync_model.estimates():
|
|
points.append(
|
|
render_point(
|
|
f"camera-sync:{estimate.sensor_id}",
|
|
sync_status_position(estimate.sensor_id),
|
|
0.035,
|
|
(0.45, 0.72, 1.0, 0.58),
|
|
estimate.confidence,
|
|
estimate.oracle_timestamp_ns,
|
|
)
|
|
)
|
|
return tuple(points)
|
|
|
|
|
|
def sync_status_position(sensor_id: str) -> np.ndarray:
|
|
positions = {
|
|
"kiyo-primary": np.array([-0.18, -0.72, 1.28], dtype=np.float64),
|
|
"kiyo-secondary": np.array([0.18, -0.72, 1.28], dtype=np.float64),
|
|
"leap": np.array([0.0, -0.18, 0.76], dtype=np.float64),
|
|
}
|
|
return positions.get(sensor_id, np.zeros(3, dtype=np.float64))
|
|
|
|
|
|
class LiveChirpPoseMapper:
|
|
def __init__(self, *, phase_cache: Path, audio_profile: Path | None = None) -> None:
|
|
self.phase_cache = phase_cache
|
|
self.camera_mics = None
|
|
self.speakers = None
|
|
if audio_profile is not None and audio_profile.exists():
|
|
loaded_mics = camera_mic_geometry_from_audio_profile(audio_profile)
|
|
loaded_speakers = speaker_geometry_from_audio_profile(audio_profile)
|
|
self.camera_mics = loaded_mics or None
|
|
self.speakers = loaded_speakers or None
|
|
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,
|
|
camera_mics=self.camera_mics,
|
|
speakers=self.speakers,
|
|
)
|
|
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))
|
|
for estimate in estimate_pose_corrections(self.constraints):
|
|
points.extend(chirp_pose_correction_points(estimate, 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 chirp_pose_correction_points(estimate, timestamp_ns: int) -> tuple[RenderPointPacket, ...]:
|
|
confidence = max(0.0, min(1.0, float(estimate.confidence)))
|
|
start = np.asarray(estimate.position_m, dtype=np.float64)
|
|
correction = np.asarray(estimate.correction_m, dtype=np.float64)
|
|
end = start + correction
|
|
magnitude = float(np.linalg.norm(correction))
|
|
strength = min(1.0, magnitude / 0.25)
|
|
alpha = 0.22 + 0.55 * strength * confidence
|
|
return (
|
|
render_point(
|
|
f"camera-pose-correction:{estimate.sensor_id}:origin",
|
|
start,
|
|
0.022 + 0.045 * strength,
|
|
(0.35, 0.78, 1.0, alpha),
|
|
confidence,
|
|
timestamp_ns,
|
|
),
|
|
render_point(
|
|
f"camera-pose-correction:{estimate.sensor_id}:target",
|
|
end,
|
|
0.028 + 0.055 * strength,
|
|
(1.0, 0.90, 0.30, alpha),
|
|
confidence,
|
|
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
|
|
direction_sensor = direction_sensor / max(1.0e-12, np.linalg.norm(direction_sensor))
|
|
direction_world = camera.world_from_sensor[:3, :3] @ direction_sensor
|
|
direction_world = direction_world / max(1.0e-12, np.linalg.norm(direction_world))
|
|
return camera.position_world + direction_world * float(distance_m)
|
|
|
|
|
|
def rgb_body_splats(
|
|
prefix: str,
|
|
frame_bgr: np.ndarray,
|
|
origin: np.ndarray,
|
|
now_s: float,
|
|
timestamp_ns: int,
|
|
*,
|
|
side: float,
|
|
step: int,
|
|
) -> list[RenderPointPacket]:
|
|
height, width = frame_bgr.shape[:2]
|
|
crop = person_crop(frame_bgr, side)
|
|
y0, y1, x0, x1 = crop
|
|
crop_pixels = frame_bgr[y0:y1, x0:x1].astype(np.float32) / 255.0
|
|
exposure = max(0.18, float(np.percentile(crop_pixels, 92))) if crop_pixels.size else 1.0
|
|
points: list[RenderPointPacket] = []
|
|
for row in range(y0, y1, step):
|
|
for col in range(x0, x1, step):
|
|
pixel = frame_bgr[row, col].astype(np.float32) / 255.0
|
|
pixel = np.clip(np.power(np.clip(pixel / exposure, 0.0, 2.2), 0.72), 0.0, 1.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.045 and saturation < 0.08:
|
|
continue
|
|
u = (col - x0) / max(1, x1 - x0 - 1)
|
|
v = (row - y0) / max(1, y1 - y0 - 1)
|
|
body = body_surface_point(u, v, side, now_s)
|
|
thickness = 0.045 * np.sin((u * 11.0) + (v * 7.0) + now_s) + 0.030 * (luminance - 0.35)
|
|
xyz = origin + body + np.array([0.0, thickness, 0.0])
|
|
b, g, r = pixel
|
|
alpha = max(0.34, min(0.92, 0.28 + luminance * 0.72 + saturation * 0.22))
|
|
confidence = max(0.45, min(1.0, 0.55 + luminance * 0.35 + saturation * 0.25))
|
|
radius = 0.014 + 0.020 * max(0.0, 1.0 - abs(v - 0.48) * 1.4)
|
|
points.append(
|
|
render_point(
|
|
f"{prefix}:{row}:{col}",
|
|
xyz,
|
|
radius,
|
|
(float(r), float(g), float(b), alpha),
|
|
confidence,
|
|
timestamp_ns,
|
|
)
|
|
)
|
|
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)
|
|
threshold = max(18.0, float(np.percentile(gray, 63)))
|
|
mask = gray > threshold
|
|
if mask.any():
|
|
ys, xs = np.nonzero(mask)
|
|
x0 = max(0, int(np.percentile(xs, 8)) - width // 20)
|
|
x1 = min(width, int(np.percentile(xs, 94)) + width // 20)
|
|
y0 = max(0, int(np.percentile(ys, 4)) - height // 24)
|
|
y1 = min(height, int(np.percentile(ys, 96)) + height // 24)
|
|
else:
|
|
x0, x1 = int(width * 0.18), int(width * 0.82)
|
|
y0, y1 = int(height * 0.05), int(height * 0.95)
|
|
if x1 - x0 < width * 0.25:
|
|
center = int(width * (0.42 if side < 0 else 0.58))
|
|
half = int(width * 0.24)
|
|
x0, x1 = max(0, center - half), min(width, center + half)
|
|
return y0, y1, x0, x1
|
|
|
|
|
|
def body_surface_point(u: float, v: float, side: float, now_s: float) -> np.ndarray:
|
|
theta = (u - 0.5) * np.pi * 1.15
|
|
if v < 0.24:
|
|
vv = v / 0.24
|
|
radius_x = 0.18 * np.sin(max(0.03, vv) * np.pi)
|
|
z = 1.42 + vv * 0.34
|
|
x = radius_x * np.sin(theta) + 0.025 * np.sin(now_s)
|
|
y = 0.02 * np.cos(theta)
|
|
elif v < 0.78:
|
|
vv = (v - 0.24) / 0.54
|
|
width = 0.24 * (1.0 - abs(vv - 0.48) * 0.42)
|
|
z = 0.74 + (1.0 - vv) * 0.68
|
|
x = width * np.sin(theta)
|
|
y = 0.055 * np.cos(theta)
|
|
else:
|
|
vv = (v - 0.78) / 0.22
|
|
arm = side * (0.20 + 0.26 * abs(u - 0.5) * 2.0)
|
|
x = arm * vv + 0.06 * np.sin(theta)
|
|
y = 0.045 * np.cos(theta)
|
|
z = 1.02 - 0.34 * vv
|
|
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,
|
|
fallback_only: bool = False,
|
|
) -> 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.fallback_only = bool(fallback_only)
|
|
self._capture = None
|
|
self._pump: LatestFramePump | None = None
|
|
self._previous_channels: dict[str, np.ndarray] = {}
|
|
self._last_timestamp_ns: int | None = None
|
|
self._last_frame_kind: str | None = None
|
|
self._latest_timestamped_frame: TimestampedFrame | None = None
|
|
self._fallback_index = 0
|
|
|
|
@property
|
|
def last_timestamp_ns(self) -> int | None:
|
|
return self._last_timestamp_ns
|
|
|
|
@property
|
|
def last_frame_kind(self) -> str | None:
|
|
return self._last_frame_kind
|
|
|
|
def latest_timestamped_frames(self) -> dict[str, TimestampedFrame]:
|
|
if self._latest_timestamped_frame is None:
|
|
return {}
|
|
return {self._latest_timestamped_frame.sensor_id: self._latest_timestamped_frame}
|
|
|
|
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()
|
|
self._latest_timestamped_frame = TimestampedFrame("leap", self._last_timestamp_ns, frame.copy())
|
|
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:
|
|
if self.fallback_only:
|
|
return self._fallback_frame()
|
|
if self._pump is None:
|
|
self._pump = LatestFramePump(
|
|
lambda: OpenCvFrameSource(
|
|
OpenCvCaptureConfig(
|
|
sensor_id="leap",
|
|
index=self.index,
|
|
api=self.api,
|
|
width=self.width,
|
|
height=self.height,
|
|
fps=self.fps,
|
|
)
|
|
)
|
|
)
|
|
self._pump.start()
|
|
packet = self._pump.latest(max_age_ns=DEFAULT_RESERVOIR_NS)
|
|
if packet is not None:
|
|
self._last_frame_kind = "capture"
|
|
return packet.image_bgr
|
|
return self._fallback_frame()
|
|
|
|
def _fallback_frame(self) -> np.ndarray | None:
|
|
import cv2
|
|
|
|
path = self.fallback_dir / "leap-probe.png"
|
|
if path.exists():
|
|
frame = cv2.imread(str(path))
|
|
if frame is not None:
|
|
self._last_frame_kind = "fallback-file"
|
|
return frame
|
|
return self._synthetic_motion_frame()
|
|
|
|
def _synthetic_motion_frame(self) -> np.ndarray:
|
|
self._last_frame_kind = "fallback-synthetic"
|
|
frame = np.zeros((self.height, self.width, 3), dtype=np.uint8)
|
|
t = self._fallback_index
|
|
self._fallback_index += 1
|
|
cx = int((self.width * 0.30) + (self.width * 0.40) * ((np.sin(t * 0.37) + 1.0) * 0.5))
|
|
cy = int((self.height * 0.30) + (self.height * 0.34) * ((np.cos(t * 0.41) + 1.0) * 0.5))
|
|
y0 = max(0, cy - 4)
|
|
y1 = min(self.height, cy + 5)
|
|
x0 = max(0, cx - 4)
|
|
x1 = min(self.width, cx + 5)
|
|
frame[y0:y1, x0:x1, 1] = 220
|
|
frame[max(0, y0 - 8) : min(self.height, y1 + 8), max(0, x0 - 8) : min(self.width, x1 + 8), 2] = 80
|
|
frame[max(0, y0 - 12) : min(self.height, y1 + 12), max(0, x0 - 12) : min(self.width, x1 + 12), 0] = 55
|
|
return frame
|
|
|
|
|
|
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"))
|
|
parser.add_argument("--fps", type=float, default=30.0)
|
|
parser.add_argument("--points", type=int, default=256)
|
|
parser.add_argument("--duration", type=float)
|
|
parser.add_argument("--no-rgb", action="store_true")
|
|
parser.add_argument("--rgb-api", choices=["any", "dshow", "msmf"], default="dshow")
|
|
parser.add_argument("--rgb-fallback-only", action="store_true")
|
|
parser.add_argument("--rgb-primary-index", type=int, default=1)
|
|
parser.add_argument("--rgb-secondary-index", type=int, default=3)
|
|
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("--cpu-dense-stereo", action="store_true", help="Debug-only CPU stereo matcher. Production dense fusion belongs on the GPU.")
|
|
parser.add_argument("--rgb-dense-step", type=int, default=16)
|
|
parser.add_argument("--no-adaptive-camera-controls", action="store_true")
|
|
parser.add_argument("--no-leap", action="store_true")
|
|
parser.add_argument("--leap-api", choices=["any", "dshow", "msmf"], default="msmf")
|
|
parser.add_argument("--leap-fallback-only", action="store_true")
|
|
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)
|
|
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("--audio-profile", default=str(ROOT / "config" / "audio-field.json"))
|
|
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")
|
|
parser.add_argument("--reservoir-seconds", type=float, default=5.0)
|
|
args = parser.parse_args()
|
|
reservoir_ns = max(1, int(round(args.reservoir_seconds * 1_000_000_000)))
|
|
|
|
left = camera("ps3eye_left", -0.25)
|
|
right = camera("ps3eye_right", 0.25)
|
|
rig = SensorRig(
|
|
cameras={left.sensor_id: left, right.sensor_id: right},
|
|
config=FusionConfig(max_pair_dt_ns=30_000_000, max_reprojection_error_px=0.01, cache_ttl_ns=reservoir_ns),
|
|
)
|
|
render_config = RenderBridgeConfig(default_point_radius_m=0.035)
|
|
interval = 1.0 / max(1.0, args.fps)
|
|
start = time.monotonic()
|
|
frame_id = 0
|
|
cache_path = Path(args.cache)
|
|
lod_cache_path = Path(args.lod_cache)
|
|
lock = acquire_runtime_lock(cache_path.with_suffix(".producer.lock"))
|
|
if lock is None:
|
|
return
|
|
rgb_sampler = None if args.no_rgb else RgbSplatSampler(
|
|
api=args.rgb_api,
|
|
primary_index=args.rgb_primary_index,
|
|
secondary_index=args.rgb_secondary_index,
|
|
width=args.rgb_width,
|
|
height=args.rgb_height,
|
|
sample_step=args.rgb_sample_step,
|
|
room_step=args.rgb_room_step,
|
|
dense_step=args.rgb_dense_step,
|
|
enable_cpu_dense_stereo=args.cpu_dense_stereo,
|
|
adaptive_controls=not args.no_adaptive_camera_controls,
|
|
fallback_dir=ROOT / "calibration" / "runs",
|
|
reservoir_ns=reservoir_ns,
|
|
fallback_only=args.rgb_fallback_only,
|
|
)
|
|
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",
|
|
fallback_only=args.leap_fallback_only,
|
|
)
|
|
clap_calibrator = None if args.no_clap_calibration else LiveClapCalibrator(
|
|
audio_cache=Path(args.audio_cache),
|
|
clap_cache=Path(args.clap_cache),
|
|
camera_width=args.rgb_width,
|
|
camera_height=args.rgb_height,
|
|
reservoir_ns=reservoir_ns,
|
|
)
|
|
audio_profile = Path(args.audio_profile)
|
|
if not audio_profile.exists() and audio_profile.name == "audio-field.json":
|
|
example_profile = audio_profile.with_name("audio-field.example.json")
|
|
audio_profile = example_profile if example_profile.exists() else audio_profile
|
|
chirp_pose_mapper = None if args.no_chirp_pose else LiveChirpPoseMapper(
|
|
phase_cache=Path(args.phase_cache),
|
|
audio_profile=audio_profile if audio_profile.exists() else None,
|
|
)
|
|
try:
|
|
while True:
|
|
now = time.monotonic()
|
|
if args.duration is not None and now - start >= args.duration:
|
|
break
|
|
observations = synthetic_observations(rig, now, args.points)
|
|
result = rig.fuse(observations)
|
|
transient_matches = () if args.no_stochastic_transients else stochastic_transient_matches(
|
|
stochastic_surface_observations(observations),
|
|
rig.cameras,
|
|
max_descriptor_distance=0.0,
|
|
max_reprojection_error_px=0.05,
|
|
samples_per_observation=2,
|
|
seed=frame_id,
|
|
)
|
|
frame = lower_points_to_render_frame(
|
|
result.points,
|
|
render_config,
|
|
frame_id=frame_id,
|
|
created_monotonic_ns=time.monotonic_ns(),
|
|
)
|
|
timestamp_ns = frame.source_time_max_ns
|
|
leap_points = () if leap_sampler is None else leap_sampler.motion_splats(timestamp_ns)
|
|
if clap_calibrator is not None:
|
|
if rgb_sampler is not None:
|
|
clap_calibrator.observe_frames(rgb_sampler.latest_timestamped_frames())
|
|
if leap_sampler is not None:
|
|
clap_calibrator.observe_frames(leap_sampler.latest_timestamped_frames())
|
|
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)
|
|
audio_edge_ns = latest_audio_time_ns(Path(args.audio_cache))
|
|
reservoir_edge_ns = max(timestamp_ns, audio_edge_ns if audio_edge_ns is not None else timestamp_ns)
|
|
leap_source_kind = (
|
|
"leap-ground-truth"
|
|
if leap_sampler is not None and leap_sampler.last_frame_kind == "capture"
|
|
else "leap-fallback"
|
|
)
|
|
leap_source_priority = 3.0 if leap_source_kind == "leap-ground-truth" else 1.1
|
|
rgb_points = ()
|
|
if rgb_sampler is not None:
|
|
if clap_calibrator is not None and clap_calibrator.sync_model.estimates():
|
|
rgb_points = rgb_sampler.synced_splats(
|
|
now,
|
|
timestamp_ns,
|
|
sync_model=clap_calibrator.sync_model,
|
|
oracle_time_ns=clap_calibrator.oracle_time_ns(timestamp_ns),
|
|
)
|
|
else:
|
|
rgb_points = rgb_sampler.splats(now, timestamp_ns)
|
|
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(reservoir_edge_ns)
|
|
chirp_pose_points = () if chirp_pose_mapper is None else chirp_pose_mapper.update(reservoir_edge_ns)
|
|
render_points = render_points_in_reservoir(
|
|
clap_points + chirp_pose_points + leap_points + rgb_points + stochastic_points + support_points,
|
|
latest_timestamp_ns=reservoir_edge_ns,
|
|
reservoir_ns=reservoir_ns,
|
|
)
|
|
evidence = evidence_in_reservoir(
|
|
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),
|
|
latest_timestamp_ns=reservoir_edge_ns,
|
|
reservoir_ns=reservoir_ns,
|
|
)
|
|
source_time_min_ns, source_time_max_ns = reservoir_window_ns(reservoir_edge_ns, reservoir_ns)
|
|
multilod_cache_from_evidence(
|
|
evidence,
|
|
levels=(0.01, 0.04, 0.16, 0.64),
|
|
created_monotonic_ns=time.monotonic_ns(),
|
|
).write_json(lod_cache_path)
|
|
frame = RenderFramePacket(
|
|
schema=frame.schema,
|
|
frame_id=frame.frame_id,
|
|
created_monotonic_ns=frame.created_monotonic_ns,
|
|
source_time_min_ns=source_time_min_ns,
|
|
source_time_max_ns=source_time_max_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=render_points,
|
|
)
|
|
put_live_render_frame(cache_path, frame)
|
|
frame_id += 1
|
|
sleep_for = interval - (time.monotonic() - now)
|
|
if sleep_for > 0:
|
|
time.sleep(sleep_for)
|
|
finally:
|
|
if rgb_sampler is not None:
|
|
rgb_sampler.close()
|
|
if leap_sampler is not None:
|
|
leap_sampler.close()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|