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

src/config_loader.py

@@ -2,7 +2,7 @@
 
 from __future__ import annotations
 
-from dataclasses import asdict, dataclass, fields
+from dataclasses import MISSING, asdict, dataclass, fields
 from pathlib import Path
 from typing import Any, Mapping, TypeVar
 
@@ -10,6 +10,7 @@ import yaml
 
 from .exceptions import ConfigurationError
 from .models import PixelRegion, PixelTriggerLine
+from .yaw_filters import FILTER_KINDS
 
 
 @dataclass(frozen=True, slots=True)
@@ -198,9 +199,19 @@ class HeadPoseSettings:
     enabled: bool
     method: str
     smoothing_enabled: bool
+    # none | ema | median_ema | hampel_ema | one_euro | median_one_euro (see src/yaw_filters.py).
     smoothing_method: str
+    # EMA coefficient; also used by median_ema and hampel_ema.
     ema_alpha: float
     max_abs_angle: float
+    # Fields below are optional in YAML; the defaults reproduce the original behaviour.
+    smoothing_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 filter state when pose samples are further apart than this [s]; 0 disables.
+    filter_reset_gap_sec: float = 0.0
 
 
 @dataclass(frozen=True, slots=True)
@@ -499,7 +510,9 @@ def _make(cls: type[T], values: Any, section: str) -> T:
     if not isinstance(values, Mapping):
         raise ConfigurationError(f"'{section}' must be a mapping")
     expected = {field.name for field in fields(cls)}
-    missing = expected - values.keys()
+    required = {field.name for field in fields(cls)
+                if field.default is MISSING and field.default_factory is MISSING}
+    missing = required - values.keys()
     unknown = values.keys() - expected
     if missing:
         raise ConfigurationError(f"Missing settings in '{section}': {sorted(missing)}")
@@ -584,10 +597,22 @@ def _validate(config: AppConfig) -> None:
     if not 0.0 <= face.min_detection_confidence <= 1.0 or not 0.0 <= face.min_tracking_confidence <= 1.0:
         raise ConfigurationError("face confidence thresholds must be in [0, 1]")
     pose = config.head_pose
-    if pose.method != "solvepnp" or pose.smoothing_method != "ema":
-        raise ConfigurationError("head_pose method must be solvepnp and smoothing_method must be ema")
+    if pose.method != "solvepnp" or pose.smoothing_method not in FILTER_KINDS:
+        raise ConfigurationError(
+            f"head_pose method must be solvepnp and smoothing_method one of {list(FILTER_KINDS)}")
     if not 0.0 < pose.ema_alpha <= 1.0 or pose.max_abs_angle <= 0:
         raise ConfigurationError("head_pose EMA alpha and maximum angle are invalid")
+    numeric = (pose.smoothing_window, pose.hampel_threshold, pose.one_euro_min_cutoff, pose.one_euro_beta,
+               pose.one_euro_d_cutoff, pose.filter_reset_gap_sec)
+    if any(isinstance(value, bool) or not isinstance(value, (int, float)) for value in numeric):
+        raise ConfigurationError("head_pose filter settings must be numeric")
+    if not isinstance(pose.smoothing_window, int) or pose.smoothing_window < 1:
+        raise ConfigurationError("head_pose.smoothing_window must be a positive integer")
+    if pose.smoothing_method == "hampel_ema" and pose.smoothing_window < 3:
+        raise ConfigurationError("head_pose.smoothing_window must be >= 3 for hampel_ema")
+    if (pose.hampel_threshold <= 0 or pose.one_euro_min_cutoff <= 0 or pose.one_euro_d_cutoff <= 0
+            or pose.one_euro_beta < 0 or pose.filter_reset_gap_sec < 0):
+        raise ConfigurationError("head_pose filter settings are out of range")
     if config.face_logging.flush_interval_frames <= 0:
         raise ConfigurationError("face_logging.flush_interval_frames must be positive")
     display_bools = (config.display.show_face_roi, config.display.show_head_pose,

src/face_processor.py

@@ -12,6 +12,7 @@ from .exceptions import FaceProcessingError
 from .face_geometry import create_face_roi
 from .head_pose import HeadPoseEstimator
 from .models import FaceLandmark, FaceTrackState, FrameInfo, HeadPoseResult, TrackedPerson
+from .yaw_filters import AngleFilter, create_angle_filter
 
 LOGGER = logging.getLogger(__name__)
 
@@ -24,7 +25,7 @@ class FaceProcessor:
         self.head_pose_config = head_pose_config
         self.estimator = HeadPoseEstimator(head_pose_config)
         self._mesh: Any = None
-        self._smoothed: dict[int, tuple[float, float, float]] = {}
+        self._filters: dict[int, tuple[AngleFilter, AngleFilter, AngleFilter]] = {}
         if config.enabled:
             self._initialize()
 
@@ -96,7 +97,9 @@ class FaceProcessor:
                 raw_pitch=estimation.pitch, raw_yaw=estimation.yaw, raw_roll=estimation.roll,
                 reprojection_error=estimation.reprojection_error, interocular_px=interocular,
             )
-        pitch, yaw, roll = self._smooth(person.track_id, estimation.pitch, estimation.yaw, estimation.roll)
+        pitch, yaw, roll = self._smooth(
+            person.track_id, estimation.pitch, estimation.yaw, estimation.roll, info.elapsed_time_sec,
+        )
         return HeadPoseResult(
             person.track_id, info.frame_number, info.timestamp_iso, info.elapsed_time_sec,
             True, True, pitch, yaw, roll, len(landmarks), roi, roi.confidence, "", tuple(landmarks),
@@ -131,19 +134,33 @@ class FaceProcessor:
             for index, point in enumerate(raw_landmarks)
         ]
 
+    def _new_filters(self) -> tuple[AngleFilter, AngleFilter, AngleFilter]:
+        """One independent filter per angle (pitch, yaw, roll), built from head_pose settings."""
+        settings = self.head_pose_config
+        kind = settings.smoothing_method if settings.smoothing_enabled else "none"
+        return tuple(  # type: ignore[return-value]
+            create_angle_filter(
+                kind, alpha=settings.ema_alpha, window=settings.smoothing_window,
+                hampel_threshold=settings.hampel_threshold,
+                one_euro_min_cutoff=settings.one_euro_min_cutoff, one_euro_beta=settings.one_euro_beta,
+                one_euro_d_cutoff=settings.one_euro_d_cutoff, reset_gap_sec=settings.filter_reset_gap_sec,
+            )
+            for _ in range(3)
+        )
+
     def _smooth(
         self, track_id: int, pitch: float | None, yaw: float | None, roll: float | None,
+        elapsed_time_sec: float,
     ) -> tuple[float, float, float]:
         assert pitch is not None and yaw is not None and roll is not None
-        current = (pitch, yaw, roll)
-        if not self.head_pose_config.smoothing_enabled or track_id not in self._smoothed:
-            smoothed = current
-        else:
-            alpha = self.head_pose_config.ema_alpha
-            previous = self._smoothed[track_id]
-            smoothed = tuple(alpha * value + (1.0 - alpha) * old for value, old in zip(current, previous))
-        self._smoothed[track_id] = smoothed
-        return smoothed
+        filters = self._filters.get(track_id)
+        if filters is None:
+            filters = self._filters[track_id] = self._new_filters()
+        smoothed = tuple(
+            float(angle_filter.update(value, elapsed_time_sec))
+            for angle_filter, value in zip(filters, (pitch, yaw, roll))
+        )
+        return smoothed  # type: ignore[return-value]
 
     @staticmethod
     def _failure(track_id: int, info: FrameInfo, reason: str, roi: Any = None) -> HeadPoseResult:
@@ -151,13 +168,13 @@ class FaceProcessor:
                               False, False, None, None, None, 0, roi, None, reason)
 
     def reset(self) -> None:
-        self._smoothed.clear()
+        self._filters.clear()
 
     def close(self) -> None:
         if self._mesh is not None:
             self._mesh.close()
             self._mesh = None
-        self._smoothed.clear()
+        self._filters.clear()
 
 
 class FaceTrackManager:

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

src/yaw_noise_report.py

@@ -21,6 +21,8 @@ import sys
 from pathlib import Path
 from typing import Callable, Sequence
 
+from .yaw_filters import create_angle_filter
+
 Sample = tuple[float, float]  # (elapsed_time_sec, yaw_deg)
 
 
@@ -140,6 +142,12 @@ def load_tracks(
     return tracks
 
 
+def apply_filter(kind: str, samples: Sequence[Sample], **params: float) -> list[float]:
+    """Run a ``src.yaw_filters`` filter over (time, yaw) samples."""
+    angle_filter = create_angle_filter(kind, **params)  # type: ignore[arg-type]
+    return [angle_filter.update(value, t) for t, value in samples]
+
+
 def build_rows(entry: dict[str, list]) -> list[tuple[str, dict[str, float]]]:
     """Statistics rows: logged yaw, raw yaw, and offline filters applied to raw yaw."""
     rows: list[tuple[str, dict[str, float]]] = []
@@ -159,6 +167,14 @@ def build_rows(entry: dict[str, list]) -> list[tuple[str, dict[str, float]]]:
         add("raw + median3 + EMA a=0.4", median_then_ema(values, 0.4))
         add("raw + OneEuro mincut=1.0 beta=0.0", one_euro(raw, 1.0, 0.0))
         add("raw + OneEuro mincut=0.5 beta=0.01", one_euro(raw, 0.5, 0.01))
+        add("raw + median5 + EMA a=0.4", apply_filter("median_ema", raw, window=5, alpha=0.4))
+        add("raw + median5 + EMA a=0.3", apply_filter("median_ema", raw, window=5, alpha=0.3))
+        add("raw + Hampel w7 k3 + EMA a=0.4",
+            apply_filter("hampel_ema", raw, window=7, hampel_threshold=3.0, alpha=0.4))
+        add("raw + median5 + OneEuro mincut=1.0 beta=0.02",
+            apply_filter("median_one_euro", raw, window=5, one_euro_min_cutoff=1.0, one_euro_beta=0.02))
+        add("raw + median5 + OneEuro mincut=1.5 beta=0.0",
+            apply_filter("median_one_euro", raw, window=5, one_euro_min_cutoff=1.5, one_euro_beta=0.0))
     return rows