move_detect/edge/geometry.py

import math

from edge.models import (
    Keypoint, BBox, NOSE, L_SHOULDER, R_SHOULDER, L_HIP, R_HIP, CORE_JOINTS,
)


def _mean(points: list[tuple[float, float]]) -> tuple[float, float]:
    n = len(points)
    return (sum(p[0] for p in points) / n, sum(p[1] for p in points) / n)


def com(keypoints: list[Keypoint]) -> tuple[float, float]:
    pts = [(keypoints[i].x, keypoints[i].y) for i in CORE_JOINTS]
    return _mean(pts)


def _shoulder_center(kps: list[Keypoint]) -> tuple[float, float]:
    return _mean([(kps[L_SHOULDER].x, kps[L_SHOULDER].y),
                  (kps[R_SHOULDER].x, kps[R_SHOULDER].y)])


def _hip_center(kps: list[Keypoint]) -> tuple[float, float]:
    return _mean([(kps[L_HIP].x, kps[L_HIP].y),
                  (kps[R_HIP].x, kps[R_HIP].y)])


def torso_angle_deg(keypoints: list[Keypoint]) -> float:
    """몸통(어깨중심→엉덩이중심) 벡터가 수직축과 이루는 각. 0=수직, 90=수평."""
    sx, sy = _shoulder_center(keypoints)
    hx, hy = _hip_center(keypoints)
    vx, vy = hx - sx, hy - sy
    # 수직축(0,1) 대비 편차. 위/아래 무관하게 |성분| 사용.
    return math.degrees(math.atan2(abs(vx), abs(vy))) if (vx or vy) else 0.0


def head_below_hips(keypoints: list[Keypoint], nose_thr: float = 0.2) -> bool:
    # Only trust the nose if the model is confident about it. An unconfident nose (commonly
    # hallucinated at (0,0) / a frame corner) would otherwise assert — or silently suppress —
    # this fall cue on data we don't trust. Same 'visible' gate as visible_count().
    nose = keypoints[NOSE]
    if nose.conf < nose_thr:
        return False
    _, hy = _hip_center(keypoints)
    return nose.y > hy


def visible_count(keypoints: list[Keypoint], thr: float = 0.2) -> int:
    return sum(1 for k in keypoints if k.conf >= thr)


def core_visible(keypoints: list[Keypoint], thr: float = 0.5) -> bool:
    return all(keypoints[i].conf >= thr for i in CORE_JOINTS)


def height_of(bbox: BBox) -> float:
    return bbox.height


def normalize(value: float, bbox_height: float) -> float:
    return value / bbox_height if bbox_height else 0.0


# Keypoints below this confidence are treated as noise (commonly hallucinated at a frame
# corner). Averaging them into the motion estimate makes a still track read as moving, which
# suppresses no_motion/immobility exactly when they matter (a motionless fallen worker).
MOTION_MIN_CONF = 0.2
MOTION_MIN_KP = 3


def track_displacement(track) -> float:
    """Mean per-keypoint SPEED between the last two frames: displacement normalized by bbox
    height AND by elapsed time (per second). Without the time term the value scales with the
    frame rate, so the same physical stillness reads as 'moving' at low fps and 'still' at
    high fps — flipping immobility/no_motion with the daemon's actual (load-dependent) fps.
    Returns 1.0 (moving/unknown) with <2 frames or a non-positive dt.

    Only keypoints the model is confident about in BOTH frames are averaged; below
    MOTION_MIN_KP trustworthy points it falls back to the bbox centre (a stable proxy that
    doesn't ride on one noisy joint).

    NOTE: motion_thr in FallConfig/ImmobilityConfig is a per-SECOND threshold (units/second),
    tunable via MOTION_THR in the environment; recalibrate on device against real jitter."""
    if len(track.frames) < 2:
        return 1.0
    a, b = track.frames[-2], track.frames[-1]
    dt = b.t - a.t
    if dt <= 0:
        return 1.0
    h = b.bbox.height or 1.0
    pairs = [(p, q) for p, q in zip(a.keypoints, b.keypoints)
             if p.conf >= MOTION_MIN_CONF and q.conf >= MOTION_MIN_CONF]
    if len(pairs) >= MOTION_MIN_KP:
        d = sum(((p.x - q.x) ** 2 + (p.y - q.y) ** 2) ** 0.5 for p, q in pairs) / len(pairs)
    else:
        d = ((a.bbox.cx - b.bbox.cx) ** 2 + (a.bbox.cy - b.bbox.cy) ** 2) ** 0.5
    return normalize(d, h) / dt