Mimir/localcast/sensor_fusion/rgb_reference.py
2026-05-19 11:13:32 +01:00

213 lines
7.8 KiB
Python

"""CPU reference RGB surface lowering used by diagnostics and tests."""
import numpy as np
from .core import CameraModel
from .dense_stereo import DenseStereoConfig, dense_stereo_points
from .render_bridge import RenderPointPacket
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]
y0, y1, x0, x1 = person_crop(frame_bgr, side)
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:
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)
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,
)