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

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)

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)