Mimir/localcast/sensor_fusion/chirp_pose.py
2026-05-19 01:02:14 +01:00

224 lines
8.2 KiB
Python

from __future__ import annotations
from dataclasses import dataclass
import json
from pathlib import Path
from typing import Mapping, Sequence
import numpy as np
SPEED_OF_SOUND_MPS = 343.0
@dataclass(frozen=True)
class CameraMicGeometry:
sensor_id: str
source_id: str
position_m: np.ndarray
@dataclass(frozen=True)
class SpeakerGeometry:
speaker_id: str
position_m: np.ndarray
@dataclass(frozen=True)
class CameraChirpPoseConstraint:
stable_key: str
sensor_id: str
source_id: str
speaker_id: str
camera_position_m: np.ndarray
speaker_position_m: np.ndarray
nominal_range_m: float
observed_range_m: float
range_residual_m: float
confidence: float
timestamp_ns: int
@dataclass(frozen=True)
class CameraPoseCorrectionEstimate:
sensor_id: str
position_m: np.ndarray
correction_m: np.ndarray
confidence: float
constraint_count: int
timestamp_ns: int
def default_camera_mic_geometry() -> tuple[CameraMicGeometry, ...]:
return (
CameraMicGeometry("kiyo-primary", "kiyo-0", np.array([-0.18, -0.72, 1.28], dtype=np.float64)),
CameraMicGeometry("kiyo-secondary", "kiyo-1", np.array([0.18, -0.72, 1.28], dtype=np.float64)),
CameraMicGeometry("ps3eye-left", "ps-eye-0", np.array([-0.78, -0.82, 1.34], dtype=np.float64)),
CameraMicGeometry("ps3eye-right", "ps-eye-1", np.array([0.78, -0.82, 1.34], dtype=np.float64)),
)
def default_speaker_geometry() -> tuple[SpeakerGeometry, ...]:
return (
SpeakerGeometry("local-speaker-left", np.array([-0.32, -1.08, 1.18], dtype=np.float64)),
SpeakerGeometry("local-speaker-right", np.array([0.32, -1.08, 1.18], dtype=np.float64)),
)
def camera_mic_geometry_from_audio_profile(path: Path) -> tuple[CameraMicGeometry, ...]:
data = json.loads(Path(path).read_text(encoding="utf-8"))
rows: list[CameraMicGeometry] = []
for mic in data.get("microphones", []):
source_id = phase_source_id_for_microphone(mic)
sensor_id = camera_sensor_id_for_microphone(mic)
position = mic.get("positionMeters")
if source_id is None or sensor_id is None or position is None:
continue
rows.append(CameraMicGeometry(sensor_id, source_id, np.asarray(position, dtype=np.float64)))
return tuple(rows)
def speaker_geometry_from_audio_profile(path: Path) -> tuple[SpeakerGeometry, ...]:
data = json.loads(Path(path).read_text(encoding="utf-8"))
rows: list[SpeakerGeometry] = []
for speaker in data.get("speakers", []):
position = speaker.get("positionMeters")
if position is None:
continue
rows.append(SpeakerGeometry(str(speaker.get("id", "")), np.asarray(position, dtype=np.float64)))
return tuple(row for row in rows if row.speaker_id)
def phase_source_id_for_microphone(mic: Mapping[str, object]) -> str | None:
explicit = mic.get("phaseSourceId") or mic.get("sourceId")
if explicit:
return str(explicit)
mic_id = str(mic.get("id", ""))
known = {
"mic_kiyo_left": "kiyo-0",
"mic_kiyo_right": "kiyo-1",
"mic_ps_eye_left": "ps-eye-0",
"mic_ps_eye_right": "ps-eye-1",
}
return known.get(mic_id)
def camera_sensor_id_for_microphone(mic: Mapping[str, object]) -> str | None:
explicit = mic.get("cameraSensorId") or mic.get("sensorId")
if explicit:
return str(explicit)
mic_id = str(mic.get("id", ""))
known = {
"mic_kiyo_left": "kiyo-primary",
"mic_kiyo_right": "kiyo-secondary",
"mic_ps_eye_left": "ps3eye-left",
"mic_ps_eye_right": "ps3eye-right",
}
return known.get(mic_id)
def constraints_from_phase_sources(
sources: Sequence[Mapping[str, object]],
*,
audio_time_ns: int,
camera_mics: Sequence[CameraMicGeometry] | None = None,
speakers: Sequence[SpeakerGeometry] | None = None,
max_abs_residual_m: float = 2.0,
) -> tuple[CameraChirpPoseConstraint, ...]:
"""Convert phase-field delay meaning into camera-body range constraints."""
camera_mics = default_camera_mic_geometry() if camera_mics is None else camera_mics
speakers = default_speaker_geometry() if speakers is None else speakers
mic_by_source = {item.source_id: item for item in camera_mics}
if not speakers:
return ()
rows: list[CameraChirpPoseConstraint] = []
for source in sources:
source_id = str(source.get("sourceId", ""))
mic = mic_by_source.get(source_id)
if mic is None:
continue
confidence = max(0.0, min(1.0, float(source.get("confidence", 0.0))))
if confidence <= 0.0:
continue
distance_delta_m = _source_distance_delta_m(source)
if not np.isfinite(distance_delta_m):
continue
speaker = _nearest_speaker(mic.position_m, speakers)
nominal = float(np.linalg.norm(mic.position_m - speaker.position_m))
residual = float(max(-max_abs_residual_m, min(max_abs_residual_m, distance_delta_m)))
observed = max(0.0, nominal + residual)
rows.append(
CameraChirpPoseConstraint(
stable_key=f"camera-chirp:{mic.sensor_id}:{speaker.speaker_id}",
sensor_id=mic.sensor_id,
source_id=source_id,
speaker_id=speaker.speaker_id,
camera_position_m=mic.position_m.astype(np.float64),
speaker_position_m=speaker.position_m.astype(np.float64),
nominal_range_m=nominal,
observed_range_m=observed,
range_residual_m=residual,
confidence=confidence,
timestamp_ns=int(audio_time_ns),
)
)
return tuple(rows)
def estimate_pose_corrections(
constraints: Sequence[CameraChirpPoseConstraint],
*,
max_step_m: float = 0.35,
) -> tuple[CameraPoseCorrectionEstimate, ...]:
grouped: dict[str, list[CameraChirpPoseConstraint]] = {}
for constraint in constraints:
grouped.setdefault(constraint.sensor_id, []).append(constraint)
estimates: list[CameraPoseCorrectionEstimate] = []
for sensor_id, rows in grouped.items():
vectors = []
weights = []
positions = []
for row in rows:
camera = np.asarray(row.camera_position_m, dtype=np.float64)
speaker = np.asarray(row.speaker_position_m, dtype=np.float64)
direction = camera - speaker
norm = float(np.linalg.norm(direction))
if norm <= 1.0e-9:
continue
unit = direction / norm
confidence = max(0.0, min(1.0, float(row.confidence)))
vectors.append(unit * float(row.range_residual_m))
weights.append(max(1.0e-6, confidence))
positions.append(camera)
if not vectors:
continue
weighted = np.average(np.asarray(vectors, dtype=np.float64), axis=0, weights=np.asarray(weights, dtype=np.float64))
magnitude = float(np.linalg.norm(weighted))
if magnitude > max_step_m:
weighted = weighted / magnitude * float(max_step_m)
estimates.append(
CameraPoseCorrectionEstimate(
sensor_id=sensor_id,
position_m=np.average(np.asarray(positions, dtype=np.float64), axis=0, weights=np.asarray(weights, dtype=np.float64)),
correction_m=weighted.astype(np.float64),
confidence=float(min(1.0, np.mean(weights))),
constraint_count=len(vectors),
timestamp_ns=max(row.timestamp_ns for row in rows),
)
)
return tuple(sorted(estimates, key=lambda item: item.sensor_id))
def _nearest_speaker(position_m: np.ndarray, speakers: Sequence[SpeakerGeometry]) -> SpeakerGeometry:
return min(speakers, key=lambda speaker: float(np.linalg.norm(np.asarray(position_m) - speaker.position_m)))
def _source_distance_delta_m(source: Mapping[str, object]) -> float:
if "distanceDeltaMeters" in source:
return float(source["distanceDeltaMeters"])
if "delayMs" in source:
return float(source["delayMs"]) * 0.001 * SPEED_OF_SOUND_MPS
if "delaySamples" in source and "sampleRate" in source:
return float(source["delaySamples"]) / max(1.0, float(source["sampleRate"])) * SPEED_OF_SOUND_MPS
return float("nan")