796 lines
30 KiB
Python
796 lines
30 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,
|
|
dense_stereo_points,
|
|
measure_frame_quality,
|
|
put_live_render_frame,
|
|
lower_points_to_render_frame,
|
|
)
|
|
from localcast.sensor_fusion.camera_control import AdaptiveCameraController, OpenCvCameraSettingPort
|
|
|
|
|
|
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 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 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,
|
|
) -> 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._captures: dict[int, object] = {}
|
|
self._frames: dict[int, np.ndarray] = {}
|
|
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)
|
|
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 _read_frame(self, index: int) -> np.ndarray | None:
|
|
import cv2
|
|
|
|
capture = self._captures.get(index)
|
|
if capture is None:
|
|
capture = cv2.VideoCapture(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_BUFFERSIZE, 1)
|
|
self._captures[index] = capture
|
|
if self.adaptive_controls:
|
|
self._controllers[index] = AdaptiveCameraController()
|
|
self._setting_ports[index] = OpenCvCameraSettingPort(capture)
|
|
else:
|
|
try:
|
|
capture.release()
|
|
except Exception:
|
|
pass
|
|
return self._fallback_frame(index)
|
|
ok = False
|
|
frame = None
|
|
for _ in range(2):
|
|
ok, frame = capture.read()
|
|
if ok and frame is not None:
|
|
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:
|
|
return frame
|
|
return None
|
|
|
|
|
|
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,
|
|
)
|
|
|
|
|
|
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,
|
|
) -> 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"))
|
|
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-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-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)
|
|
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=500_000_000),
|
|
)
|
|
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)
|
|
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",
|
|
)
|
|
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:
|
|
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)
|
|
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
|
|
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=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=leap_points + rgb_points + support_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()
|