静止時のノイズ対処のためyaw値フィルタ機能を追加

src/yaw_filters.py

@@ -0,0 +1,223 @@
+"""Causal, dependency-free smoothing filters for head-pose angles.
+
+All filters share one interface so they can be swapped from configuration::
+
+    f = create_angle_filter("median_ema", alpha=0.4, window=5)
+    smoothed = f.update(raw_angle_deg, elapsed_time_sec)
+
+``update`` must be called once per valid sample in time order.  ``reset`` forgets
+all state (call it when a track is lost).  Every filter is causal, so each adds
+some delay; see the per-class notes.  Delay and noise should be judged together
+(see ``src.yaw_noise_report``).
+
+Kinds
+-----
+``none``             pass-through.
+``ema``              exponential moving average, per sample (the original behaviour).
+``median_ema``       sliding median over ``window`` samples, then EMA.  Removes
+                     isolated spikes; step delay is about (window - 1) / 2 samples.
+``hampel_ema``       Hampel identifier: a sample further than ``hampel_threshold``
+                     robust sigmas from the window median is replaced by that median,
+                     everything else passes unchanged, then EMA.  Keeps small real
+                     changes sharper than a plain median.
+``one_euro``         One Euro Filter (time based).  Beware: with very noisy input the
+                     speed term inflates the cutoff, so keep ``beta`` near 0 or
+                     put a median in front (``median_one_euro``).
+``median_one_euro``  sliding median, then One Euro Filter.
+
+Window sizes are counted in samples, so their duration depends on the pose update
+rate (frame rate / ``face.process_every_n_frames``).
+"""
+
+from __future__ import annotations
+
+import math
+import statistics
+from collections import deque
+from typing import Protocol
+
+FILTER_KINDS: tuple[str, ...] = ("none", "ema", "median_ema", "hampel_ema", "one_euro", "median_one_euro")
+
+# Floor for the robust sigma used by the Hampel identifier [deg]; avoids flagging
+# every sample as an outlier when the window is nearly constant.
+HAMPEL_MIN_SCALE_DEG = 1.0
+_MAD_TO_SIGMA = 1.4826
+
+
+class AngleFilter(Protocol):
+    def update(self, value: float, t: float) -> float: ...
+
+    def reset(self) -> None: ...
+
+
+class PassThroughFilter:
+    def update(self, value: float, t: float) -> float:
+        return value
+
+    def reset(self) -> None:
+        return None
+
+
+class EmaFilter:
+    """``y = alpha * x + (1 - alpha) * y_prev``; the first sample passes through."""
+
+    def __init__(self, alpha: float) -> None:
+        if not 0.0 < alpha <= 1.0:
+            raise ValueError("alpha must be in (0, 1]")
+        self.alpha = alpha
+        self._state: float | None = None
+
+    def update(self, value: float, t: float) -> float:
+        self._state = value if self._state is None else self.alpha * value + (1.0 - self.alpha) * self._state
+        return self._state
+
+    def reset(self) -> None:
+        self._state = None
+
+
+class MedianEmaFilter:
+    def __init__(self, window: int, alpha: float) -> None:
+        if window < 1:
+            raise ValueError("window must be >= 1")
+        self._window: deque[float] = deque(maxlen=window)
+        self._ema = EmaFilter(alpha)
+
+    def update(self, value: float, t: float) -> float:
+        self._window.append(value)
+        return self._ema.update(statistics.median(self._window), t)
+
+    def reset(self) -> None:
+        self._window.clear()
+        self._ema.reset()
+
+
+class HampelEmaFilter:
+    def __init__(self, window: int, threshold: float, alpha: float) -> None:
+        if window < 3:
+            raise ValueError("hampel window must be >= 3")
+        if threshold <= 0.0:
+            raise ValueError("hampel threshold must be positive")
+        self.threshold = threshold
+        self._window: deque[float] = deque(maxlen=window)
+        self._ema = EmaFilter(alpha)
+
+    def update(self, value: float, t: float) -> float:
+        self._window.append(value)
+        cleaned = value
+        if len(self._window) >= 3:
+            center = statistics.median(self._window)
+            mad = statistics.median(abs(item - center) for item in self._window)
+            scale = max(_MAD_TO_SIGMA * mad, HAMPEL_MIN_SCALE_DEG)
+            if abs(value - center) > self.threshold * scale:
+                cleaned = center
+        return self._ema.update(cleaned, t)
+
+    def reset(self) -> None:
+        self._window.clear()
+        self._ema.reset()
+
+
+class OneEuroFilter:
+    """One Euro Filter (Casiez et al., 2012) driven by real timestamps [s]."""
+
+    def __init__(self, min_cutoff: float, beta: float, d_cutoff: float = 1.0) -> None:
+        if min_cutoff <= 0.0 or d_cutoff <= 0.0 or beta < 0.0:
+            raise ValueError("min_cutoff and d_cutoff must be > 0 and beta >= 0")
+        self.min_cutoff = min_cutoff
+        self.beta = beta
+        self.d_cutoff = d_cutoff
+        self._x: float | None = None
+        self._dx = 0.0
+        self._t = 0.0
+
+    @staticmethod
+    def _alpha(dt: float, cutoff: float) -> float:
+        tau = 1.0 / (2.0 * math.pi * cutoff)
+        return 1.0 / (1.0 + tau / dt)
+
+    def update(self, value: float, t: float) -> float:
+        if self._x is None:
+            self._x, self._t, self._dx = value, t, 0.0
+            return value
+        dt = max(t - self._t, 1e-6)
+        self._t = t
+        raw_dx = (value - self._x) / dt
+        a_d = self._alpha(dt, self.d_cutoff)
+        self._dx = a_d * raw_dx + (1.0 - a_d) * self._dx
+        cutoff = self.min_cutoff + self.beta * abs(self._dx)
+        a = self._alpha(dt, cutoff)
+        self._x = a * value + (1.0 - a) * self._x
+        return self._x
+
+    def reset(self) -> None:
+        self._x = None
+        self._dx = 0.0
+
+
+class MedianOneEuroFilter:
+    def __init__(self, window: int, min_cutoff: float, beta: float, d_cutoff: float = 1.0) -> None:
+        if window < 1:
+            raise ValueError("window must be >= 1")
+        self._window: deque[float] = deque(maxlen=window)
+        self._euro = OneEuroFilter(min_cutoff, beta, d_cutoff)
+
+    def update(self, value: float, t: float) -> float:
+        self._window.append(value)
+        return self._euro.update(statistics.median(self._window), t)
+
+    def reset(self) -> None:
+        self._window.clear()
+        self._euro.reset()
+
+
+class GapResettingFilter:
+    """Reset the wrapped filter when samples are more than ``max_gap_sec`` apart.
+
+    Prevents stale state from blending into the first samples after the face was
+    lost.  ``max_gap_sec <= 0`` disables the behaviour.
+    """
+
+    def __init__(self, inner: AngleFilter, max_gap_sec: float) -> None:
+        self._inner = inner
+        self._max_gap = max_gap_sec
+        self._last_t: float | None = None
+
+    def update(self, value: float, t: float) -> float:
+        if self._max_gap > 0.0 and self._last_t is not None and t - self._last_t > self._max_gap:
+            self._inner.reset()
+        self._last_t = t
+        return self._inner.update(value, t)
+
+    def reset(self) -> None:
+        self._inner.reset()
+        self._last_t = None
+
+
+def create_angle_filter(
+    kind: str,
+    *,
+    alpha: float = 0.4,
+    window: int = 5,
+    hampel_threshold: float = 3.0,
+    one_euro_min_cutoff: float = 1.0,
+    one_euro_beta: float = 0.0,
+    one_euro_d_cutoff: float = 1.0,
+    reset_gap_sec: float = 0.0,
+) -> AngleFilter:
+    """Build a filter by name; raises ``ValueError`` for unknown kinds."""
+    inner: AngleFilter
+    if kind == "none":
+        inner = PassThroughFilter()
+    elif kind == "ema":
+        inner = EmaFilter(alpha)
+    elif kind == "median_ema":
+        inner = MedianEmaFilter(window, alpha)
+    elif kind == "hampel_ema":
+        inner = HampelEmaFilter(window, hampel_threshold, alpha)
+    elif kind == "one_euro":
+        inner = OneEuroFilter(one_euro_min_cutoff, one_euro_beta, one_euro_d_cutoff)
+    elif kind == "median_one_euro":
+        inner = MedianOneEuroFilter(window, one_euro_min_cutoff, one_euro_beta, one_euro_d_cutoff)
+    else:
+        raise ValueError(f"unknown smoothing filter {kind!r}; expected one of {FILTER_KINDS}")
+    return GapResettingFilter(inner, reset_gap_sec) if reset_gap_sec > 0.0 else inner