"""Lissage EMA et IIR pour commandes eyes/neck.""" import time import logging from typing import Optional, Tuple from dataclasses import dataclass logger = logging.getLogger(__name__) @dataclass class SmoothedPose: """Pose lissée avec timestamp.""" yaw: float pitch: float confidence: float target_id: Optional[int] timestamp: float class EMAFilter: """Filtre EMA (Exponential Moving Average) pour lissage.""" def __init__(self, alpha: float = 0.5): self.alpha = max(0.01, min(0.99, alpha)) # Clamp [0.01, 0.99] self._smoothed_value: Optional[float] = None self._initialized = False def update(self, new_value: float) -> float: """Met à jour et retourne valeur lissée.""" if not self._initialized: self._smoothed_value = new_value self._initialized = True return new_value self._smoothed_value = ( self.alpha * new_value + (1.0 - self.alpha) * self._smoothed_value ) return self._smoothed_value def reset(self) -> None: """Remet à zéro le filtre.""" self._smoothed_value = None self._initialized = False @property def value(self) -> Optional[float]: return self._smoothed_value @property def is_initialized(self) -> bool: return self._initialized class PoseSmoother: """Lisseur de pose avec filtres EMA séparés.""" def __init__(self, eyes_alpha: float = 0.5, neck_alpha: float = 0.2): # Filtres pour eyes (plus réactif) self.eyes_yaw_filter = EMAFilter(eyes_alpha) self.eyes_pitch_filter = EMAFilter(eyes_alpha) self.eyes_conf_filter = EMAFilter(eyes_alpha) # Filtres pour neck (plus lent/stable) self.neck_yaw_filter = EMAFilter(neck_alpha) self.neck_pitch_filter = EMAFilter(neck_alpha) self.neck_conf_filter = EMAFilter(neck_alpha) self._last_target_id: Optional[int] = None def update_pose( self, yaw: float, pitch: float, confidence: float, target_id: Optional[int] ) -> Tuple[SmoothedPose, SmoothedPose]: """Met à jour et retourne poses lissées pour eyes et neck.""" current_time = time.time() # Reset si changement de cible if target_id != self._last_target_id: self._reset_filters() self._last_target_id = target_id # Lisser pour eyes (réactif) eyes_yaw = self.eyes_yaw_filter.update(yaw) eyes_pitch = self.eyes_pitch_filter.update(pitch) eyes_conf = self.eyes_conf_filter.update(confidence) eyes_pose = SmoothedPose( yaw=eyes_yaw, pitch=eyes_pitch, confidence=eyes_conf, target_id=target_id, timestamp=current_time, ) # Lisser pour neck (stable) neck_yaw = self.neck_yaw_filter.update(yaw) neck_pitch = self.neck_pitch_filter.update(pitch) neck_conf = self.neck_conf_filter.update(confidence) neck_pose = SmoothedPose( yaw=neck_yaw, pitch=neck_pitch, confidence=neck_conf, target_id=target_id, timestamp=current_time, ) return eyes_pose, neck_pose def _reset_filters(self) -> None: """Remet à zéro tous les filtres.""" self.eyes_yaw_filter.reset() self.eyes_pitch_filter.reset() self.eyes_conf_filter.reset() self.neck_yaw_filter.reset() self.neck_pitch_filter.reset() self.neck_conf_filter.reset() def update_alphas(self, eyes_alpha: float, neck_alpha: float) -> None: """Met à jour les coefficients de lissage.""" # Mise à jour eyes if abs(self.eyes_yaw_filter.alpha - eyes_alpha) > 0.01: old_eyes = self.eyes_yaw_filter.alpha self.eyes_yaw_filter.alpha = max(0.01, min(0.99, eyes_alpha)) self.eyes_pitch_filter.alpha = self.eyes_yaw_filter.alpha self.eyes_conf_filter.alpha = self.eyes_yaw_filter.alpha logger.debug( f"Eyes alpha: {old_eyes:.2f} -> {self.eyes_yaw_filter.alpha:.2f}" ) # Mise à jour neck if abs(self.neck_yaw_filter.alpha - neck_alpha) > 0.01: old_neck = self.neck_yaw_filter.alpha self.neck_yaw_filter.alpha = max(0.01, min(0.99, neck_alpha)) self.neck_pitch_filter.alpha = self.neck_yaw_filter.alpha self.neck_conf_filter.alpha = self.neck_yaw_filter.alpha logger.debug( f"Neck alpha: {old_neck:.2f} -> {self.neck_yaw_filter.alpha:.2f}" ) def get_current_poses( self, ) -> Tuple[Optional[SmoothedPose], Optional[SmoothedPose]]: """Retourne les dernières poses lissées si disponibles.""" current_time = time.time() eyes_pose = None if ( self.eyes_yaw_filter.is_initialized and self.eyes_pitch_filter.is_initialized and self.eyes_conf_filter.is_initialized ): eyes_pose = SmoothedPose( yaw=self.eyes_yaw_filter.value, pitch=self.eyes_pitch_filter.value, confidence=self.eyes_conf_filter.value, target_id=self._last_target_id, timestamp=current_time, ) neck_pose = None if ( self.neck_yaw_filter.is_initialized and self.neck_pitch_filter.is_initialized and self.neck_conf_filter.is_initialized ): neck_pose = SmoothedPose( yaw=self.neck_yaw_filter.value, pitch=self.neck_pitch_filter.value, confidence=self.neck_conf_filter.value, target_id=self._last_target_id, timestamp=current_time, ) return eyes_pose, neck_pose class DelayBuffer: """Buffer de retard pour commandes neck.""" def __init__(self, delay_ms: int = 120): self.delay_ms = delay_ms self._buffer = [] # Liste de (timestamp, pose) def add_pose(self, pose: SmoothedPose) -> None: """Ajoute pose au buffer avec timestamp.""" self._buffer.append((time.time(), pose)) # Nettoyer buffer ancien cutoff_time = time.time() - (self.delay_ms + 500) / 1000.0 # +500ms marge self._buffer = [(t, p) for t, p in self._buffer if t > cutoff_time] def get_delayed_pose(self) -> Optional[SmoothedPose]: """Récupère pose avec retard appliqué.""" if not self._buffer: return None target_time = time.time() - self.delay_ms / 1000.0 # Chercher pose la plus proche du temps cible best_pose = None best_diff = float("inf") for timestamp, pose in self._buffer: diff = abs(timestamp - target_time) if diff < best_diff: best_diff = diff best_pose = pose # Retourner seulement si proche du délai voulu (±50ms) if best_diff <= 0.05: # 50ms de tolérance return best_pose return None def update_delay(self, delay_ms: int) -> None: """Met à jour le délai.""" self.delay_ms = max(0, delay_ms) logger.debug(f"Délai neck mis à jour: {self.delay_ms}ms") def clear(self) -> None: """Vide le buffer.""" self._buffer.clear() @property def buffer_size(self) -> int: return len(self._buffer) class SmoothingManager: """Gestionnaire centralisé du lissage.""" def __init__( self, eyes_alpha: float = 0.5, neck_alpha: float = 0.2, neck_lag_ms: int = 120 ): self.pose_smoother = PoseSmoother(eyes_alpha, neck_alpha) self.delay_buffer = DelayBuffer(neck_lag_ms) def process_pose( self, yaw: float, pitch: float, confidence: float, target_id: Optional[int] ) -> Tuple[SmoothedPose, Optional[SmoothedPose]]: """Traite pose brute et retourne poses lissées (eyes immédiate, neck retardée).""" # Lisser eyes_pose, neck_pose_smooth = self.pose_smoother.update_pose( yaw, pitch, confidence, target_id ) # Ajouter au buffer de retard if neck_pose_smooth: self.delay_buffer.add_pose(neck_pose_smooth) # Récupérer pose neck retardée neck_pose_delayed = self.delay_buffer.get_delayed_pose() return eyes_pose, neck_pose_delayed def update_config( self, eyes_alpha: float, neck_alpha: float, neck_lag_ms: int ) -> None: """Met à jour configuration lissage.""" self.pose_smoother.update_alphas(eyes_alpha, neck_alpha) self.delay_buffer.update_delay(neck_lag_ms) def reset(self) -> None: """Remet à zéro lissage et buffer.""" self.pose_smoother._reset_filters() self.delay_buffer.clear()