cae14023b7
Cross-correlates left/right ear audio frames (512 samples, ~32ms window) to find the sub-millisecond delay between arrays. Converts delay to bearing angle using speed of sound and array separation. At 16kHz with 175mm separation, resolution is ~1 sample = 62.5μs = ~7°. Not lab-grade, but adds a third independent angle estimate alongside DoA and ILD. Works with current 2-channel firmware — no raw mics needed. New fields in /doa spatial response: itd_angle: bearing from cross-correlation (degrees) itd_delay_us: raw time delay (microseconds, positive = source on right) Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
385 lines
15 KiB
Python
385 lines
15 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
|
|
|
|
import numpy as np
|
|
|
|
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
|
|
|
|
# ITD (Interaural Time Difference)
|
|
SPEED_OF_SOUND_MM_S = 343000.0 # ~343 m/s in mm/s
|
|
SAMPLE_RATE = 16000
|
|
ITD_MAX_DELAY_SAMPLES = 9 # ±175mm / (343m/s * 62.5μs/sample) ≈ ±8.2 samples
|
|
ITD_WEIGHT = 0.3 # weight of ITD angle in fusion (DoA=0.5, ITD=0.3, ILD=0.2)
|
|
DOA_WEIGHT = 0.5
|
|
ILD_DIST_WEIGHT = 0.3
|
|
|
|
# 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
|
|
self._smooth_itd_angle: float = 0.0 # degrees, from cross-correlation
|
|
self._last_itd_samples: float = 0.0 # raw delay in samples
|
|
|
|
# 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,
|
|
left_audio: bytes = None, right_audio: bytes = None) -> Optional[dict]:
|
|
"""
|
|
Process DoA readings + audio energy + raw audio 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)
|
|
left_audio: raw PCM bytes from left ear (int16, for ITD cross-correlation)
|
|
right_audio: raw PCM bytes from right ear (int16, for ITD cross-correlation)
|
|
|
|
Returns:
|
|
{"x_mm", "y_mm", "distance_mm", "ild_db", "itd_angle", "itd_delay_us",
|
|
"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)
|
|
|
|
# Compute ITD if we have audio from both ears
|
|
itd_angle = None
|
|
if left_audio and right_audio and any_vad:
|
|
itd_result = self._compute_itd(left_audio, right_audio)
|
|
if itd_result is not None:
|
|
itd_angle, self._last_itd_samples = itd_result
|
|
self._smooth_itd_angle += SMOOTHING_ALPHA * (
|
|
self._shortest_angle_diff(itd_angle, self._smooth_itd_angle))
|
|
|
|
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)
|
|
fused_dist = (1.0 - ILD_DIST_WEIGHT) * tri_dist + ILD_DIST_WEIGHT * 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),
|
|
"itd_angle": round(self._smooth_itd_angle, 1),
|
|
"itd_delay_us": round(self._last_itd_samples * 1e6 / SAMPLE_RATE, 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)
|
|
|
|
def _compute_itd(self, left_audio: bytes, right_audio: bytes) -> Optional[tuple[float, float]]:
|
|
"""Compute Interaural Time Difference via cross-correlation.
|
|
|
|
Returns (angle_degrees, delay_samples) or None if insufficient data.
|
|
Positive delay = sound arrives at right ear first = source on right.
|
|
"""
|
|
try:
|
|
left = np.frombuffer(left_audio, dtype=np.int16).astype(np.float32)
|
|
right = np.frombuffer(right_audio, dtype=np.int16).astype(np.float32)
|
|
except Exception:
|
|
return None
|
|
|
|
min_len = min(len(left), len(right))
|
|
if min_len < 64:
|
|
return None
|
|
|
|
# Use the last 512 samples (~32ms window) for correlation
|
|
window = min(512, min_len)
|
|
left = left[-window:]
|
|
right = right[-window:]
|
|
|
|
# Normalize to prevent overflow
|
|
left_norm = np.linalg.norm(left)
|
|
right_norm = np.linalg.norm(right)
|
|
if left_norm < 1.0 or right_norm < 1.0:
|
|
return None # silence
|
|
left = left / left_norm
|
|
right = right / right_norm
|
|
|
|
# Cross-correlate within the expected delay range
|
|
max_delay = ITD_MAX_DELAY_SAMPLES
|
|
corr = np.correlate(left, right, mode='full')
|
|
# corr center is at index len(left)-1, corresponding to zero delay
|
|
center = len(left) - 1
|
|
search = corr[center - max_delay:center + max_delay + 1]
|
|
|
|
if len(search) == 0:
|
|
return None
|
|
|
|
# Peak delay in samples (positive = right leads = source on right)
|
|
peak_idx = np.argmax(search)
|
|
delay_samples = peak_idx - max_delay # centered: negative=left leads, positive=right leads
|
|
|
|
# Convert delay to angle
|
|
# delay_samples * (1/sample_rate) = time_diff
|
|
# sin(angle) = time_diff * speed_of_sound / separation
|
|
time_diff = delay_samples / SAMPLE_RATE
|
|
sin_angle = (time_diff * SPEED_OF_SOUND_MM_S) / self.separation
|
|
|
|
# Clamp to valid range (cross-correlation can overshoot)
|
|
sin_angle = max(-1.0, min(1.0, sin_angle))
|
|
angle_deg = math.degrees(math.asin(sin_angle))
|
|
|
|
# Convert from ±90° (negative=left, positive=right) to 0-360° convention
|
|
# 0°=front, 90°=right, 270°=left
|
|
if angle_deg >= 0:
|
|
bearing = 90.0 - angle_deg # right side: 0° → 90°, 90° → 0°
|
|
else:
|
|
bearing = 270.0 + angle_deg # left side: -90° → 180°
|
|
|
|
# Keep in 0-360
|
|
bearing = bearing % 360
|
|
|
|
return bearing, delay_samples
|
|
|
|
@staticmethod
|
|
def _shortest_angle_diff(target: float, current: float) -> float:
|
|
"""Shortest signed difference between two angles, for smooth interpolation."""
|
|
diff = target - current
|
|
if diff > 180:
|
|
diff -= 360
|
|
elif diff < -180:
|
|
diff += 360
|
|
return diff
|
|
|
|
@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
|