静止時のyaw値変動に対して平滑化を行うための計測基盤を実装

src/face_logger.py

@@ -17,7 +17,9 @@ class FaceLogger:
 
     FRAME_FIELDS = ["experiment_id", "frame_number", "timestamp", "elapsed_time_sec", "track_id", "face_detected",
                     "pose_estimated", "pitch", "yaw", "roll", "landmarks_count", "roi_x1", "roi_y1", "roi_x2",
-                    "roi_y2", "roi_width", "roi_height", "confidence", "failure_reason"]
+                    "roi_y2", "roi_width", "roi_height", "confidence", "failure_reason",
+                    "raw_pitch", "raw_yaw", "raw_roll", "reprojection_error", "interocular_px",
+                    "bbox_center_x", "bbox_center_y"]
     SUMMARY_FIELDS = ["experiment_id", "track_id", "total_frames", "face_detected_frames", "pose_estimated_frames",
                       "face_detection_rate", "pose_estimation_rate", "min_yaw", "max_yaw", "mean_yaw", "last_pitch",
                       "last_yaw", "last_roll", "valid_face_track", "invalid_reason"]
@@ -68,6 +70,12 @@ class FaceLogger:
                 "roi_x2": roi.x2 if roi else "", "roi_y2": roi.y2 if roi else "",
                 "roi_width": roi.width if roi else "", "roi_height": roi.height if roi else "",
                 "confidence": self._number(result.confidence), "failure_reason": result.failure_reason,
+                "raw_pitch": self._number(result.raw_pitch), "raw_yaw": self._number(result.raw_yaw),
+                "raw_roll": self._number(result.raw_roll),
+                "reprojection_error": self._number(result.reprojection_error),
+                "interocular_px": self._number(result.interocular_px),
+                "bbox_center_x": roi.source_bbox.center_x if roi else "",
+                "bbox_center_y": roi.source_bbox.center_y if roi else "",
             })
         self._frames_since_flush += 1
         if self._frames_since_flush >= self.config.flush_interval_frames:

src/face_processor.py

@@ -3,6 +3,7 @@
 from __future__ import annotations
 
 import logging
+import math
 from collections import deque
 from typing import Any
 
@@ -86,18 +87,32 @@ class FaceProcessor:
             LOGGER.exception("MediaPipe failed for track %d on frame %d", person.track_id, info.frame_number)
             return self._failure(person.track_id, info, "mediapipe_error", roi)
         estimation = self.estimator.estimate(landmarks, width, height)
+        interocular = self._interocular_px(landmarks)
         if not estimation.success:
             return HeadPoseResult(
                 person.track_id, info.frame_number, info.timestamp_iso, info.elapsed_time_sec,
                 True, False, estimation.pitch, estimation.yaw, estimation.roll, len(landmarks), roi, roi.confidence,
                 estimation.failure_reason, tuple(landmarks),
+                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)
         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),
+            raw_pitch=estimation.pitch, raw_yaw=estimation.yaw, raw_roll=estimation.roll,
+            reprojection_error=estimation.reprojection_error, interocular_px=interocular,
         )
 
+    @staticmethod
+    def _interocular_px(landmarks: list[FaceLandmark]) -> float | None:
+        """Distance between the outer eye corners (MediaPipe 33/263) in frame pixels."""
+        by_index = {landmark.index: landmark for landmark in landmarks}
+        left, right = by_index.get(33), by_index.get(263)
+        if left is None or right is None:
+            return None
+        return float(math.hypot(right.x - left.x, right.y - left.y))
+
     @staticmethod
     def _face_center_distance(face: Any) -> float:
         landmarks = face.landmark

src/models.py

@@ -281,6 +281,13 @@ class HeadPoseResult:
     confidence: float | None
     failure_reason: str = ""
     landmarks: tuple[FaceLandmark, ...] = field(default_factory=tuple, repr=False)
+    # Diagnostics for yaw-jitter analysis: pre-smoothing angles, PnP reprojection
+    # error [px], and outer-eye-corner distance [px] as a face-size indicator.
+    raw_pitch: float | None = None
+    raw_yaw: float | None = None
+    raw_roll: float | None = None
+    reprojection_error: float | None = None
+    interocular_px: float | None = None
 
     def __post_init__(self) -> None:
         if self.track_id < 0 or self.frame_number < 0 or self.landmarks_count < 0:

src/yaw_noise_report.py

@@ -0,0 +1,238 @@
+"""Offline yaw-jitter report for stationary-subject ``*_face_frames.csv`` logs.
+
+Usage (record 30-60 s with the subject facing the camera and not moving)::
+
+    python -m src.yaw_noise_report data/logs/face/<stamp>_<id>_face_frames.csv --track-id 1 --t-start 3 --t-end 40
+
+The report compares the logged (pipeline-smoothed) yaw with the raw yaw and with
+several filters applied offline to the raw yaw, so filter candidates can be
+evaluated without touching the live pipeline. Only noise is measured here; filter
+lag during real turns must be checked separately against the FPS and
+``turn_detection.min_turn_duration_sec``.
+"""
+
+from __future__ import annotations
+
+import argparse
+import csv
+import math
+import statistics
+import sys
+from pathlib import Path
+from typing import Callable, Sequence
+
+Sample = tuple[float, float]  # (elapsed_time_sec, yaw_deg)
+
+
+def _to_float(value: str | None) -> float | None:
+    if value is None or value == "":
+        return None
+    try:
+        number = float(value)
+    except ValueError:
+        return None
+    return number if math.isfinite(number) else None
+
+
+def percentile(sorted_values: Sequence[float], q: float) -> float:
+    """Linear-interpolated percentile, q in [0, 100]."""
+    if not sorted_values:
+        raise ValueError("percentile requires at least one value")
+    position = (len(sorted_values) - 1) * q / 100.0
+    lower, upper = math.floor(position), math.ceil(position)
+    if lower == upper:
+        return float(sorted_values[lower])
+    weight = position - lower
+    return float(sorted_values[lower] * (1.0 - weight) + sorted_values[upper] * weight)
+
+
+def summarize(values: Sequence[float]) -> dict[str, float] | None:
+    """Noise statistics of a yaw series; None when fewer than two samples."""
+    if len(values) < 2:
+        return None
+    ordered = sorted(values)
+    diffs = [b - a for a, b in zip(values, values[1:])]
+    jitter = statistics.stdev(diffs) / math.sqrt(2.0) if len(diffs) >= 2 else float("nan")
+    return {
+        "n": float(len(values)),
+        "mean": statistics.fmean(values),
+        "sd": statistics.stdev(values),
+        "range95": percentile(ordered, 97.5) - percentile(ordered, 2.5),
+        "p2p": ordered[-1] - ordered[0],
+        "jitter": jitter,
+    }
+
+
+def ema(values: Sequence[float], alpha: float) -> list[float]:
+    """Per-sample EMA, identical in form to FaceProcessor._smooth."""
+    output: list[float] = []
+    for value in values:
+        output.append(value if not output else alpha * value + (1.0 - alpha) * output[-1])
+    return output
+
+
+def median_then_ema(values: Sequence[float], alpha: float, window: int = 3) -> list[float]:
+    medians = [statistics.median(values[max(0, i - window + 1):i + 1]) for i in range(len(values))]
+    return ema(medians, alpha)
+
+
+def one_euro(samples: Sequence[Sample], min_cutoff: float, beta: float, d_cutoff: float = 1.0) -> list[float]:
+    """One Euro Filter (Casiez et al. 2012) using real timestamps."""
+    def smoothing_factor(dt: float, cutoff: float) -> float:
+        tau = 1.0 / (2.0 * math.pi * cutoff)
+        return 1.0 / (1.0 + tau / dt)
+
+    output: list[float] = []
+    previous_t = 0.0
+    previous_dx = 0.0
+    for t, x in samples:
+        if not output:
+            output.append(x)
+            previous_t = t
+            continue
+        dt = max(t - previous_t, 1e-6)
+        previous_t = t
+        dx = (x - output[-1]) / dt
+        a_d = smoothing_factor(dt, d_cutoff)
+        dx_hat = a_d * dx + (1.0 - a_d) * previous_dx
+        previous_dx = dx_hat
+        cutoff = min_cutoff + beta * abs(dx_hat)
+        a = smoothing_factor(dt, cutoff)
+        output.append(a * x + (1.0 - a) * output[-1])
+    return output
+
+
+def load_tracks(
+    path: Path, track_id: int | None = None, t_start: float | None = None, t_end: float | None = None,
+) -> dict[int, dict[str, list]]:
+    """Read pose-estimated rows grouped by track.
+
+    t_start/t_end are seconds relative to each track's first pose-estimated row.
+    Rows without ``raw_yaw`` (older logs) contribute to ``logged`` only.
+    """
+    tracks: dict[int, dict[str, list]] = {}
+    with path.open(encoding="utf-8-sig", newline="") as stream:
+        for row in csv.DictReader(stream):
+            if str(row.get("pose_estimated", "")).lower() != "true":
+                continue
+            tid = int(row["track_id"])
+            if track_id is not None and tid != track_id:
+                continue
+            t, yaw = _to_float(row.get("elapsed_time_sec")), _to_float(row.get("yaw"))
+            if t is None or yaw is None:
+                continue
+            entry = tracks.setdefault(tid, {"logged": [], "raw": [], "interocular": [], "reproj": []})
+            entry["logged"].append((t, yaw))
+            raw = _to_float(row.get("raw_yaw"))
+            if raw is not None:
+                entry["raw"].append((t, raw))
+            for key, column in (("interocular", "interocular_px"), ("reproj", "reprojection_error")):
+                value = _to_float(row.get(column))
+                if value is not None:
+                    entry[key].append((t, value))
+    for entry in tracks.values():
+        t0 = entry["logged"][0][0] if entry["logged"] else 0.0
+        for key, series in entry.items():
+            entry[key] = [
+                item for item in series
+                if (t_start is None or item[0] - t0 >= t_start) and (t_end is None or item[0] - t0 <= t_end)
+            ]
+    return tracks
+
+
+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]]] = []
+
+    def add(name: str, values: Sequence[float]) -> None:
+        stats = summarize(values)
+        if stats is not None:
+            rows.append((name, stats))
+
+    add("logged (pipeline output)", [yaw for _, yaw in entry["logged"]])
+    raw: list[Sample] = entry["raw"]
+    if len(raw) >= 2:
+        values = [yaw for _, yaw in raw]
+        add("raw", values)
+        for alpha in (0.4, 0.2, 0.1):
+            add(f"raw + EMA a={alpha}", ema(values, alpha))
+        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))
+    return rows
+
+
+def _mean(series: Sequence[Sample]) -> float | None:
+    return statistics.fmean(value for _, value in series) if series else None
+
+
+def duration_and_rate(entry: dict[str, list]) -> tuple[float, float | None]:
+    series = entry["logged"]
+    if len(series) < 2:
+        return 0.0, None
+    duration = series[-1][0] - series[0][0]
+    return duration, (len(series) - 1) / duration if duration > 0 else None
+
+
+def format_report(tracks: dict[int, dict[str, list]]) -> str:
+    lines: list[str] = []
+    for tid in sorted(tracks):
+        entry = tracks[tid]
+        duration, rate = duration_and_rate(entry)
+        lines.append(f"## track {tid}")
+        rate_text = f"{rate:.1f} Hz" if rate else "n/a"
+        lines.append(f"- duration {duration:.1f} s, pose update rate {rate_text}")
+        eye, reproj = _mean(entry["interocular"]), _mean(entry["reproj"])
+        if eye is not None:
+            lines.append(f"- mean outer-eye-corner distance {eye:.1f} px")
+        if reproj is not None:
+            lines.append(f"- mean reprojection error {reproj:.2f} px")
+        if not entry["raw"]:
+            lines.append("- raw_yaw column absent: only the logged (smoothed) yaw can be evaluated")
+        lines.append("")
+        lines.append("| signal | n | mean | SD | 3*SD | 95% range | peak-to-peak | frame jitter |")
+        lines.append("|---|---:|---:|---:|---:|---:|---:|---:|")
+        for name, s in build_rows(entry):
+            lines.append(
+                f"| {name} | {int(s['n'])} | {s['mean']:.2f} | {s['sd']:.2f} | {3 * s['sd']:.2f} | "
+                f"{s['range95']:.2f} | {s['p2p']:.2f} | {s['jitter']:.2f} |"
+            )
+        lines.append("")
+    lines.append("Units: degrees. 'frame jitter' = SD of consecutive differences / sqrt(2), insensitive to slow drift.")
+    lines.append("If the subject moved, SD includes real motion; trim with --t-start/--t-end.")
+    return "\n".join(lines)
+
+
+def write_csv(tracks: dict[int, dict[str, list]], path: Path) -> None:
+    with path.open("w", encoding="utf-8-sig", newline="") as stream:
+        writer = csv.writer(stream)
+        writer.writerow(["track_id", "signal", "n", "mean", "sd", "three_sd", "range95", "peak_to_peak", "frame_jitter"])
+        for tid in sorted(tracks):
+            for name, s in build_rows(tracks[tid]):
+                writer.writerow([tid, name, int(s["n"]), f"{s['mean']:.4f}", f"{s['sd']:.4f}", f"{3 * s['sd']:.4f}",
+                                 f"{s['range95']:.4f}", f"{s['p2p']:.4f}", f"{s['jitter']:.4f}"])
+
+
+def main(argv: Sequence[str] | None = None, out: Callable[[str], object] = print) -> int:
+    parser = argparse.ArgumentParser(description="Yaw jitter report from a stationary face_frames.csv")
+    parser.add_argument("csv_path", type=Path)
+    parser.add_argument("--track-id", type=int, default=None)
+    parser.add_argument("--t-start", type=float, default=None, help="seconds after the track's first pose row")
+    parser.add_argument("--t-end", type=float, default=None, help="seconds after the track's first pose row")
+    parser.add_argument("--csv", type=Path, default=None, help="optional output CSV of the statistics table")
+    args = parser.parse_args(argv)
+    if not args.csv_path.is_file():
+        print(f"file not found: {args.csv_path}", file=sys.stderr)
+        return 2
+    tracks = load_tracks(args.csv_path, args.track_id, args.t_start, args.t_end)
+    if not tracks:
+        print("no pose-estimated rows found for the given filters", file=sys.stderr)
+        return 1
+    out(format_report(tracks))
+    if args.csv is not None:
+        write_csv(tracks, args.csv)
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tests/test_yaw_noise_report.py

@@ -0,0 +1,80 @@
+import csv
+from pathlib import Path
+
+import pytest
+
+from src.config_loader import FaceLoggingSettings
+from src.face_logger import FaceLogger
+from src.models import BoundingBox, FaceROI, HeadPoseResult
+from src.yaw_noise_report import (
+    ema, load_tracks, main, median_then_ema, one_euro, percentile, summarize,
+)
+
+
+def test_percentile_interpolates() -> None:
+    assert percentile([0.0, 10.0], 50) == pytest.approx(5.0)
+    assert percentile([1.0, 2.0, 3.0], 0) == 1.0
+    assert percentile([1.0, 2.0, 3.0], 100) == 3.0
+
+
+def test_summarize_constant_and_alternating_series() -> None:
+    constant = summarize([2.0] * 5)
+    assert constant is not None and constant["sd"] == 0.0 and constant["p2p"] == 0.0
+    alternating = summarize([0.0, 2.0, 0.0, 2.0])
+    assert alternating is not None
+    assert alternating["p2p"] == 2.0 and alternating["mean"] == 1.0
+    assert summarize([1.0]) is None
+
+
+def test_filters_preserve_length_and_constant_signal() -> None:
+    values = [3.0] * 6
+    samples = [(i * 0.05, 3.0) for i in range(6)]
+    assert ema(values, 0.4) == pytest.approx(values)
+    assert median_then_ema(values, 0.4) == pytest.approx(values)
+    assert one_euro(samples, 1.0, 0.0) == pytest.approx(values)
+    assert ema([1.0, 5.0], 1.0) == [1.0, 5.0]
+
+
+def test_ema_reduces_alternating_noise() -> None:
+    noisy = [0.0, 4.0] * 20
+    raw_sd = summarize(noisy)["sd"]  # type: ignore[index]
+    assert summarize(ema(noisy, 0.2))["sd"] < raw_sd  # type: ignore[index]
+
+
+def _result(frame: int, t: float, yaw: float, raw: float | None) -> HeadPoseResult:
+    roi = FaceROI(1, 10, 10, 110, 110, BoundingBox(0, 0, 200, 400), .9)
+    return HeadPoseResult(1, frame, "now", t, True, True, 0.0, yaw, 0.0, 468, roi, .9,
+                          raw_yaw=raw, reprojection_error=1.5, interocular_px=60.0)
+
+
+def test_logger_writes_diagnostic_columns_and_report_reads_them(tmp_path: Path, capsys: pytest.CaptureFixture[str]) -> None:
+    logger = FaceLogger(FaceLoggingSettings(True, True, True, 1, str(tmp_path)), "exp", "20260101_000000")
+    logger.open()
+    raws = [0.0, 2.0, -2.0, 1.0, -1.0, 0.5]
+    logger.write_results([_result(i + 1, i * 0.1, r * 0.5, r) for i, r in enumerate(raws)])
+    logger.close()
+    path = next(tmp_path.glob("*face_frames.csv"))
+    with path.open(encoding="utf-8-sig", newline="") as stream:
+        rows = list(csv.DictReader(stream))
+    assert rows[1]["raw_yaw"] == "2.000000" and rows[1]["interocular_px"] == "60.000000"
+    assert rows[0]["bbox_center_x"] == "100" and rows[0]["reprojection_error"] == "1.500000"
+
+    tracks = load_tracks(path)
+    assert len(tracks[1]["raw"]) == 6
+    trimmed = load_tracks(path, track_id=1, t_start=0.2)
+    assert len(trimmed[1]["raw"]) == 4
+    assert main([str(path), "--track-id", "1", "--csv", str(tmp_path / "out.csv")]) == 0
+    assert "raw + OneEuro" in capsys.readouterr().out
+    assert (tmp_path / "out.csv").is_file()
+
+
+def test_report_handles_old_logs_without_raw_columns(tmp_path: Path) -> None:
+    path = tmp_path / "old_face_frames.csv"
+    path.write_text(
+        "track_id,pose_estimated,elapsed_time_sec,yaw\n1,True,0.0,1.0\n1,True,0.1,2.0\n1,False,0.2,\n",
+        encoding="utf-8-sig",
+    )
+    tracks = load_tracks(path)
+    assert len(tracks[1]["logged"]) == 2 and tracks[1]["raw"] == []
+    assert main([str(path)]) == 0
+    assert main([str(tmp_path / "missing.csv")]) == 2