再投影誤差による破綻フレームの除外および窓の時間指定の実装と実験陽設定の更新

src/yaw_filters.py

@@ -25,8 +25,12 @@ Kinds
                      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``).
+Median-type windows hold ``window`` samples by default, so their duration depends
+on the pose update rate (frame rate / ``face.process_every_n_frames``).  With
+``window_sec > 0`` the window instead holds every sample from the last
+``window_sec`` seconds (trimmed to an odd count), which keeps the filter strength
+independent of the update rate.  A median removes a spike run of at most
+(n - 1) / 2 samples out of n.
 """
 
 from __future__ import annotations
@@ -44,6 +48,39 @@ HAMPEL_MIN_SCALE_DEG = 1.0
 _MAD_TO_SIGMA = 1.4826
 
 
+class _SampleWindow:
+    """Recent samples bounded by count, or by age when ``duration_sec`` > 0."""
+
+    MAX_SAMPLES_IN_DURATION_MODE = 256
+
+    def __init__(self, size: int, duration_sec: float = 0.0) -> None:
+        if size < 1:
+            raise ValueError("window must be >= 1")
+        if duration_sec < 0.0:
+            raise ValueError("window_sec must be >= 0")
+        self._size = size
+        self._duration = duration_sec
+        self._items: deque[tuple[float, float]] = deque()
+
+    def add(self, t: float, value: float) -> list[float]:
+        """Append a sample and return the values currently inside the window."""
+        self._items.append((t, value))
+        if self._duration > 0.0:
+            while len(self._items) > 1 and t - self._items[0][0] > self._duration:
+                self._items.popleft()
+            while len(self._items) > self.MAX_SAMPLES_IN_DURATION_MODE:
+                self._items.popleft()
+            values = [item[1] for item in self._items]
+            # An odd count makes the median an actual sample (and symmetric).
+            return values[1:] if len(values) % 2 == 0 else values
+        while len(self._items) > self._size:
+            self._items.popleft()
+        return [item[1] for item in self._items]
+
+    def clear(self) -> None:
+        self._items.clear()
+
+
 class AngleFilter(Protocol):
     def update(self, value: float, t: float) -> float: ...
 
@@ -76,15 +113,12 @@ class EmaFilter:
 
 
 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)
+    def __init__(self, window: int, alpha: float, window_sec: float = 0.0) -> None:
+        self._window = _SampleWindow(window, window_sec)
         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)
+        return self._ema.update(statistics.median(self._window.add(t, value)), t)
 
     def reset(self) -> None:
         self._window.clear()
@@ -92,21 +126,21 @@ class MedianEmaFilter:
 
 
 class HampelEmaFilter:
-    def __init__(self, window: int, threshold: float, alpha: float) -> None:
+    def __init__(self, window: int, threshold: float, alpha: float, window_sec: float = 0.0) -> 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._window = _SampleWindow(window, window_sec)
         self._ema = EmaFilter(alpha)
 
     def update(self, value: float, t: float) -> float:
-        self._window.append(value)
+        values = self._window.add(t, value)
         cleaned = value
-        if len(self._window) >= 3:
-            center = statistics.median(self._window)
-            mad = statistics.median(abs(item - center) for item in self._window)
+        if len(values) >= 3:
+            center = statistics.median(values)
+            mad = statistics.median(abs(item - center) for item in values)
             scale = max(_MAD_TO_SIGMA * mad, HAMPEL_MIN_SCALE_DEG)
             if abs(value - center) > self.threshold * scale:
                 cleaned = center
@@ -155,15 +189,14 @@ class OneEuroFilter:
 
 
 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)
+    def __init__(
+        self, window: int, min_cutoff: float, beta: float, d_cutoff: float = 1.0, window_sec: float = 0.0,
+    ) -> None:
+        self._window = _SampleWindow(window, window_sec)
         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)
+        return self._euro.update(statistics.median(self._window.add(t, value)), t)
 
     def reset(self) -> None:
         self._window.clear()
@@ -203,6 +236,7 @@ def create_angle_filter(
     one_euro_beta: float = 0.0,
     one_euro_d_cutoff: float = 1.0,
     reset_gap_sec: float = 0.0,
+    window_sec: float = 0.0,
 ) -> AngleFilter:
     """Build a filter by name; raises ``ValueError`` for unknown kinds."""
     inner: AngleFilter
@@ -211,13 +245,13 @@ def create_angle_filter(
     elif kind == "ema":
         inner = EmaFilter(alpha)
     elif kind == "median_ema":
-        inner = MedianEmaFilter(window, alpha)
+        inner = MedianEmaFilter(window, alpha, window_sec)
     elif kind == "hampel_ema":
-        inner = HampelEmaFilter(window, hampel_threshold, alpha)
+        inner = HampelEmaFilter(window, hampel_threshold, alpha, window_sec)
     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)
+        inner = MedianOneEuroFilter(window, one_euro_min_cutoff, one_euro_beta, one_euro_d_cutoff, window_sec)
     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