2a25db8498
Computes Interaural Level Difference (dB) from left/right ear energy. Fuses with triangulated distance (70/30 weight) for more robust estimate. Classifies into proximity zones: intimate (<0.5m), conversational (0.5-2m), across_room (2-5m), far (>5m). ILD→distance mapping is empirical and should be calibrated per install. Gaze vertical component now responds to proximity (closer = eyes down). Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
285 lines
11 KiB
Python
285 lines
11 KiB
Python
"""
|
|
Binaural spatial hearing — triangulation, tracking, gaze.
|
|
|
|
Combines DoA angles from two XVF3800 arrays into a sound source position,
|
|
smooths the tracking, and pushes gaze coordinates to the eye service.
|
|
"""
|
|
|
|
import logging
|
|
import math
|
|
import time
|
|
from typing import Optional
|
|
|
|
logger = logging.getLogger("headmic.spatial")
|
|
|
|
# Array geometry (measured on skull, can be overridden from config)
|
|
DEFAULT_ARRAY_SEPARATION_MM = 175.0 # center-to-center distance between arrays
|
|
|
|
# Gaze mapping
|
|
GAZE_CENTER = 127 # neutral gaze (0-255 range)
|
|
GAZE_X_RANGE = 80 # max horizontal deflection from center
|
|
GAZE_Y_RANGE = 30 # max vertical deflection from center
|
|
GAZE_MAX_DISTANCE_MM = 3000 # beyond this, gaze is "far" (no convergence)
|
|
|
|
# Smoothing
|
|
SMOOTHING_ALPHA = 0.4 # exponential smoothing (0=sluggish, 1=instant) — slightly snappy
|
|
IDLE_RETURN_SPEED = 0.03 # how fast gaze drifts to center when no VAD — gentle drift
|
|
IDLE_TIMEOUT_S = 1.5 # seconds of no VAD before drifting to center
|
|
|
|
# Distance estimation (ILD-based)
|
|
# ILD = 20 * log10(louder_energy / quieter_energy) in dB
|
|
# Empirical mapping: ILD varies with angle and distance.
|
|
# At 175mm separation, a source at 45° off-center produces:
|
|
# ~0.5m: ILD ≈ 6-10 dB
|
|
# ~1.5m: ILD ≈ 3-5 dB
|
|
# ~3.0m: ILD ≈ 1-2 dB
|
|
# These are rough — calibrate on real hardware.
|
|
PROXIMITY_ZONES = [
|
|
("intimate", 0, 500), # < 0.5m — whispering distance
|
|
("conversational", 500, 2000), # 0.5-2m — normal talking
|
|
("across_room", 2000, 5000), # 2-5m — raised voice
|
|
("far", 5000, 99999), # > 5m — shouting distance
|
|
]
|
|
|
|
|
|
class SpatialTracker:
|
|
"""Triangulates sound source from two DoA angles and produces smooth gaze."""
|
|
|
|
def __init__(self, array_separation_mm: float = DEFAULT_ARRAY_SEPARATION_MM):
|
|
self.separation = array_separation_mm
|
|
self.half_sep = array_separation_mm / 2.0
|
|
|
|
# Smoothed state
|
|
self._smooth_x: float = 0.0 # mm, relative to skull center
|
|
self._smooth_y: float = 0.0 # mm, forward from skull
|
|
self._smooth_gaze_x: float = float(GAZE_CENTER)
|
|
self._smooth_gaze_y: float = float(GAZE_CENTER)
|
|
self._smooth_distance: float = GAZE_MAX_DISTANCE_MM
|
|
self._smooth_ild: float = 0.0 # dB
|
|
|
|
# VAD tracking
|
|
self._last_vad_time: float = 0.0
|
|
self._any_vad: bool = False
|
|
|
|
# Last raw result for API
|
|
self.last_position: Optional[dict] = None
|
|
|
|
def update(self, doa: dict, left_energy: float = 0.0, right_energy: float = 0.0) -> Optional[dict]:
|
|
"""
|
|
Process DoA readings + audio energy from both arrays.
|
|
|
|
Args:
|
|
doa: {"left": {"angle": 0-359, "vad": bool}, "right": {"angle": 0-359, "vad": bool}}
|
|
left_energy: RMS energy from left mic stream (0.0-1.0)
|
|
right_energy: RMS energy from right mic stream (0.0-1.0)
|
|
|
|
Returns:
|
|
{"x_mm", "y_mm", "distance_mm", "ild_db", "proximity",
|
|
"gaze_x", "gaze_y", "vad", "side"}
|
|
or None if insufficient data.
|
|
"""
|
|
left = doa.get("left")
|
|
right = doa.get("right")
|
|
|
|
if not left or not right:
|
|
return self._idle_drift()
|
|
|
|
left_vad = left.get("vad", False)
|
|
right_vad = right.get("vad", False)
|
|
any_vad = left_vad or right_vad
|
|
|
|
if any_vad:
|
|
self._last_vad_time = time.monotonic()
|
|
self._any_vad = True
|
|
|
|
left_angle = left["angle"]
|
|
right_angle = right["angle"]
|
|
|
|
# Triangulate position
|
|
pos = self._triangulate(left_angle, right_angle)
|
|
|
|
# Compute ILD (Interaural Level Difference)
|
|
ild_db = self._compute_ild(left_energy, right_energy)
|
|
|
|
if pos and any_vad:
|
|
# Smooth the position
|
|
self._smooth_x += SMOOTHING_ALPHA * (pos["x_mm"] - self._smooth_x)
|
|
self._smooth_y += SMOOTHING_ALPHA * (pos["y_mm"] - self._smooth_y)
|
|
self._smooth_ild += SMOOTHING_ALPHA * (ild_db - self._smooth_ild)
|
|
|
|
# Fuse triangulated distance with ILD
|
|
tri_dist = math.sqrt(self._smooth_x**2 + self._smooth_y**2)
|
|
ild_dist = self._ild_to_distance(self._smooth_ild)
|
|
# Weighted average: trust triangulation more (0.7) but let ILD correct it (0.3)
|
|
fused_dist = 0.7 * tri_dist + 0.3 * ild_dist
|
|
self._smooth_distance += SMOOTHING_ALPHA * (fused_dist - self._smooth_distance)
|
|
elif not any_vad:
|
|
return self._idle_drift()
|
|
|
|
# Convert to gaze
|
|
gaze_x, gaze_y = self._position_to_gaze(self._smooth_x, self._smooth_y)
|
|
|
|
# Smooth gaze
|
|
self._smooth_gaze_x += SMOOTHING_ALPHA * (gaze_x - self._smooth_gaze_x)
|
|
self._smooth_gaze_y += SMOOTHING_ALPHA * (gaze_y - self._smooth_gaze_y)
|
|
|
|
# Classify proximity zone
|
|
proximity = self._classify_proximity(self._smooth_distance)
|
|
|
|
result = {
|
|
"x_mm": round(self._smooth_x, 1),
|
|
"y_mm": round(self._smooth_y, 1),
|
|
"distance_mm": round(self._smooth_distance, 1),
|
|
"ild_db": round(self._smooth_ild, 1),
|
|
"proximity": proximity,
|
|
"gaze_x": int(round(self._smooth_gaze_x)),
|
|
"gaze_y": int(round(self._smooth_gaze_y)),
|
|
"vad": any_vad,
|
|
"side": "left" if self._smooth_x < 0 else "right",
|
|
}
|
|
self.last_position = result
|
|
return result
|
|
|
|
def _idle_drift(self) -> Optional[dict]:
|
|
"""When no VAD, smoothly return gaze to center."""
|
|
elapsed = time.monotonic() - self._last_vad_time
|
|
|
|
if elapsed < IDLE_TIMEOUT_S:
|
|
# Hold last position briefly
|
|
return self.last_position
|
|
|
|
# Drift toward center
|
|
self._smooth_gaze_x += IDLE_RETURN_SPEED * (GAZE_CENTER - self._smooth_gaze_x)
|
|
self._smooth_gaze_y += IDLE_RETURN_SPEED * (GAZE_CENTER - self._smooth_gaze_y)
|
|
|
|
result = {
|
|
"x_mm": round(self._smooth_x, 1),
|
|
"y_mm": round(self._smooth_y, 1),
|
|
"distance_mm": round(self._smooth_distance, 1),
|
|
"ild_db": round(self._smooth_ild, 1),
|
|
"proximity": self._classify_proximity(self._smooth_distance),
|
|
"gaze_x": int(round(self._smooth_gaze_x)),
|
|
"gaze_y": int(round(self._smooth_gaze_y)),
|
|
"vad": False,
|
|
"side": "center",
|
|
}
|
|
self.last_position = result
|
|
return result
|
|
|
|
@staticmethod
|
|
def _compute_ild(left_energy: float, right_energy: float) -> float:
|
|
"""Compute Interaural Level Difference in dB.
|
|
Positive = louder on left, negative = louder on right."""
|
|
# Clamp to avoid log(0)
|
|
left_e = max(left_energy, 1e-10)
|
|
right_e = max(right_energy, 1e-10)
|
|
return 20.0 * math.log10(left_e / right_e)
|
|
|
|
@staticmethod
|
|
def _ild_to_distance(ild_db: float) -> float:
|
|
"""Estimate distance from ILD magnitude.
|
|
Higher ILD = closer source (head shadow effect is stronger up close).
|
|
This is a rough empirical mapping — should be calibrated per-installation."""
|
|
ild_abs = abs(ild_db)
|
|
if ild_abs > 8.0:
|
|
return 300.0 # very close, ~30cm
|
|
elif ild_abs > 5.0:
|
|
return 700.0 # close, ~70cm
|
|
elif ild_abs > 3.0:
|
|
return 1500.0 # conversational, ~1.5m
|
|
elif ild_abs > 1.5:
|
|
return 2500.0 # across room, ~2.5m
|
|
else:
|
|
return 4000.0 # far or directly ahead (no ILD)
|
|
|
|
@staticmethod
|
|
def _classify_proximity(distance_mm: float) -> str:
|
|
"""Classify distance into a proximity zone."""
|
|
for name, lo, hi in PROXIMITY_ZONES:
|
|
if lo <= distance_mm < hi:
|
|
return name
|
|
return "far"
|
|
|
|
def _triangulate(self, left_deg: float, right_deg: float) -> Optional[dict]:
|
|
"""
|
|
Triangulate sound source position from two DoA angles.
|
|
|
|
Array coordinate system:
|
|
- Origin: center of skull
|
|
- X axis: positive = right (toward right ear)
|
|
- Y axis: positive = forward (in front of skull)
|
|
|
|
Each array's DoA is 0° = front, 90° = right, 180° = back, 270° = left.
|
|
The arrays are positioned at (-half_sep, 0) and (+half_sep, 0).
|
|
"""
|
|
# Convert DoA angles to bearing vectors
|
|
# DoA 0° = forward (+Y), 90° = right (+X) for each array
|
|
left_rad = math.radians(left_deg)
|
|
right_rad = math.radians(right_deg)
|
|
|
|
# Direction vectors from each array position
|
|
# Left array at (-half_sep, 0), right array at (+half_sep, 0)
|
|
left_dx = math.sin(left_rad)
|
|
left_dy = math.cos(left_rad)
|
|
right_dx = math.sin(right_rad)
|
|
right_dy = math.cos(right_rad)
|
|
|
|
# Solve intersection of two rays:
|
|
# P_left + t * D_left = P_right + s * D_right
|
|
# (-half_sep + t*left_dx, t*left_dy) = (half_sep + s*right_dx, s*right_dy)
|
|
#
|
|
# t*left_dx - s*right_dx = separation
|
|
# t*left_dy - s*right_dy = 0
|
|
|
|
denom = left_dx * right_dy - left_dy * right_dx
|
|
|
|
if abs(denom) < 0.001:
|
|
# Parallel rays — can't triangulate, source is very far away or directly ahead
|
|
# Fall back to bearing midpoint at a default distance
|
|
avg_rad = (left_rad + right_rad) / 2
|
|
return {
|
|
"x_mm": GAZE_MAX_DISTANCE_MM * math.sin(avg_rad),
|
|
"y_mm": GAZE_MAX_DISTANCE_MM * math.cos(avg_rad),
|
|
}
|
|
|
|
t = (self.separation * right_dy) / denom
|
|
|
|
if t < 0:
|
|
# Intersection is behind the arrays — likely noise or rear source
|
|
# Use the bearing with positive t scaled to max distance
|
|
avg_rad = (left_rad + right_rad) / 2
|
|
return {
|
|
"x_mm": GAZE_MAX_DISTANCE_MM * math.sin(avg_rad) * 0.5,
|
|
"y_mm": GAZE_MAX_DISTANCE_MM * math.cos(avg_rad) * 0.5,
|
|
}
|
|
|
|
# Compute intersection point relative to left array, then shift to skull center
|
|
x = -self.half_sep + t * left_dx
|
|
y = t * left_dy
|
|
|
|
return {"x_mm": x, "y_mm": y}
|
|
|
|
def _position_to_gaze(self, x_mm: float, y_mm: float) -> tuple[float, float]:
|
|
"""
|
|
Convert position (mm) to gaze coordinates (0-255).
|
|
|
|
Horizontal: source on the right → eyes look right (gaze_x > 127)
|
|
Vertical: source closer → eyes look slightly down, farther → straight ahead
|
|
"""
|
|
distance = math.sqrt(x_mm**2 + y_mm**2)
|
|
if distance < 1.0:
|
|
return float(GAZE_CENTER), float(GAZE_CENTER)
|
|
|
|
# Horizontal: angle from center
|
|
angle = math.atan2(x_mm, max(y_mm, 100.0)) # clamp y to avoid extreme angles
|
|
# Map angle (roughly -pi/2 to pi/2) to gaze range
|
|
gaze_x = GAZE_CENTER + GAZE_X_RANGE * (angle / (math.pi / 2))
|
|
gaze_x = max(GAZE_CENTER - GAZE_X_RANGE, min(GAZE_CENTER + GAZE_X_RANGE, gaze_x))
|
|
|
|
# Vertical: closer = slightly down, far = center
|
|
# This simulates looking down at someone close vs straight ahead at someone far
|
|
proximity = max(0.0, 1.0 - distance / GAZE_MAX_DISTANCE_MM)
|
|
gaze_y = GAZE_CENTER + GAZE_Y_RANGE * proximity * 0.3 # subtle effect
|
|
|
|
return gaze_x, gaze_y
|