diff --git a/config/default.yaml b/config/default.yaml index 7b9c298..92ffb88 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -88,9 +88,16 @@ head_pose: enabled: true method: solvepnp smoothing_enabled: true + # none | ema | median_ema | hampel_ema | one_euro | median_one_euro (see src/yaw_filters.py) smoothing_method: ema - ema_alpha: 0.4 + ema_alpha: 0.4 # also used by median_ema / hampel_ema max_abs_angle: 120.0 + smoothing_window: 5 # samples; median_ema / hampel_ema / median_one_euro + hampel_threshold: 3.0 # robust sigmas; hampel_ema + one_euro_min_cutoff: 1.0 # Hz; one_euro / median_one_euro + one_euro_beta: 0.0 # keep ~0 unless a median precedes it (e.g. 0.02) + one_euro_d_cutoff: 1.0 + filter_reset_gap_sec: 0.0 # reset filter state after a pose gap longer than this; 0 = never face_logging: enabled: true log_frame_faces: true diff --git a/docs/head_pose_smoothing.md b/docs/head_pose_smoothing.md new file mode 100644 index 0000000..a64163c --- /dev/null +++ b/docs/head_pose_smoothing.md @@ -0,0 +1,63 @@ +# 頭部角度（Yaw）の平滑化フィルタと静止時ゆらぎ + +静止していてもYawが変化する問題に対する調査記録と、選択可能なフィルタの説明。 + +## 設定（`config/default.yaml` の `head_pose`） + +| キー | 既定値 | 内容 | +|---|---|---| +| `smoothing_enabled` | true | false で平滑化なし（`none` と同じ） | +| `smoothing_method` | `ema` | `none` / `ema` / `median_ema` / `hampel_ema` / `one_euro` / `median_one_euro` | +| `ema_alpha` | 0.4 | EMA係数。`median_ema`・`hampel_ema` でも使用 | +| `smoothing_window` | 5 | 窓幅（サンプル数）。窓の時間幅は姿勢更新レートに依存する | +| `hampel_threshold` | 3.0 | Hampel判定のしきい値（頑健σの倍数） | +| `one_euro_min_cutoff` / `one_euro_beta` / `one_euro_d_cutoff` | 1.0 / 0.0 / 1.0 | One Euro Filter のパラメータ | +| `filter_reset_gap_sec` | 0.0 | 姿勢サンプルがこれより離れたらフィルタ状態をリセット（0で無効） | + +既定値は従来動作（`ema`, α=0.4）と同じ。新キーはYAMLに書かなくてもよい（既定値が使われる）。 +フィルタ本体は `src/yaw_filters.py`。 + +## 計測と比較の手順 + +1. 正面静止で30〜60秒ログを取る（`data/logs/face/*_face_frames.csv`。`raw_yaw` 列が生値）。 +2. `python -m src.yaw_noise_report --track-id N --t-start 4 --t-end 44 --csv out.csv` +3. 各フィルタのSD・最大変動幅・フレーム間ジッタを比較する。 + 遅れは静止データからは測れないので、階段・ランプ入力のシミュレーションまたは実際の振り向き試行で確認する。 + +## 計測結果（2026-10-01, 研究室, 正面・3m・静止, 4〜44秒, n=714, 約17.8Hz, 1280x720） + +| フィルタ | SD | 最大変動幅 | 階段90%遅れ | ランプ遅れ | +|---|---:|---:|---:|---:| +| 生Yaw | 6.27° | 49.6° | 0 | 0 | +| ema 0.4（従来） | 3.77° | 28.1° | 0.22秒 | 0.06秒 | +| median5 + ema 0.4 | 1.92° | 17.1° | 0.34秒 | 0.17秒 | +| median5 + one_euro(1.0, β=0.02) | 1.96° | 20.0° | 0.22秒 | 0.11秒 | +| hampel w7 k3 + ema 0.4 | 2.17° | 21.9° | 0.39秒 | - | + +- 遅れは、サンプル間隔0.056秒で、0→30°のステップ入力が27°に達するまでの時間（ランプは60°/sで増加する入力に対する遅れ）。 +- 1か所・1距離・1人の静止データによる比較。実際の振り向き中の挙動は未検証。 +- 現行（ema 0.4）の出力は、静止していても3σ=11.3°、最大変動幅28°あり、`subtle`(10°)・`weak`(20°)のしきい値に達しうる。 + +## 分かったこと + +- 生Yawの分散の大部分は、フレームごとの独立なノイズと、単発の+側スパイク（約5%）による。 + 中央値系フィルタ（median / Hampel）が単発スパイクに有効。 +- ROI・bboxのフレーム間ゆらぎは0.3〜0.5px程度で、原因ではない。 +- 再投影誤差はスパイクと相関せず（外れ値フレーム1.59px、その他1.58px）、除外基準には使えない。 +- 顔が小さい（3mで目尻間距離が約24px）ため、ランドマーク0.5px程度の誤差で、Yawの標準偏差は4〜5°程度になる + （シミュレーションで確認。実測の6.3°と同水準）。ROIを拡大しても元画素数は増えない。 + → 精度を上げるには、距離を縮める・解像度を上げる・望遠にするなど、顔を大きく映す対策が最も効く。 +- `useExtrinsicGuess`（前フレームの解を初期値にする）はシミュレーション上で効果なし。実装していない。 +- 焦点距離の仮定（現行はフレーム幅=1280px、実際は約800px相当と推定）は、Yawへの影響が1°未満。 + +## 注意点（要確認） + +- **Yawの符号が反転している可能性が高い。** `src/head_pose.py` の `MODEL_POINTS` はYを下向きに直した一方で + zの符号は据え置かれ、目・口が鼻先より手前にある（奥行きが反転した）顔になっている。 + シミュレーションでは、実際の頭部回転に対して推定Yawの符号が反転した（真値+15°→推定-15°、大きさは保たれる）。 + `turn_detection.signage_yaw_direction` が実測に合わせて設定されていれば結果は一貫するが、 + 「正のYaw＝物理的にどちら向きか」を前提にした解釈・報告では注意が必要。 + モデルを直す場合は符号が変わるので、`signage_yaw_direction` と過去ログの扱いも同時に見直すこと。 +- 正面を向いた静止時の平均が約-19.3°だった。立ち位置は画像中心から約31px（約1.4°）しか離れておらず、 + 位置では説明できない。カメラのレンズでなくプレビュー画面を見ていた可能性などを、レンズに目印を付けて再測定して確認する。 +- 一部の設定値（`median_one_euro` の β など）は、この静止データのノイズ規模に合わせたもの。距離が変わったら再評価する。 diff --git a/src/config_loader.py b/src/config_loader.py index 4d663d7..d3ca98a 100644 --- a/src/config_loader.py +++ b/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, diff --git a/src/face_processor.py b/src/face_processor.py index a8e1dbe..94ccf78 100644 --- a/src/face_processor.py +++ b/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: diff --git a/src/yaw_filters.py b/src/yaw_filters.py new file mode 100644 index 0000000..e31e2af --- /dev/null +++ b/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 diff --git a/src/yaw_noise_report.py b/src/yaw_noise_report.py index 2e0c7fc..e27b0a4 100644 --- a/src/yaw_noise_report.py +++ b/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 diff --git a/tests/test_head_pose_smoothing.py b/tests/test_head_pose_smoothing.py new file mode 100644 index 0000000..e1eb5de --- /dev/null +++ b/tests/test_head_pose_smoothing.py @@ -0,0 +1,104 @@ +from pathlib import Path +from typing import Any + +import pytest + +from src.config_loader import HeadPoseSettings, _make, load_config +from src.exceptions import ConfigurationError +from src.face_processor import FaceProcessor +from src.yaw_filters import FILTER_KINDS + +DEFAULT = Path(__file__).parents[1] / "config" / "default.yaml" + + +def build(head_pose: dict[str, Any] | None = None): + overrides: dict[str, Any] = {"face": {"enabled": False}} # no MediaPipe needed + if head_pose is not None: + overrides["head_pose"] = head_pose + return load_config(DEFAULT, None, overrides) + + +def processor(head_pose: dict[str, Any] | None = None) -> FaceProcessor: + config = build(head_pose) + return FaceProcessor(config.face, config.head_pose) + + +def test_defaults_reproduce_the_original_ema() -> None: + settings = build().head_pose + assert settings.smoothing_method == "ema" and settings.smoothing_enabled + assert settings.filter_reset_gap_sec == 0.0 + proc = processor() + first = proc._smooth(1, 0.0, 10.0, 1.0, 0.0) + second = proc._smooth(1, 10.0, 20.0, 3.0, 0.05) + assert first == pytest.approx((0.0, 10.0, 1.0)) + alpha = settings.ema_alpha + assert second == pytest.approx((alpha * 10.0, alpha * 20.0 + (1 - alpha) * 10.0, alpha * 3.0 + (1 - alpha) * 1.0)) + + +def test_smoothing_disabled_passes_values_through() -> None: + proc = processor({"smoothing_enabled": False}) + assert proc._smooth(1, 1.0, 2.0, 3.0, 0.0) == (1.0, 2.0, 3.0) + assert proc._smooth(1, 9.0, 8.0, 7.0, 0.1) == (9.0, 8.0, 7.0) + + +@pytest.mark.parametrize("kind", FILTER_KINDS) +def test_every_filter_kind_loads_and_runs(kind: str) -> None: + proc = processor({"smoothing_method": kind}) + for index in range(10): + pitch, yaw, roll = proc._smooth(1, 1.0, -20.0, 0.5, index * 0.05) + assert (pitch, yaw, roll) == pytest.approx((1.0, -20.0, 0.5)) + + +@pytest.mark.parametrize("kind", ["median_ema", "hampel_ema", "median_one_euro"]) +def test_spike_is_suppressed_by_robust_filters(kind: str) -> None: + proc = processor({"smoothing_method": kind, "smoothing_window": 5}) + outputs = [proc._smooth(1, 0.0, 25.0 if index == 10 else -20.0, 0.0, index * 0.056)[1] for index in range(21)] + assert max(abs(value + 20.0) for value in outputs) < 1.0 + legacy = processor() + plain = [legacy._smooth(1, 0.0, 25.0 if index == 10 else -20.0, 0.0, index * 0.056)[1] for index in range(21)] + assert max(abs(value + 20.0) for value in plain) > 10.0 + + +def test_tracks_are_independent_and_reset_clears_state() -> None: + proc = processor() + proc._smooth(1, 0.0, 0.0, 0.0, 0.0) + assert proc._smooth(2, 0.0, 30.0, 0.0, 0.0)[1] == 30.0 # track 2 starts fresh + proc.reset() + assert proc._smooth(1, 0.0, 30.0, 0.0, 1.0)[1] == 30.0 + + +def test_gap_reset_drops_stale_state() -> None: + proc = processor({"filter_reset_gap_sec": 0.5}) + proc._smooth(1, 0.0, 0.0, 0.0, 0.0) + assert proc._smooth(1, 0.0, 40.0, 0.0, 3.0)[1] == 40.0 + no_reset = processor() + no_reset._smooth(1, 0.0, 0.0, 0.0, 0.0) + assert no_reset._smooth(1, 0.0, 40.0, 0.0, 3.0)[1] == pytest.approx(16.0) + + +def test_new_keys_are_optional_in_yaml_sections() -> None: + legacy_section = {"enabled": True, "method": "solvepnp", "smoothing_enabled": True, + "smoothing_method": "ema", "ema_alpha": 0.4, "max_abs_angle": 120.0} + settings = _make(HeadPoseSettings, legacy_section, "head_pose") + assert settings.smoothing_window == 5 and settings.one_euro_beta == 0.0 + with pytest.raises(ConfigurationError): + _make(HeadPoseSettings, {**legacy_section, "not_a_setting": 1}, "head_pose") + with pytest.raises(ConfigurationError): + _make(HeadPoseSettings, {key: value for key, value in legacy_section.items() if key != "method"}, "head_pose") + + +@pytest.mark.parametrize("override", [ + {"smoothing_method": "kalman"}, + {"smoothing_window": 0}, + {"smoothing_window": 2.5}, + {"smoothing_method": "hampel_ema", "smoothing_window": 2}, + {"hampel_threshold": 0}, + {"one_euro_min_cutoff": 0}, + {"one_euro_d_cutoff": -1}, + {"one_euro_beta": -0.1}, + {"filter_reset_gap_sec": -1}, + {"smoothing_window": True}, +]) +def test_invalid_filter_settings_are_rejected(override: dict[str, Any]) -> None: + with pytest.raises(ConfigurationError): + build(override) diff --git a/tests/test_yaw_filters.py b/tests/test_yaw_filters.py new file mode 100644 index 0000000..e258aab --- /dev/null +++ b/tests/test_yaw_filters.py @@ -0,0 +1,79 @@ +import statistics + +import pytest + +from src.yaw_filters import FILTER_KINDS, create_angle_filter + + +def _run(kind: str, values: list[float], dt: float = 0.056, **params: float) -> list[float]: + f = create_angle_filter(kind, **params) + return [f.update(v, i * dt) for i, v in enumerate(values)] + + +def test_none_and_ema_basics() -> None: + assert _run("none", [1.0, 5.0]) == [1.0, 5.0] + assert _run("ema", [1.0, 5.0], alpha=1.0) == [1.0, 5.0] + out = _run("ema", [0.0, 10.0], alpha=0.4) + assert out == pytest.approx([0.0, 4.0]) + + +@pytest.mark.parametrize("kind", [k for k in FILTER_KINDS]) +def test_constant_signal_is_preserved(kind: str) -> None: + out = _run(kind, [7.0] * 20) + assert out == pytest.approx([7.0] * 20) + + +@pytest.mark.parametrize("kind", ["median_ema", "hampel_ema", "median_one_euro"]) +def test_isolated_spike_is_suppressed(kind: str) -> None: + values = [-20.0] * 10 + [25.0] + [-20.0] * 10 + out = _run(kind, values, alpha=0.5, window=5) + assert max(abs(v + 20.0) for v in out) < 1.0 + plain = _run("ema", values, alpha=0.5) + assert max(abs(v + 20.0) for v in plain) > 10.0 + + +@pytest.mark.parametrize("kind", ["median_ema", "hampel_ema", "one_euro", "median_one_euro"]) +def test_step_is_followed(kind: str) -> None: + out = _run(kind, [0.0] * 20 + [30.0] * 60, alpha=0.4, window=5, one_euro_min_cutoff=1.0, one_euro_beta=0.0) + assert out[19] == pytest.approx(0.0, abs=1e-6) + assert out[-1] == pytest.approx(30.0, abs=0.5) + assert out[20] < 30.0 # causal: cannot jump immediately + + +def test_hampel_passes_small_noise_unchanged_before_ema() -> None: + f = create_angle_filter("hampel_ema", alpha=1.0, window=5, hampel_threshold=3.0) + values = [0.0, 0.5, -0.5, 0.4, -0.4, 0.3] + assert [f.update(v, i * 0.05) for i, v in enumerate(values)] == pytest.approx(values) + + +def test_noise_reduction_on_alternating_input() -> None: + noisy = [0.0, 6.0] * 40 + raw_sd = statistics.pstdev(noisy) + for kind in ("ema", "median_ema", "hampel_ema", "one_euro", "median_one_euro"): + out = _run(kind, noisy, alpha=0.3, window=5) + assert statistics.pstdev(out[10:]) < raw_sd + + +def test_gap_resets_state() -> None: + f = create_angle_filter("ema", alpha=0.1, reset_gap_sec=0.5) + f.update(0.0, 0.0) + f.update(0.0, 0.05) + assert f.update(40.0, 2.0) == 40.0 # gap > 0.5 s -> first sample passes through + g = create_angle_filter("ema", alpha=0.1, reset_gap_sec=0.0) + g.update(0.0, 0.0) + assert g.update(40.0, 2.0) == pytest.approx(4.0) + + +def test_reset_and_validation() -> None: + f = create_angle_filter("median_ema", alpha=0.5, window=3, reset_gap_sec=1.0) + f.update(10.0, 0.0) + f.reset() + assert f.update(0.0, 0.1) == 0.0 + with pytest.raises(ValueError): + create_angle_filter("kalman") + with pytest.raises(ValueError): + create_angle_filter("ema", alpha=0.0) + with pytest.raises(ValueError): + create_angle_filter("hampel_ema", window=2) + with pytest.raises(ValueError): + create_angle_filter("one_euro", one_euro_min_cutoff=0.0)