move_detect/edge/geometry.py

import math

from edge.models import (
    Keypoint, BBox, NOSE, L_SHOULDER, R_SHOULDER, L_ELBOW, R_ELBOW,
    L_HIP, R_HIP, L_KNEE, R_KNEE, L_ANKLE, R_ANKLE, 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 arm_elevation_deg(keypoints: list[Keypoint], min_conf: float = 0.5) -> float | None:
    """어깨→팔꿈치 벡터가 '아래 수직'과 이루는 각의 좌우 최대값. 0=팔 내림, 90=수평 거상,
    180=머리 위. 신뢰 가능한 쪽 팔만 사용; 양쪽 다 불가하면 None (측정 불가 ≠ 0°)."""
    best = None
    for s_i, e_i in ((L_SHOULDER, L_ELBOW), (R_SHOULDER, R_ELBOW)):
        s, e = keypoints[s_i], keypoints[e_i]
        if s.conf < min_conf or e.conf < min_conf:
            continue
        vx, vy = e.x - s.x, e.y - s.y
        if not (vx or vy):
            continue
        # 이미지 y축은 아래로 증가: (0, +1)이 '팔 내림' 기준축.
        ang = math.degrees(math.atan2(abs(vx), vy)) if vy >= 0 else \
            180.0 - math.degrees(math.atan2(abs(vx), -vy))
        best = ang if best is None else max(best, ang)
    return best


def knee_flexion_deg(keypoints: list[Keypoint], min_conf: float = 0.5) -> float | None:
    """무릎 관절의 내부 각(엉덩이-무릎-발목)의 좌우 최소값. 180=곧게 섬, 작을수록 깊은
    굴곡(웅크림). 신뢰 가능한 쪽 다리만 사용; 양쪽 다 불가하면 None."""
    best = None
    for h_i, k_i, a_i in ((L_HIP, L_KNEE, L_ANKLE), (R_HIP, R_KNEE, R_ANKLE)):
        h, k, a = keypoints[h_i], keypoints[k_i], keypoints[a_i]
        if h.conf < min_conf or k.conf < min_conf or a.conf < min_conf:
            continue
        v1 = (h.x - k.x, h.y - k.y)
        v2 = (a.x - k.x, a.y - k.y)
        n1 = math.hypot(*v1)
        n2 = math.hypot(*v2)
        if not n1 or not n2:
            continue
        cos = max(-1.0, min(1.0, (v1[0] * v2[0] + v1[1] * v2[1]) / (n1 * n2)))
        ang = math.degrees(math.acos(cos))
        best = ang if best is None else min(best, ang)
    return best


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