feat(v1): add pose quality evidence

2026-07-21 09:57:00 +08:00
parent fca91b6a91
commit 7de360a01f
9 changed files with 301 additions and 15 deletions
@@ -0,0 +1,90 @@
+"""Pose quality and geometric evidence calculations without alarm decisions."""
+
+from dataclasses import dataclass
+from math import acos, degrees, hypot
+from typing import Optional
+
+from v1.pose import PersonPose
+
+
+_REQUIRED_JOINTS = (5, 6, 11, 12, 13, 14, 15, 16)
+
+
+@dataclass(frozen=True)
+class PoseQuality:
+    accepted: bool
+    reason: str
+    visible_joint_count: int
+
+
+@dataclass(frozen=True)
+class PoseEvidence:
+    accepted: bool
+    horizontal_pose: bool
+    rapid_vertical_change: bool
+    horizontal_angle_degrees: Optional[float]
+    hip_center_y: Optional[float]
+    torso_length: Optional[float]
+    reason: str
+
+
+def assess_pose_quality(
+    pose: Optional[PersonPose], threshold: float
+) -> PoseQuality:
+    if not 0.0 <= threshold <= 1.0:
+        raise ValueError("threshold must be between 0 and 1")
+    if pose is None:
+        return PoseQuality(False, "missing_pose", 0)
+    if len(pose.keypoints) != 17:
+        return PoseQuality(False, "invalid_keypoint_count", 0)
+    visible_count = sum(
+        point.confidence >= threshold for point in pose.keypoints
+    )
+    if any(pose.keypoints[index].confidence < threshold for index in _REQUIRED_JOINTS):
+        return PoseQuality(False, "required_joint_low_confidence", visible_count)
+    return PoseQuality(True, "accepted", visible_count)
+
+
+def extract_evidence(
+    pose: Optional[PersonPose],
+    quality: PoseQuality,
+    previous: Optional[PoseEvidence],
+    rapid_drop_torso_ratio: float = 0.5,
+    horizontal_angle_threshold_degrees: float = 35.0,
+) -> PoseEvidence:
+    """Return geometric facts; a later state machine decides whether to alarm."""
+
+    if not quality.accepted or pose is None:
+        return PoseEvidence(False, False, False, None, None, None, quality.reason)
+    if rapid_drop_torso_ratio < 0:
+        raise ValueError("rapid_drop_torso_ratio must be non-negative")
+    shoulder_x, shoulder_y = _midpoint(pose, 5, 6)
+    hip_x, hip_y = _midpoint(pose, 11, 12)
+    vector_x = hip_x - shoulder_x
+    vector_y = hip_y - shoulder_y
+    torso_length = hypot(vector_x, vector_y)
+    if torso_length == 0:
+        return PoseEvidence(False, False, False, None, hip_y, 0.0, "degenerate_torso")
+    cosine_to_horizontal = max(-1.0, min(1.0, abs(vector_x) / torso_length))
+    horizontal_angle = degrees(acos(cosine_to_horizontal))
+    horizontal_pose = horizontal_angle <= horizontal_angle_threshold_degrees
+    rapid_vertical_change = False
+    if previous is not None and previous.accepted and previous.hip_center_y is not None:
+        rapid_vertical_change = (
+            hip_y - previous.hip_center_y >= rapid_drop_torso_ratio * torso_length
+        )
+    return PoseEvidence(
+        accepted=True,
+        horizontal_pose=horizontal_pose,
+        rapid_vertical_change=rapid_vertical_change,
+        horizontal_angle_degrees=horizontal_angle,
+        hip_center_y=hip_y,
+        torso_length=torso_length,
+        reason="accepted",
+    )
+
+
+def _midpoint(pose: PersonPose, first_index: int, second_index: int):
+    first = pose.keypoints[first_index]
+    second = pose.keypoints[second_index]
+    return ((first.x + second.x) / 2.0, (first.y + second.y) / 2.0)
@@ -0,0 +1,76 @@
+from v1.pose import Keypoint, PersonPose
+from v1.evidence import assess_pose_quality, extract_evidence
+from v1.tracking import PersonTracker
+
+
+def _pose(box=(20.0, 20.0, 60.0, 140.0), horizontal=False, missing_ankles=False):
+    points = [Keypoint(float(index), float(index), 0.9) for index in range(17)]
+    if horizontal:
+        points[5] = Keypoint(20.0, 50.0, 0.9)
+        points[6] = Keypoint(30.0, 50.0, 0.9)
+        points[11] = Keypoint(70.0, 53.0, 0.9)
+        points[12] = Keypoint(80.0, 53.0, 0.9)
+    if missing_ankles:
+        points[15] = Keypoint(35.0, 120.0, 0.1)
+        points[16] = Keypoint(45.0, 120.0, 0.1)
+    return PersonPose(box_xyxy=box, box_confidence=0.9, keypoints=tuple(points))
+
+
+def test_tracker_keeps_id_for_nearby_person_in_next_frame():
+    tracker = PersonTracker(max_match_distance_ratio=0.2)
+
+    first = tracker.update([_pose()], detected_at_monotonic=1.0, frame_size=(200, 200))
+    second = tracker.update(
+        [_pose(box=(23.0, 22.0, 63.0, 142.0))],
+        detected_at_monotonic=1.1,
+        frame_size=(200, 200),
+    )
+
+    assert first[0].track_id == second[0].track_id
+
+
+def test_missing_ankles_rejects_pose():
+    quality = assess_pose_quality(_pose(missing_ankles=True), threshold=0.4)
+
+    assert quality.accepted is False
+    assert quality.reason == "required_joint_low_confidence"
+
+
+def test_horizontal_body_is_evidence_not_event():
+    pose = _pose(horizontal=True)
+    quality = assess_pose_quality(pose, threshold=0.4)
+
+    evidence = extract_evidence(pose, quality=quality, previous=None)
+
+    assert evidence.accepted is True
+    assert evidence.horizontal_pose is True
+    assert evidence.rapid_vertical_change is False
+
+
+def test_missing_pose_cannot_create_usable_evidence():
+    quality = assess_pose_quality(None, threshold=0.4)
+
+    evidence = extract_evidence(None, quality=quality, previous=None)
+
+    assert quality.accepted is False
+    assert evidence.accepted is False
+
+
+def test_large_hip_drop_is_normalized_as_rapid_vertical_evidence():
+    previous_pose = _pose()
+    current_points = list(previous_pose.keypoints)
+    current_points[11] = Keypoint(11.0, 100.0, 0.9)
+    current_points[12] = Keypoint(12.0, 100.0, 0.9)
+    current_pose = PersonPose(
+        box_xyxy=previous_pose.box_xyxy,
+        box_confidence=previous_pose.box_confidence,
+        keypoints=tuple(current_points),
+    )
+    previous_quality = assess_pose_quality(previous_pose, threshold=0.4)
+    current_quality = assess_pose_quality(current_pose, threshold=0.4)
+    previous = extract_evidence(previous_pose, quality=previous_quality, previous=None)
+
+    evidence = extract_evidence(current_pose, quality=current_quality, previous=previous)
+
+    assert evidence.accepted is True
+    assert evidence.rapid_vertical_change is True
@@ -0,0 +1,102 @@
+"""Lightweight deterministic person tracking for the single-camera V1 flow."""
+
+from dataclasses import dataclass
+from math import hypot
+from typing import Dict, Sequence, Tuple
+
+from v1.pose import PersonPose
+
+
+@dataclass(frozen=True)
+class TrackedPersonPose:
+    track_id: str
+    detected_at_monotonic: float
+    pose: PersonPose
+
+
+@dataclass
+class _Track:
+    center: Tuple[float, float]
+    last_seen_at: float
+
+
+class PersonTracker:
+    """Assign stable IDs by nearest normalized box center across adjacent frames."""
+
+    def __init__(
+        self, max_match_distance_ratio: float = 0.2, max_age_seconds: float = 2.0
+    ) -> None:
+        if not 0.0 < max_match_distance_ratio <= 1.0:
+            raise ValueError("max_match_distance_ratio must be in (0, 1]")
+        if max_age_seconds <= 0:
+            raise ValueError("max_age_seconds must be positive")
+        self._max_match_distance_ratio = max_match_distance_ratio
+        self._max_age_seconds = max_age_seconds
+        self._tracks: Dict[str, _Track] = {}
+        self._next_track_number = 1
+
+    def update(
+        self,
+        poses: Sequence[PersonPose],
+        detected_at_monotonic: float,
+        frame_size: Tuple[int, int],
+    ) -> Sequence[TrackedPersonPose]:
+        width, height = frame_size
+        if width <= 0 or height <= 0:
+            raise ValueError("frame_size must contain positive width and height")
+        self._expire_tracks(detected_at_monotonic)
+        available_ids = set(self._tracks)
+        tracked = []
+        for pose in poses:
+            center = self._box_center(pose)
+            track_id = self._nearest_available_track(center, available_ids, width, height)
+            if track_id is None:
+                track_id = "P-{0:04d}".format(self._next_track_number)
+                self._next_track_number += 1
+            else:
+                available_ids.remove(track_id)
+            self._tracks[track_id] = _Track(center=center, last_seen_at=detected_at_monotonic)
+            tracked.append(
+                TrackedPersonPose(
+                    track_id=track_id,
+                    detected_at_monotonic=float(detected_at_monotonic),
+                    pose=pose,
+                )
+            )
+        return tuple(tracked)
+
+    def _nearest_available_track(
+        self,
+        center: Tuple[float, float],
+        available_ids: set,
+        width: int,
+        height: int,
+    ):
+        closest_id = None
+        closest_distance = None
+        for track_id in available_ids:
+            previous = self._tracks[track_id].center
+            distance = hypot(
+                (center[0] - previous[0]) / float(width),
+                (center[1] - previous[1]) / float(height),
+            )
+            if distance <= self._max_match_distance_ratio and (
+                closest_distance is None or distance < closest_distance
+            ):
+                closest_id = track_id
+                closest_distance = distance
+        return closest_id
+
+    def _expire_tracks(self, now: float) -> None:
+        expired_ids = [
+            track_id
+            for track_id, track in self._tracks.items()
+            if now - track.last_seen_at > self._max_age_seconds
+        ]
+        for track_id in expired_ids:
+            del self._tracks[track_id]
+
+    @staticmethod
+    def _box_center(pose: PersonPose) -> Tuple[float, float]:
+        left, top, right, bottom = pose.box_xyxy
+        return ((left + right) / 2.0, (top + bottom) / 2.0)