From 40591a15a06ab50c2eba13534c6844ef1fe7e900 Mon Sep 17 00:00:00 2001 From: KoyoNoguchi Date: Mon, 5 Oct 2026 21:30:13 +0900 Subject: [PATCH] =?UTF-8?q?=E6=96=B9=E4=BD=8D=E8=A7=92=E3=81=A7=E8=A3=9C?= =?UTF-8?q?=E6=AD=A3=E3=81=97=EF=BC=8C=E6=8C=AF=E3=82=8A=E5=90=91=E3=81=8B?= =?UTF-8?q?=E3=81=9A=E6=AD=A9=E3=81=84=E3=81=9F=E5=A0=B4=E5=90=88=E3=81=AB?= =?UTF-8?q?=E6=8C=AF=E3=82=8A=E5=90=91=E3=81=8D=E5=88=A4=E5=AE=9A=E3=81=95?= =?UTF-8?q?=E3=82=8C=E3=81=AA=E3=81=84=E3=82=88=E3=81=86=E3=81=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config/default.yaml | 5 +++++ src/config_loader.py | 9 +++++++++ src/models.py | 2 ++ src/turn_detector.py | 36 +++++++++++++++++++++++++++++------- src/turn_logger.py | 7 +++++-- 5 files changed, 50 insertions(+), 9 deletions(-) diff --git a/config/default.yaml b/config/default.yaml index 8f81f2a..a7af9c9 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -112,6 +112,11 @@ turn_detection: enabled: true signage_yaw_direction: positive baseline_strategy: first_valid + # Camera-relative position compensation for side-crossing pedestrians. + # corrected_yaw = raw_yaw - bearing_correction_sign * bearing_deg + bearing_correction_enabled: true + horizontal_fov_deg: 70.0 + bearing_correction_sign: 1.0 baseline_window_sec: 0.8 baseline_min_samples: 3 subtle_threshold_deg: 10.0 diff --git a/src/config_loader.py b/src/config_loader.py index b3fbf1b..09178cf 100644 --- a/src/config_loader.py +++ b/src/config_loader.py @@ -236,6 +236,9 @@ class TurnDetectionSettings: enabled: bool signage_yaw_direction: str baseline_strategy: str + bearing_correction_enabled: bool + horizontal_fov_deg: float + bearing_correction_sign: float baseline_window_sec: float baseline_min_samples: int subtle_threshold_deg: float @@ -637,6 +640,12 @@ def _validate(config: AppConfig) -> None: turn = config.turn_detection if not isinstance(turn.enabled, bool) or turn.signage_yaw_direction not in {"positive", "negative"}: raise ConfigurationError("turn enabled/direction settings are invalid") + if not isinstance(turn.bearing_correction_enabled, bool): + raise ConfigurationError("turn bearing_correction_enabled must be boolean") + if not isinstance(turn.horizontal_fov_deg, (int, float)) or not 1.0 <= turn.horizontal_fov_deg < 179.0: + raise ConfigurationError("turn horizontal_fov_deg must be in [1, 179)") + if turn.bearing_correction_sign not in {-1.0, 1.0, -1, 1}: + raise ConfigurationError("turn bearing_correction_sign must be +1 or -1") if turn.baseline_strategy not in {"first_valid", "before_trigger", "region_entry_window"}: raise ConfigurationError("invalid turn baseline_strategy") if turn.baseline_window_sec <= 0 or turn.baseline_min_samples <= 0: diff --git a/src/models.py b/src/models.py index 45066b0..91fe844 100644 --- a/src/models.py +++ b/src/models.py @@ -358,6 +358,8 @@ class TurnFrameResult: turn_level: str continuous_duration_sec: float failure_reason: str = "" + bearing_deg: float | None = None + corrected_yaw: float | None = None def __post_init__(self) -> None: if self.track_id < 0 or self.frame_number < 0 or self.continuous_duration_sec < 0: diff --git a/src/turn_detector.py b/src/turn_detector.py index 8c1d66c..c7b2968 100644 --- a/src/turn_detector.py +++ b/src/turn_detector.py @@ -2,6 +2,7 @@ from __future__ import annotations +import math import statistics from collections import deque @@ -55,6 +56,8 @@ class TurnDetector: return self._frame(state, pose, None, None, 0.0, pose.failure_reason or "pose_not_estimated"), events yaw = pose.yaw + bearing_deg = self._estimate_bearing_deg(track, info) + corrected_yaw = self._correct_yaw_for_bearing(yaw, bearing_deg) if (self.config.face_appearance_enabled and state.missing_started_elapsed is not None and info.elapsed_time_sec - state.missing_started_elapsed >= self.config.face_missing_before_appearance_sec and not state.face_appeared_after_missing): @@ -62,12 +65,12 @@ class TurnDetector: events.append(self._event("face_appeared", state, info, yaw, None, TurnLevel.FACE_APPEARED.value)) state.missing_started_elapsed = None state.last_pose_elapsed = info.elapsed_time_sec - state.yaw_history.append((info.elapsed_time_sec, yaw)) - self._collect_baseline(state, pose, info, track, events) + state.yaw_history.append((info.elapsed_time_sec, corrected_yaw)) + self._collect_baseline(state, pose, info, track, events, corrected_yaw) if not state.baseline_acquired or state.baseline_yaw is None: - return self._frame(state, pose, None, None, 0.0, "baseline_not_acquired"), events + return self._frame(state, pose, None, None, 0.0, "baseline_not_acquired", bearing_deg, corrected_yaw), events - raw, toward = yaw_delta_toward_signage(yaw, state.baseline_yaw, self.config.signage_yaw_direction) + raw, toward = yaw_delta_toward_signage(corrected_yaw, state.baseline_yaw, self.config.signage_yaw_direction) if state.max_yaw_delta_toward_signage is None or toward > state.max_yaw_delta_toward_signage: state.max_yaw_delta_toward_signage = toward state.max_yaw_delta_frame, state.max_yaw_delta_time = info.frame_number, info.timestamp_iso @@ -95,11 +98,11 @@ class TurnDetector: else: events.extend(self._end_candidate(state, info, yaw)) state.turn_level = state.strongest_level if state.strongest_level != TurnLevel.NONE.value else level - return self._frame(state, pose, raw, toward, duration, ""), events + return self._frame(state, pose, raw, toward, duration, "", bearing_deg, corrected_yaw), events def _collect_baseline( self, state: TurnTrackState, pose: HeadPoseResult, info: FrameInfo, - track: TrackState | None, events: list[TurnEvent], + track: TrackState | None, events: list[TurnEvent], corrected_yaw: float, ) -> None: if state.baseline_acquired or pose.yaw is None: return @@ -111,7 +114,7 @@ class TurnDetector: and 0 <= info.elapsed_time_sec - track.entry_monotonic <= self.config.baseline_window_sec) if not allowed: return - state.baseline_samples.append((info.elapsed_time_sec, pose.yaw, info.frame_number, info.timestamp_iso)) + state.baseline_samples.append((info.elapsed_time_sec, corrected_yaw, info.frame_number, info.timestamp_iso)) cutoff = info.elapsed_time_sec - self.config.baseline_window_sec while state.baseline_samples and state.baseline_samples[0][0] < cutoff: state.baseline_samples.popleft() @@ -133,6 +136,7 @@ class TurnDetector: def _frame( self, state: TurnTrackState, pose: HeadPoseResult, raw: float | None, toward: float | None, duration: float, reason: str, + bearing_deg: float | None = None, corrected_yaw: float | None = None, ) -> TurnFrameResult: level = state.turn_level if not pose.pose_estimated: @@ -144,8 +148,26 @@ class TurnDetector: pose.face_detected, pose.pose_estimated, state.baseline_acquired, state.baseline_yaw, pose.yaw, raw, toward, self.config.signage_yaw_direction, state.candidate_start_elapsed is not None, state.turn_confirmed, level, duration, reason, + bearing_deg, corrected_yaw, ) + def _estimate_bearing_deg(self, track: TrackState | None, info: FrameInfo) -> float | None: + """Estimate the person's camera-relative bearing from tracked x position.""" + if not self.config.bearing_correction_enabled or track is None or info.width <= 1: + return 0.0 if self.config.bearing_correction_enabled and track is None else None + if not track.trajectory: + return None + center_x = track.trajectory[-1].center_x + x_norm = max(0.0, min(1.0, center_x / float(info.width - 1))) + half_fov_rad = math.radians(self.config.horizontal_fov_deg) / 2.0 + phi_rad = math.atan((x_norm - 0.5) * 2.0 * math.tan(half_fov_rad)) + return math.degrees(phi_rad) + + def _correct_yaw_for_bearing(self, yaw: float, bearing_deg: float | None) -> float: + if not self.config.bearing_correction_enabled or bearing_deg is None: + return yaw + return yaw - self.config.bearing_correction_sign * bearing_deg + def finalize_track(self, track_id: int, frame_info: FrameInfo) -> tuple[TurnTrackState | None, list[TurnEvent]]: state = self._states.pop(track_id, None) if state is None: diff --git a/src/turn_logger.py b/src/turn_logger.py index e67c37e..4884e2e 100644 --- a/src/turn_logger.py +++ b/src/turn_logger.py @@ -18,7 +18,8 @@ class TurnLogger: FRAME_FIELDS = ["experiment_id", "frame_number", "timestamp", "elapsed_time_sec", "track_id", "face_detected", "pose_estimated", "baseline_available", "baseline_yaw", "current_yaw", "yaw_delta_raw", - "yaw_delta_toward_signage", "signage_yaw_direction", "turn_candidate", "turn_confirmed", + "yaw_delta_toward_signage", "signage_yaw_direction", "bearing_deg", "corrected_yaw", + "turn_candidate", "turn_confirmed", "turn_level", "continuous_duration_sec", "failure_reason"] EVENT_FIELDS = ["experiment_id", "event_type", "track_id", "frame_number", "timestamp", "elapsed_time_sec", "yaw", "yaw_delta_toward_signage", "turn_level", "metadata_json"] @@ -73,7 +74,9 @@ class TurnLogger: "baseline_available": item.baseline_available, "baseline_yaw": self._number(item.baseline_yaw), "current_yaw": self._number(item.current_yaw), "yaw_delta_raw": self._number(item.yaw_delta_raw), "yaw_delta_toward_signage": self._number(item.yaw_delta_toward_signage), - "signage_yaw_direction": item.signage_direction, "turn_candidate": item.turn_candidate, + "signage_yaw_direction": item.signage_direction, + "bearing_deg": self._number(item.bearing_deg), "corrected_yaw": self._number(item.corrected_yaw), + "turn_candidate": item.turn_candidate, "turn_confirmed": item.turn_confirmed, "turn_level": item.turn_level, "continuous_duration_sec": self._number(item.continuous_duration_sec), "failure_reason": item.failure_reason,