Spaces:
Paused
Paused
Add front facing metrics module
Browse files- app/models/front_facing_metrics.py +1203 -0
app/models/front_facing_metrics.py
ADDED
|
@@ -0,0 +1,1203 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
"""
|
| 2 |
+
Front-Facing Golf Swing Metrics Calculator
|
| 3 |
+
|
| 4 |
+
This module contains all the mathematical calculations for front-facing golf swing metrics.
|
| 5 |
+
The metrics_calculator.py file handles DTL (Down-the-Line) view metrics.
|
| 6 |
+
"""
|
| 7 |
+
|
| 8 |
+
import math
|
| 9 |
+
import numpy as np
|
| 10 |
+
import cv2
|
| 11 |
+
from typing import Dict, List, Tuple, Optional, Union
|
| 12 |
+
|
| 13 |
+
# Landmark indices
|
| 14 |
+
L_HIP, R_HIP = 23, 24
|
| 15 |
+
L_SHO, R_SHO = 11, 12
|
| 16 |
+
L_EYE, R_EYE = 2, 5
|
| 17 |
+
|
| 18 |
+
# Debug helper
|
| 19 |
+
VERBOSE = False
|
| 20 |
+
def dbg(msg):
|
| 21 |
+
if VERBOSE: print(f"DEBUG: {msg}")
|
| 22 |
+
|
| 23 |
+
# Core math utilities
|
| 24 |
+
def wrap180(a):
|
| 25 |
+
return (a + 180.0) % 360.0 - 180.0
|
| 26 |
+
|
| 27 |
+
def unit2(v):
|
| 28 |
+
"""Normalize 2D vector"""
|
| 29 |
+
n = (v[0]*v[0] + v[1]*v[1])**0.5
|
| 30 |
+
return (v[0]/n, v[1]/n) if n > 1e-6 else (0.0, 1.0)
|
| 31 |
+
|
| 32 |
+
def signed_angle2(u, v):
|
| 33 |
+
"""Signed angle between two 2D vectors in degrees (-180, 180]"""
|
| 34 |
+
dot = u[0]*v[0] + u[1]*v[1]
|
| 35 |
+
det = u[0]*v[1] - u[1]*v[0]
|
| 36 |
+
return math.degrees(math.atan2(det, dot))
|
| 37 |
+
|
| 38 |
+
def smooth_landmarks(pose_data: Dict[int, List], alpha: float = 0.35) -> Dict[int, List]:
|
| 39 |
+
"""Apply exponential moving average smoothing to landmarks"""
|
| 40 |
+
if not pose_data:
|
| 41 |
+
return pose_data
|
| 42 |
+
|
| 43 |
+
frame_indices = sorted(pose_data.keys())
|
| 44 |
+
smoothed_data = {}
|
| 45 |
+
|
| 46 |
+
for frame_idx in frame_indices:
|
| 47 |
+
landmarks = pose_data[frame_idx]
|
| 48 |
+
if not landmarks or not isinstance(landmarks, list):
|
| 49 |
+
continue
|
| 50 |
+
|
| 51 |
+
smoothed_landmarks = []
|
| 52 |
+
for i, lm in enumerate(landmarks):
|
| 53 |
+
if lm and isinstance(lm, list) and len(lm) >= 3:
|
| 54 |
+
x, y, vis = lm[0], lm[1], lm[2]
|
| 55 |
+
|
| 56 |
+
# Get previous smoothed values
|
| 57 |
+
if frame_idx > 0 and frame_idx - 1 in smoothed_data:
|
| 58 |
+
prev_lms = smoothed_data[frame_idx - 1]
|
| 59 |
+
if (i < len(prev_lms) and prev_lms[i] and
|
| 60 |
+
isinstance(prev_lms[i], list) and len(prev_lms[i]) >= 3):
|
| 61 |
+
px, py, pv = prev_lms[i][0], prev_lms[i][1], prev_lms[i][2]
|
| 62 |
+
x = alpha * x + (1 - alpha) * px
|
| 63 |
+
y = alpha * y + (1 - alpha) * py
|
| 64 |
+
vis = alpha * vis + (1 - alpha) * pv
|
| 65 |
+
|
| 66 |
+
smoothed_landmarks.append([x, y, vis])
|
| 67 |
+
else:
|
| 68 |
+
smoothed_landmarks.append(lm)
|
| 69 |
+
|
| 70 |
+
smoothed_data[frame_idx] = smoothed_landmarks
|
| 71 |
+
|
| 72 |
+
return smoothed_data
|
| 73 |
+
|
| 74 |
+
|
| 75 |
+
def get_landmark_position(landmarks: List, landmark_idx: int) -> Optional[Tuple[float, float, float]]:
|
| 76 |
+
"""Get landmark position (x, y, visibility) by index"""
|
| 77 |
+
if landmarks is None or not isinstance(landmarks, list) or landmark_idx >= len(landmarks) or landmarks[landmark_idx] is None:
|
| 78 |
+
return None
|
| 79 |
+
|
| 80 |
+
landmark = landmarks[landmark_idx]
|
| 81 |
+
if landmark is not None and isinstance(landmark, list) and len(landmark) >= 3:
|
| 82 |
+
return (landmark[0], landmark[1], landmark[2])
|
| 83 |
+
return None
|
| 84 |
+
|
| 85 |
+
|
| 86 |
+
def calculate_shoulder_tilt_at_impact(
|
| 87 |
+
pose_data,
|
| 88 |
+
swing_phases,
|
| 89 |
+
world_landmarks=None,
|
| 90 |
+
frames=None, # NEW: pass list/array of frames OR
|
| 91 |
+
image_shape=None, # NEW: (H, W) if frames not available
|
| 92 |
+
handedness='right'
|
| 93 |
+
):
|
| 94 |
+
"""
|
| 95 |
+
Shoulder Tilt @ Impact (front-facing).
|
| 96 |
+
+ve if trail shoulder lower (right shoulder for right-handed).
|
| 97 |
+
FIXED: Uses image-plane only for shoulder tilt; 3D only for frame rejection.
|
| 98 |
+
"""
|
| 99 |
+
|
| 100 |
+
import math, numpy as np
|
| 101 |
+
L_EYE, R_EYE = 2, 5
|
| 102 |
+
L_HIP, R_HIP = 23, 24
|
| 103 |
+
L_SHO, R_SHO = 11, 12
|
| 104 |
+
|
| 105 |
+
# ---------- helpers ----------
|
| 106 |
+
def _scale_xy(x, y, H, W):
|
| 107 |
+
"""Unit detection guard to prevent double-scaling"""
|
| 108 |
+
if H is None or W is None:
|
| 109 |
+
return x, y
|
| 110 |
+
# If values look like pixels (>>2), don't rescale
|
| 111 |
+
if max(abs(x), abs(y), 1.0) > 2.0:
|
| 112 |
+
return x, y
|
| 113 |
+
return x * W, y * H
|
| 114 |
+
|
| 115 |
+
def _line_tilt_deg(lm, iL, iR):
|
| 116 |
+
# raw image tilt (no aspect correction); used only for small-roll estimate
|
| 117 |
+
if not lm or iL >= len(lm) or iR >= len(lm) or lm[iL] is None or lm[iR] is None:
|
| 118 |
+
return None
|
| 119 |
+
dx = lm[iR][0] - lm[iL][0]
|
| 120 |
+
dy = lm[iR][1] - lm[iL][1]
|
| 121 |
+
if abs(dx) + abs(dy) < 1e-8:
|
| 122 |
+
return None
|
| 123 |
+
return math.degrees(math.atan2(dy, dx))
|
| 124 |
+
|
| 125 |
+
def _consensus_roll_deg(lm, H, W):
|
| 126 |
+
"""Camera roll from eyes/hips with unit detection"""
|
| 127 |
+
if not lm:
|
| 128 |
+
return 0.0
|
| 129 |
+
cand = []
|
| 130 |
+
for line_idxs in [(L_EYE, R_EYE), (L_HIP, R_HIP)]:
|
| 131 |
+
iL, iR = line_idxs
|
| 132 |
+
if lm[iL] and lm[iR]:
|
| 133 |
+
dx_raw = lm[iR][0] - lm[iL][0]
|
| 134 |
+
dy_raw = lm[iR][1] - lm[iL][1]
|
| 135 |
+
dx, dy = _scale_xy(dx_raw, dy_raw, H, W)
|
| 136 |
+
if abs(dx) + abs(dy) > 1e-8:
|
| 137 |
+
tilt = math.degrees(math.atan2(dy, dx))
|
| 138 |
+
tilt = ((tilt + 90.0) % 180.0) - 90.0 # (-90,90]
|
| 139 |
+
if abs(tilt) <= 8.0: # Only small rolls
|
| 140 |
+
cand.append(tilt)
|
| 141 |
+
|
| 142 |
+
if len(cand) < 2 or np.std(cand) > 3.0:
|
| 143 |
+
return 0.0
|
| 144 |
+
return float(np.median(cand))
|
| 145 |
+
|
| 146 |
+
def _get_HW():
|
| 147 |
+
# prefer frames; else image_shape; else no correction
|
| 148 |
+
if frames is not None and len(frames):
|
| 149 |
+
H, W = frames[0].shape[:2]
|
| 150 |
+
return float(H), float(W)
|
| 151 |
+
if image_shape is not None and len(image_shape) >= 2:
|
| 152 |
+
H, W = image_shape[:2]
|
| 153 |
+
return float(H), float(W)
|
| 154 |
+
return None, None # no pixel aspect info
|
| 155 |
+
|
| 156 |
+
def _deroll(dx, dy, roll_deg):
|
| 157 |
+
cr = math.cos(math.radians(roll_deg))
|
| 158 |
+
sr = math.sin(math.radians(roll_deg))
|
| 159 |
+
# rotate by -roll: [ c s; -s c ] * [dx, dy]
|
| 160 |
+
return (cr*dx + sr*dy, -sr*dx + cr*dy)
|
| 161 |
+
|
| 162 |
+
def _width_px_derolled(lm, iL, iR, roll_deg):
|
| 163 |
+
"""Get derolled horizontal width and vertical separation in pixels"""
|
| 164 |
+
if not lm or not lm[iL] or not lm[iR]:
|
| 165 |
+
return 0.0, 0.0
|
| 166 |
+
xL, yL = lm[iL][0], lm[iL][1]
|
| 167 |
+
xR, yR = lm[iR][0], lm[iR][1]
|
| 168 |
+
cr, sr = math.cos(math.radians(roll_deg)), math.sin(math.radians(roll_deg))
|
| 169 |
+
# rotate by -roll
|
| 170 |
+
dx = (cr*(xR-xL) + sr*(yR-yL))
|
| 171 |
+
dy = (-sr*(xR-xL) + cr*(yR-yL))
|
| 172 |
+
return abs(dx), dy # horizontal width (derolled), vertical (derolled)
|
| 173 |
+
|
| 174 |
+
def _check_frame_quality(lm, H, W):
|
| 175 |
+
"""Quality gates: visibility, shoulder width, basic checks"""
|
| 176 |
+
if not lm or not lm[L_SHO] or not lm[R_SHO]:
|
| 177 |
+
return False, "missing_shoulders"
|
| 178 |
+
|
| 179 |
+
# Visibility check
|
| 180 |
+
if lm[L_SHO][2] < 0.6 or lm[R_SHO][2] < 0.6:
|
| 181 |
+
return False, "low_visibility"
|
| 182 |
+
|
| 183 |
+
# Shoulder width check (>8% of frame width)
|
| 184 |
+
if W is not None:
|
| 185 |
+
dx_raw = abs(lm[R_SHO][0] - lm[L_SHO][0])
|
| 186 |
+
dx_scaled, _ = _scale_xy(dx_raw, 0, H, W)
|
| 187 |
+
if abs(dx_scaled) < 0.08 * W:
|
| 188 |
+
return False, "too_narrow"
|
| 189 |
+
|
| 190 |
+
return True, "ok"
|
| 191 |
+
|
| 192 |
+
def _estimate_torso_yaw_3d(world_frame):
|
| 193 |
+
"""Estimate torso yaw from 3D shoulders to reject severe angles"""
|
| 194 |
+
if not world_frame or not world_frame[L_SHO] or not world_frame[R_SHO]:
|
| 195 |
+
return None
|
| 196 |
+
|
| 197 |
+
dx = world_frame[R_SHO][0] - world_frame[L_SHO][0]
|
| 198 |
+
dz = world_frame[R_SHO][2] - world_frame[L_SHO][2]
|
| 199 |
+
|
| 200 |
+
if abs(dx) + abs(dz) < 1e-8:
|
| 201 |
+
return None
|
| 202 |
+
|
| 203 |
+
# Rough yaw estimate - angle from frontal (dz=0)
|
| 204 |
+
yaw = abs(math.degrees(math.atan2(abs(dz), abs(dx))))
|
| 205 |
+
return yaw
|
| 206 |
+
|
| 207 |
+
# ---------- pick exact impact frame ----------
|
| 208 |
+
if world_landmarks:
|
| 209 |
+
setup_frames = swing_phases.get("setup", [])
|
| 210 |
+
impact_f = _impact_frame(world_landmarks, swing_phases)
|
| 211 |
+
if impact_f is None:
|
| 212 |
+
impact_frames = swing_phases.get("impact", [])
|
| 213 |
+
if impact_frames:
|
| 214 |
+
impact_f = impact_frames[0]
|
| 215 |
+
|
| 216 |
+
# Try subframe refinement if available
|
| 217 |
+
if impact_f is not None:
|
| 218 |
+
f_refined = _subframe_impact_refinement(world_landmarks, impact_f)
|
| 219 |
+
# Use integer part for now, but structure supports fractional
|
| 220 |
+
impact_f = int(round(f_refined))
|
| 221 |
+
else:
|
| 222 |
+
impact_frames = swing_phases.get("impact", [])
|
| 223 |
+
if not impact_frames:
|
| 224 |
+
allf = sorted(pose_data.keys())
|
| 225 |
+
if not allf:
|
| 226 |
+
return None
|
| 227 |
+
impact_f = allf[-1]
|
| 228 |
+
else:
|
| 229 |
+
impact_f = impact_frames[0]
|
| 230 |
+
|
| 231 |
+
# Get the refined impact frame data
|
| 232 |
+
if impact_f not in pose_data:
|
| 233 |
+
return None
|
| 234 |
+
|
| 235 |
+
lm = pose_data[impact_f]
|
| 236 |
+
H, W = _get_HW()
|
| 237 |
+
|
| 238 |
+
# ---------- quality checks ----------
|
| 239 |
+
quality_ok, quality_reason = _check_frame_quality(lm, H, W)
|
| 240 |
+
if not quality_ok:
|
| 241 |
+
print(f"DEBUG shoulder_tilt: frame_quality_failed - {quality_reason}")
|
| 242 |
+
# Still continue but mark as suspect
|
| 243 |
+
|
| 244 |
+
# 3D yaw check for frame rejection only
|
| 245 |
+
yaw_ok = True
|
| 246 |
+
if world_landmarks and impact_f in world_landmarks:
|
| 247 |
+
yaw = _estimate_torso_yaw_3d(world_landmarks[impact_f])
|
| 248 |
+
if yaw is not None and yaw > 30.0:
|
| 249 |
+
yaw_ok = False
|
| 250 |
+
print(f"DEBUG shoulder_tilt: rejecting_frame - torso_yaw={yaw:.1f}° > 30°")
|
| 251 |
+
|
| 252 |
+
# ---------- 2D image-plane calculation (primary method) ----------
|
| 253 |
+
# Get shoulder positions with unit detection
|
| 254 |
+
xL, yL = lm[L_SHO][0], lm[L_SHO][1]
|
| 255 |
+
xR, yR = lm[R_SHO][0], lm[R_SHO][1]
|
| 256 |
+
|
| 257 |
+
dx_raw = xR - xL
|
| 258 |
+
dy_raw = yR - yL
|
| 259 |
+
|
| 260 |
+
dx, dy = _scale_xy(dx_raw, dy_raw, H, W)
|
| 261 |
+
|
| 262 |
+
# Camera roll correction (minimal smoothing - use single frame)
|
| 263 |
+
alpha_micro = 0.8 # Much lighter smoothing for impact
|
| 264 |
+
lm_for_roll = [lm[i][:] if lm[i] else None for i in range(len(lm))] # Copy landmarks
|
| 265 |
+
if impact_f > 0 and (impact_f - 1) in pose_data:
|
| 266 |
+
prev_lm = pose_data[impact_f - 1]
|
| 267 |
+
# Light smoothing only if previous frame available
|
| 268 |
+
if prev_lm and prev_lm[L_SHO] and prev_lm[R_SHO]:
|
| 269 |
+
for i in [L_SHO, R_SHO, L_EYE, R_EYE, L_HIP, R_HIP]:
|
| 270 |
+
if lm_for_roll[i] and prev_lm[i]:
|
| 271 |
+
lm_for_roll[i] = [
|
| 272 |
+
alpha_micro * lm[i][0] + (1 - alpha_micro) * prev_lm[i][0],
|
| 273 |
+
alpha_micro * lm[i][1] + (1 - alpha_micro) * prev_lm[i][1],
|
| 274 |
+
lm[i][2] # Keep original visibility
|
| 275 |
+
]
|
| 276 |
+
|
| 277 |
+
roll = _consensus_roll_deg(lm_for_roll, H, W)
|
| 278 |
+
|
| 279 |
+
# Hybrid de-yaw shoulder tilt calculation
|
| 280 |
+
# Derolled shoulder and pelvis widths (pixels)
|
| 281 |
+
sho_dx_r, sho_dy_r = _width_px_derolled(lm, L_SHO, R_SHO, roll)
|
| 282 |
+
pel_dx_r, _ = _width_px_derolled(lm, L_HIP, R_HIP, roll)
|
| 283 |
+
|
| 284 |
+
# Get setup pelvis width median as scale anchor (use only good frames)
|
| 285 |
+
setup_frames = swing_phases.get("setup", [])[:8]
|
| 286 |
+
setup_pelvis_widths = []
|
| 287 |
+
for f in setup_frames:
|
| 288 |
+
lmf = pose_data.get(f)
|
| 289 |
+
if not lmf or not lmf[L_HIP] or not lmf[R_HIP]:
|
| 290 |
+
continue
|
| 291 |
+
# prefer low-yaw frames if world_landmarks are available
|
| 292 |
+
if world_landmarks and f in world_landmarks:
|
| 293 |
+
_yaw = _estimate_torso_yaw_3d(world_landmarks[f]) or 0.0
|
| 294 |
+
if _yaw > 25.0:
|
| 295 |
+
continue
|
| 296 |
+
w, _ = _width_px_derolled(lmf, L_HIP, R_HIP, roll)
|
| 297 |
+
if w > 1.0:
|
| 298 |
+
setup_pelvis_widths.append(w)
|
| 299 |
+
|
| 300 |
+
pel_setup_med = float(np.median(setup_pelvis_widths)) if setup_pelvis_widths else None
|
| 301 |
+
|
| 302 |
+
# Decide if we must de-yaw: narrow shoulders or big yaw
|
| 303 |
+
need_deyaw = (W is not None and sho_dx_r < 0.08*W) or (world_landmarks and not yaw_ok)
|
| 304 |
+
|
| 305 |
+
# Build corrected horizontal width
|
| 306 |
+
dx_corr = sho_dx_r
|
| 307 |
+
deyaw_ratio = 1.0
|
| 308 |
+
if need_deyaw and pel_setup_med and pel_dx_r > 1.0:
|
| 309 |
+
# De-yaw using pelvis scale ratio; clamp to avoid blowups
|
| 310 |
+
deyaw_ratio = pel_setup_med / pel_dx_r
|
| 311 |
+
deyaw_ratio = max(1.0, min(deyaw_ratio, 8.0)) # 1×..8×
|
| 312 |
+
dx_corr = sho_dx_r * deyaw_ratio
|
| 313 |
+
|
| 314 |
+
# As an optional second path (if 3D exists & stable), use 3D shoulder length
|
| 315 |
+
if need_deyaw and world_landmarks and impact_f in world_landmarks:
|
| 316 |
+
wf = world_landmarks[impact_f]
|
| 317 |
+
if wf and wf[L_SHO] and wf[R_SHO]:
|
| 318 |
+
Ls, Rs = wf[L_SHO], wf[R_SHO]
|
| 319 |
+
L3D = math.hypot(Rs[0]-Ls[0], Rs[2]-Ls[2]) # horizontal shoulder sep in XZ (meters, scale-free wrt yaw)
|
| 320 |
+
# Calibrate px-per-meter from setup shoulders (robust to yaw via pelvis ratio)
|
| 321 |
+
if L3D > 1e-6 and setup_frames:
|
| 322 |
+
px_per_m = None
|
| 323 |
+
for f in setup_frames:
|
| 324 |
+
if f in world_landmarks and pose_data.get(f):
|
| 325 |
+
wf_s, lm_s = world_landmarks[f], pose_data[f]
|
| 326 |
+
if wf_s and lm_s and lm_s[L_SHO] and lm_s[R_SHO]:
|
| 327 |
+
sx, _ = _width_px_derolled(lm_s, L_SHO, R_SHO, roll)
|
| 328 |
+
L3D_s = None
|
| 329 |
+
if wf_s[L_SHO] and wf_s[R_SHO]:
|
| 330 |
+
L3D_s = math.hypot(wf_s[R_SHO][0]-wf_s[L_SHO][0], wf_s[R_SHO][2]-wf_s[L_SHO][2])
|
| 331 |
+
if sx > 1.0 and L3D_s and L3D_s > 1e-6:
|
| 332 |
+
px_per_m = sx / L3D_s
|
| 333 |
+
break
|
| 334 |
+
if px_per_m:
|
| 335 |
+
dx_corr_3d = L3D * px_per_m
|
| 336 |
+
dx_corr = max(dx_corr, dx_corr_3d) # take the more conservative (larger) width
|
| 337 |
+
|
| 338 |
+
# Now compute tilt using corrected horizontal width and derolled vertical
|
| 339 |
+
if abs(dx_corr) + abs(sho_dy_r) < 1e-8:
|
| 340 |
+
return None
|
| 341 |
+
|
| 342 |
+
tilt_mag = math.degrees(math.atan2(abs(sho_dy_r), max(dx_corr, 1e-3)))
|
| 343 |
+
trail_lower = (sho_dy_r > 0) if handedness=='right' else (sho_dy_r < 0)
|
| 344 |
+
tilt_final = tilt_mag if trail_lower else -tilt_mag
|
| 345 |
+
|
| 346 |
+
# Plausibility clamp for front-facing tilt (pros ~30-45°)
|
| 347 |
+
if abs(tilt_final) < 10.0 or abs(tilt_final) > 70.0:
|
| 348 |
+
# If we still look crazy, median over ±1 frame
|
| 349 |
+
neigh = []
|
| 350 |
+
for f in [impact_f-1, impact_f, impact_f+1]:
|
| 351 |
+
if f in pose_data and pose_data[f] and pose_data[f][L_SHO] and pose_data[f][R_SHO]:
|
| 352 |
+
sx, sy = _width_px_derolled(pose_data[f], L_SHO, R_SHO, roll)
|
| 353 |
+
px, _ = _width_px_derolled(pose_data[f], L_HIP, R_HIP, roll)
|
| 354 |
+
if pel_setup_med and px > 1.0:
|
| 355 |
+
r = max(1.0, min(pel_setup_med/px, 8.0))
|
| 356 |
+
t = math.degrees(math.atan2(abs(sy), max(sx*r, 1e-3)))
|
| 357 |
+
sgn = (sy > 0) if handedness=='right' else (sy < 0)
|
| 358 |
+
neigh.append(t if sgn else -t)
|
| 359 |
+
if neigh:
|
| 360 |
+
tilt_final = float(np.median(neigh))
|
| 361 |
+
|
| 362 |
+
# Hard reroute for problematic cases (already handled by hybrid path above)
|
| 363 |
+
if not yaw_ok or (W is not None and sho_dx_r < 0.08*W):
|
| 364 |
+
# force hybrid path (already handled above). Also avoid labeling as "final"
|
| 365 |
+
pass
|
| 366 |
+
|
| 367 |
+
# Sanity clamp and flags
|
| 368 |
+
definition_suspect = False
|
| 369 |
+
if not (10.0 <= abs(tilt_final) <= 70.0):
|
| 370 |
+
definition_suspect = True
|
| 371 |
+
|
| 372 |
+
if not quality_ok or not yaw_ok:
|
| 373 |
+
definition_suspect = True
|
| 374 |
+
|
| 375 |
+
# Debug output with de-yaw info
|
| 376 |
+
unit_note = " (units_detected_as_pixels)" if max(abs(dx_raw), abs(dy_raw)) > 2.0 else ""
|
| 377 |
+
yaw_note = f" yaw_ok={yaw_ok}" if world_landmarks else ""
|
| 378 |
+
deyaw_note = f" hybrid_deyaw r={deyaw_ratio:.1f}, dx_corr={dx_corr:.1f}" if need_deyaw else ""
|
| 379 |
+
print(f"DEBUG shoulder_tilt: sho_dx_r={sho_dx_r:.1f}, sho_dy_r={sho_dy_r:.1f}, roll={roll:.1f}°{deyaw_note}, "
|
| 380 |
+
f"tilt={tilt_final:.1f}°{unit_note}{yaw_note}, suspect={definition_suspect}")
|
| 381 |
+
|
| 382 |
+
if pel_setup_med and pel_dx_r > 1.0:
|
| 383 |
+
print(f"DEBUG shoulder_tilt: pel_setup_med={pel_setup_med:.1f}, pel_dx_r={pel_dx_r:.1f}")
|
| 384 |
+
|
| 385 |
+
return max(-70.0, min(70.0, tilt_final))
|
| 386 |
+
|
| 387 |
+
|
| 388 |
+
|
| 389 |
+
|
| 390 |
+
|
| 391 |
+
|
| 392 |
+
def pelvis_hat(world_frame):
|
| 393 |
+
"""Get unit pelvis vector (R_hip - L_hip) in XZ plane"""
|
| 394 |
+
try:
|
| 395 |
+
L = world_frame[L_HIP] # Left hip (23)
|
| 396 |
+
R = world_frame[R_HIP] # Right hip (24)
|
| 397 |
+
return unit2((R[0]-L[0], R[2]-L[2]))
|
| 398 |
+
except Exception:
|
| 399 |
+
return None
|
| 400 |
+
|
| 401 |
+
def _target_hat_from_toes_world(world_landmarks, setup_frames, handedness='right'):
|
| 402 |
+
"""Build stable target axis from toe line at address"""
|
| 403 |
+
LEFT_TOE, RIGHT_TOE = 31, 32
|
| 404 |
+
|
| 405 |
+
toe_vecs = []
|
| 406 |
+
for f in setup_frames:
|
| 407 |
+
if f in world_landmarks:
|
| 408 |
+
frame = world_landmarks[f]
|
| 409 |
+
if frame and len(frame) > 32:
|
| 410 |
+
try:
|
| 411 |
+
Ltoe = frame[LEFT_TOE]
|
| 412 |
+
Rtoe = frame[RIGHT_TOE]
|
| 413 |
+
if Ltoe and Rtoe and len(Ltoe) >= 3 and len(Rtoe) >= 3:
|
| 414 |
+
toe_vec = (Rtoe[0]-Ltoe[0], Rtoe[2]-Ltoe[2])
|
| 415 |
+
if (toe_vec[0]**2 + toe_vec[1]**2) > 1e-6:
|
| 416 |
+
toe_vecs.append(toe_vec)
|
| 417 |
+
except Exception:
|
| 418 |
+
continue
|
| 419 |
+
|
| 420 |
+
if not toe_vecs:
|
| 421 |
+
dbg("target_axis - NO VALID TOE VECTORS, using default")
|
| 422 |
+
return (1.0, 0.0)
|
| 423 |
+
|
| 424 |
+
# Target axis is parallel to toe line (not perpendicular)
|
| 425 |
+
target_hat = unit2(np.median(toe_vecs, axis=0))
|
| 426 |
+
|
| 427 |
+
# Optional: flip sign for consistency if you want +X toward target for left-handed
|
| 428 |
+
if handedness == 'left':
|
| 429 |
+
target_hat = (-target_hat[0], -target_hat[1])
|
| 430 |
+
|
| 431 |
+
return target_hat
|
| 432 |
+
|
| 433 |
+
|
| 434 |
+
def _simple_pixel_sway_fallback(sm, setup_frames, top_frame):
|
| 435 |
+
"""Simple pixel-based sway fallback that ALWAYS returns a value"""
|
| 436 |
+
# Get pelvis centers in pixels
|
| 437 |
+
def get_pelvis_center_px(frame_idx):
|
| 438 |
+
lm = sm.get(frame_idx)
|
| 439 |
+
if not lm or not lm[23] or not lm[24]:
|
| 440 |
+
return None
|
| 441 |
+
return [(lm[23][0] + lm[24][0]) / 2, (lm[23][1] + lm[24][1]) / 2]
|
| 442 |
+
|
| 443 |
+
# Setup center (median)
|
| 444 |
+
setup_centers = [get_pelvis_center_px(f) for f in setup_frames]
|
| 445 |
+
setup_centers = [c for c in setup_centers if c is not None]
|
| 446 |
+
if not setup_centers:
|
| 447 |
+
return 0.0 # Default value if no setup data
|
| 448 |
+
setup_ctr = np.median(setup_centers, axis=0)
|
| 449 |
+
|
| 450 |
+
# Top center
|
| 451 |
+
top_ctr = get_pelvis_center_px(top_frame)
|
| 452 |
+
if top_ctr is None:
|
| 453 |
+
return 0.0 # Default value if no top data
|
| 454 |
+
|
| 455 |
+
# Simple horizontal movement in pixels, convert with rough estimate
|
| 456 |
+
delta_px = top_ctr[0] - setup_ctr[0]
|
| 457 |
+
# Rough conversion: assume ~10 pixels per inch (typical for phone videos)
|
| 458 |
+
sway_inches = delta_px / 10.0
|
| 459 |
+
|
| 460 |
+
return float(sway_inches)
|
| 461 |
+
|
| 462 |
+
|
| 463 |
+
def _subframe_impact_refinement(world_landmarks: Dict[int, List], impact_candidate: int) -> float:
|
| 464 |
+
"""
|
| 465 |
+
Refine impact timing to sub-frame precision using parabolic fit to hand speed
|
| 466 |
+
Fits parabola around the impact candidate and returns fractional frame timestamp
|
| 467 |
+
"""
|
| 468 |
+
try:
|
| 469 |
+
# Get frames around impact candidate for parabolic fit
|
| 470 |
+
test_frames = [f for f in range(impact_candidate-2, impact_candidate+3)
|
| 471 |
+
if f in world_landmarks]
|
| 472 |
+
|
| 473 |
+
if len(test_frames) < 3:
|
| 474 |
+
return float(impact_candidate) # Not enough data for refinement
|
| 475 |
+
|
| 476 |
+
# Calculate hand speeds around impact
|
| 477 |
+
speeds = []
|
| 478 |
+
frame_indices = []
|
| 479 |
+
|
| 480 |
+
for i in range(1, len(test_frames)):
|
| 481 |
+
f_curr = test_frames[i]
|
| 482 |
+
f_prev = test_frames[i-1]
|
| 483 |
+
|
| 484 |
+
curr_frame = world_landmarks[f_curr]
|
| 485 |
+
prev_frame = world_landmarks[f_prev]
|
| 486 |
+
|
| 487 |
+
if (curr_frame and len(curr_frame) > 16 and
|
| 488 |
+
prev_frame and len(prev_frame) > 16 and
|
| 489 |
+
curr_frame[16] and prev_frame[16]):
|
| 490 |
+
|
| 491 |
+
curr_wrist = curr_frame[16]
|
| 492 |
+
prev_wrist = prev_frame[16]
|
| 493 |
+
|
| 494 |
+
# Calculate 3D speed
|
| 495 |
+
dx = curr_wrist[0] - prev_wrist[0]
|
| 496 |
+
dy = curr_wrist[1] - prev_wrist[1]
|
| 497 |
+
dz = curr_wrist[2] - prev_wrist[2]
|
| 498 |
+
speed = math.sqrt(dx*dx + dy*dy + dz*dz)
|
| 499 |
+
|
| 500 |
+
speeds.append(speed)
|
| 501 |
+
frame_indices.append(f_curr)
|
| 502 |
+
|
| 503 |
+
if len(speeds) < 3:
|
| 504 |
+
return float(impact_candidate)
|
| 505 |
+
|
| 506 |
+
# Fit parabola to speed curve: speed = a*t^2 + b*t + c
|
| 507 |
+
# Find peak (maximum) of parabola
|
| 508 |
+
t = np.array(frame_indices, dtype=float)
|
| 509 |
+
s = np.array(speeds)
|
| 510 |
+
|
| 511 |
+
# Polynomial fit (degree 2)
|
| 512 |
+
coeffs = np.polyfit(t, s, 2)
|
| 513 |
+
a, b, c = coeffs
|
| 514 |
+
|
| 515 |
+
if a >= 0: # Parabola opens upward → no maximum
|
| 516 |
+
return float(impact_candidate)
|
| 517 |
+
|
| 518 |
+
# Maximum at t = -b/(2a)
|
| 519 |
+
peak_frame_fractional = -b / (2*a)
|
| 520 |
+
|
| 521 |
+
# Impact happens just before peak (typically 0.5-1.0 frames before)
|
| 522 |
+
# Use the original logic of peak-1, but with fractional precision
|
| 523 |
+
impact_fractional = peak_frame_fractional - 0.7 # Refined offset
|
| 524 |
+
|
| 525 |
+
# Constrain to reasonable range around original estimate
|
| 526 |
+
if abs(impact_fractional - impact_candidate) > 2.0:
|
| 527 |
+
impact_fractional = float(impact_candidate)
|
| 528 |
+
|
| 529 |
+
print(f"DEBUG: subframe_refinement - parabola_coeffs=[{a:.6f}, {b:.6f}, {c:.6f}]")
|
| 530 |
+
print(f"DEBUG: subframe_refinement - peak_frame={peak_frame_fractional:.2f}, impact_refined={impact_fractional:.2f}")
|
| 531 |
+
|
| 532 |
+
return impact_fractional
|
| 533 |
+
|
| 534 |
+
except Exception as e:
|
| 535 |
+
print(f"DEBUG: subframe_refinement - failed: {e}, using integer frame")
|
| 536 |
+
return float(impact_candidate)
|
| 537 |
+
|
| 538 |
+
|
| 539 |
+
def _address_frames(swing_phases: Dict[str, List]) -> List[int]:
|
| 540 |
+
"""Get address/setup frames"""
|
| 541 |
+
return swing_phases.get("setup", [])[:10]
|
| 542 |
+
|
| 543 |
+
def _impact_frame(world_landmarks: Dict[int, List], swing_phases: Dict[str, List]) -> Optional[int]:
|
| 544 |
+
"""Find impact using peak lead-hand speed - 1 frame with inline refinement"""
|
| 545 |
+
downswing_frames = swing_phases.get("downswing", [])
|
| 546 |
+
impact_frames = swing_phases.get("impact", [])
|
| 547 |
+
|
| 548 |
+
analysis_frames = sorted(downswing_frames + impact_frames)
|
| 549 |
+
if len(analysis_frames) < 5:
|
| 550 |
+
return impact_frames[0] if impact_frames else None
|
| 551 |
+
|
| 552 |
+
# Calculate hand speeds
|
| 553 |
+
hand_speeds = []
|
| 554 |
+
valid_frames = []
|
| 555 |
+
|
| 556 |
+
for i in range(1, len(analysis_frames)):
|
| 557 |
+
f_curr = analysis_frames[i]
|
| 558 |
+
f_prev = analysis_frames[i-1]
|
| 559 |
+
|
| 560 |
+
if f_curr in world_landmarks and f_prev in world_landmarks:
|
| 561 |
+
curr_frame = world_landmarks[f_curr]
|
| 562 |
+
prev_frame = world_landmarks[f_prev]
|
| 563 |
+
|
| 564 |
+
if (curr_frame and len(curr_frame) > 16 and
|
| 565 |
+
prev_frame and len(prev_frame) > 16):
|
| 566 |
+
|
| 567 |
+
# Use lead hand (left wrist for right-handed golfer)
|
| 568 |
+
wrist_idx = 15 # Lead wrist for right-handed
|
| 569 |
+
curr_wrist = curr_frame[wrist_idx]
|
| 570 |
+
prev_wrist = prev_frame[wrist_idx]
|
| 571 |
+
|
| 572 |
+
if (curr_wrist and len(curr_wrist) >= 3 and
|
| 573 |
+
prev_wrist and len(prev_wrist) >= 3):
|
| 574 |
+
|
| 575 |
+
dx = curr_wrist[0] - prev_wrist[0]
|
| 576 |
+
dy = curr_wrist[1] - prev_wrist[1]
|
| 577 |
+
dz = curr_wrist[2] - prev_wrist[2]
|
| 578 |
+
speed = math.sqrt(dx*dx + dy*dy + dz*dz)
|
| 579 |
+
|
| 580 |
+
hand_speeds.append(speed)
|
| 581 |
+
valid_frames.append(f_curr)
|
| 582 |
+
|
| 583 |
+
if len(hand_speeds) >= 3:
|
| 584 |
+
# Find peak speed frame
|
| 585 |
+
max_speed_idx = np.argmax(hand_speeds)
|
| 586 |
+
peak_speed_frame = valid_frames[max_speed_idx]
|
| 587 |
+
|
| 588 |
+
# Initial estimate: peak speed - 1 frame
|
| 589 |
+
impact_candidate = peak_speed_frame
|
| 590 |
+
if max_speed_idx > 0:
|
| 591 |
+
impact_candidate = valid_frames[max_speed_idx - 1]
|
| 592 |
+
|
| 593 |
+
# Optional quadratic refinement
|
| 594 |
+
try:
|
| 595 |
+
test_frames = [f for f in range(impact_candidate-1, impact_candidate+2)
|
| 596 |
+
if f in world_landmarks]
|
| 597 |
+
if len(test_frames) >= 3:
|
| 598 |
+
speeds = []
|
| 599 |
+
for f in test_frames:
|
| 600 |
+
if f in valid_frames:
|
| 601 |
+
idx = valid_frames.index(f)
|
| 602 |
+
speeds.append(hand_speeds[idx])
|
| 603 |
+
|
| 604 |
+
if len(speeds) >= 3:
|
| 605 |
+
coeffs = np.polyfit(test_frames, speeds, 2)
|
| 606 |
+
a, b, c = coeffs
|
| 607 |
+
if a < 0: # Parabola opens downward
|
| 608 |
+
peak_fractional = -b / (2*a)
|
| 609 |
+
impact_refined = peak_fractional - 1.0
|
| 610 |
+
if abs(impact_refined - impact_candidate) <= 2.0:
|
| 611 |
+
impact_candidate = int(round(impact_refined))
|
| 612 |
+
except:
|
| 613 |
+
pass
|
| 614 |
+
|
| 615 |
+
# Clamp to ±2 frames around original estimate
|
| 616 |
+
original = valid_frames[max_speed_idx - 1] if max_speed_idx > 0 else peak_speed_frame
|
| 617 |
+
impact_candidate = max(original - 2, min(original + 2, impact_candidate))
|
| 618 |
+
|
| 619 |
+
return impact_candidate
|
| 620 |
+
|
| 621 |
+
return impact_frames[0] if impact_frames else None
|
| 622 |
+
|
| 623 |
+
|
| 624 |
+
def _conf3d(world_landmarks, impact_frame, setup_frames):
|
| 625 |
+
"""Compute 3D confidence and pelvis change in one function"""
|
| 626 |
+
if not world_landmarks or impact_frame not in world_landmarks:
|
| 627 |
+
return 0.0, 100.0
|
| 628 |
+
|
| 629 |
+
try:
|
| 630 |
+
# Check frames around impact for stability
|
| 631 |
+
test_frames = [f for f in range(impact_frame-2, impact_frame+3) if f in world_landmarks]
|
| 632 |
+
if len(test_frames) < 3:
|
| 633 |
+
return 0.3, 100.0
|
| 634 |
+
|
| 635 |
+
yaws = []
|
| 636 |
+
z_positions = []
|
| 637 |
+
|
| 638 |
+
for f in test_frames:
|
| 639 |
+
frame = world_landmarks[f]
|
| 640 |
+
if frame and len(frame) > 24 and frame[23] and frame[24]:
|
| 641 |
+
ph = pelvis_hat(frame)
|
| 642 |
+
if ph is not None:
|
| 643 |
+
yaw_approx = math.degrees(math.atan2(ph[1], ph[0]))
|
| 644 |
+
yaws.append(yaw_approx)
|
| 645 |
+
|
| 646 |
+
# Z position (depth stability)
|
| 647 |
+
lz, rz = frame[23][2], frame[24][2]
|
| 648 |
+
z_positions.append((lz + rz) / 2)
|
| 649 |
+
|
| 650 |
+
if len(yaws) < 3:
|
| 651 |
+
return 0.3, 100.0
|
| 652 |
+
|
| 653 |
+
# Compute stability metrics
|
| 654 |
+
yaw_std = np.std(yaws)
|
| 655 |
+
z_std = np.std(z_positions)
|
| 656 |
+
|
| 657 |
+
# Pelvis length change from setup
|
| 658 |
+
setup_pelvis_lens = []
|
| 659 |
+
for f in setup_frames[:5]:
|
| 660 |
+
if f in world_landmarks:
|
| 661 |
+
frame = world_landmarks[f]
|
| 662 |
+
if frame and len(frame) > 24 and frame[23] and frame[24]:
|
| 663 |
+
lx, lz = frame[23][0], frame[23][2]
|
| 664 |
+
rx, rz = frame[24][0], frame[24][2]
|
| 665 |
+
setup_pelvis_lens.append(math.sqrt((rx-lx)**2 + (rz-lz)**2))
|
| 666 |
+
|
| 667 |
+
pelvis_change_pct = 100.0
|
| 668 |
+
if setup_pelvis_lens:
|
| 669 |
+
impact_frame_data = world_landmarks[impact_frame]
|
| 670 |
+
if (impact_frame_data and len(impact_frame_data) > 24 and
|
| 671 |
+
impact_frame_data[23] and impact_frame_data[24]):
|
| 672 |
+
lx, lz = impact_frame_data[23][0], impact_frame_data[23][2]
|
| 673 |
+
rx, rz = impact_frame_data[24][0], impact_frame_data[24][2]
|
| 674 |
+
impact_pelvis_len = math.sqrt((rx-lx)**2 + (rz-lz)**2)
|
| 675 |
+
|
| 676 |
+
avg_setup = np.mean(setup_pelvis_lens)
|
| 677 |
+
if avg_setup > 0:
|
| 678 |
+
pelvis_change_pct = abs(impact_pelvis_len - avg_setup) / avg_setup * 100
|
| 679 |
+
|
| 680 |
+
# Score components (higher = better)
|
| 681 |
+
yaw_score = max(0, 1.0 - yaw_std / 5.0) # Good if <5° std
|
| 682 |
+
pelvis_score = max(0, 1.0 - pelvis_change_pct / 15.0) # Good if <15% change
|
| 683 |
+
z_score = max(0, 1.0 - z_std / 0.05) # Good if <5cm Z std
|
| 684 |
+
|
| 685 |
+
# Combined confidence
|
| 686 |
+
conf_3d = (yaw_score * 0.4 + pelvis_score * 0.4 + z_score * 0.2)
|
| 687 |
+
|
| 688 |
+
if pelvis_change_pct > 15.0:
|
| 689 |
+
dbg(f"3D_conf - pelvis_change={pelvis_change_pct:.1f}% suggests occlusion")
|
| 690 |
+
|
| 691 |
+
return max(0.0, min(1.0, conf_3d)), pelvis_change_pct
|
| 692 |
+
|
| 693 |
+
except Exception:
|
| 694 |
+
return 0.3, 100.0
|
| 695 |
+
|
| 696 |
+
def _compute_2d_confidence(pose_data, swing_phases, setup_frames):
|
| 697 |
+
"""
|
| 698 |
+
Compute 2D confidence based on homography quality and geometric consistency
|
| 699 |
+
Returns confidence score 0.0-1.0
|
| 700 |
+
"""
|
| 701 |
+
try:
|
| 702 |
+
sm = smooth_landmarks(pose_data)
|
| 703 |
+
H, valid_H, reason = _build_ground_homography(sm, setup_frames)
|
| 704 |
+
|
| 705 |
+
if not valid_H:
|
| 706 |
+
return 0.0
|
| 707 |
+
|
| 708 |
+
# Check target/toe angle consistency
|
| 709 |
+
target_vecs_2d = []
|
| 710 |
+
toe_vecs_2d = []
|
| 711 |
+
|
| 712 |
+
for f in setup_frames[:3]:
|
| 713 |
+
lm = sm.get(f)
|
| 714 |
+
if lm and len(lm) > 32 and lm[31] and lm[32]:
|
| 715 |
+
# Toe vector in ground plane
|
| 716 |
+
toe_img = [[lm[31][0], lm[31][1]], [lm[32][0], lm[32][1]]]
|
| 717 |
+
try:
|
| 718 |
+
import cv2
|
| 719 |
+
toe_ground = cv2.perspectiveTransform(np.array(toe_img, dtype=np.float32).reshape(1,-1,2), H)[0]
|
| 720 |
+
toe_vec = toe_ground[1] - toe_ground[0]
|
| 721 |
+
norm = np.linalg.norm(toe_vec)
|
| 722 |
+
if norm > 1e-6:
|
| 723 |
+
toe_vecs_2d.append(toe_vec / norm)
|
| 724 |
+
# Target perpendicular to toe
|
| 725 |
+
target_vec = np.array([-toe_vec[1], toe_vec[0]]) / norm
|
| 726 |
+
target_vecs_2d.append(target_vec)
|
| 727 |
+
except:
|
| 728 |
+
continue
|
| 729 |
+
|
| 730 |
+
if not target_vecs_2d:
|
| 731 |
+
return 0.2
|
| 732 |
+
|
| 733 |
+
# Check consistency of target direction
|
| 734 |
+
target_consistency = 1.0 - np.std([math.degrees(math.atan2(tv[1], tv[0])) for tv in target_vecs_2d]) / 10.0
|
| 735 |
+
|
| 736 |
+
# Check toe-target perpendicularity
|
| 737 |
+
toe_target_angles = []
|
| 738 |
+
for toe_vec, target_vec in zip(toe_vecs_2d, target_vecs_2d):
|
| 739 |
+
angle = abs(math.degrees(math.acos(np.clip(np.dot(toe_vec, target_vec), -1, 1))))
|
| 740 |
+
toe_target_angles.append(abs(angle - 90.0))
|
| 741 |
+
|
| 742 |
+
perp_score = max(0, 1.0 - np.mean(toe_target_angles) / 5.0) # Good if within 5° of 90°
|
| 743 |
+
|
| 744 |
+
# Homography quality score (det and error already checked in valid_H)
|
| 745 |
+
h_score = 0.8 if valid_H else 0.0
|
| 746 |
+
|
| 747 |
+
conf_2d = (target_consistency * 0.3 + perp_score * 0.4 + h_score * 0.3)
|
| 748 |
+
|
| 749 |
+
|
| 750 |
+
return max(0.0, min(1.0, conf_2d))
|
| 751 |
+
|
| 752 |
+
except Exception:
|
| 753 |
+
return 0.0
|
| 754 |
+
|
| 755 |
+
def calculate_hip_turn_at_impact(
|
| 756 |
+
pose_data: Dict[int, List],
|
| 757 |
+
swing_phases: Dict[str, List],
|
| 758 |
+
world_landmarks: Optional[Dict[int, List]] = None,
|
| 759 |
+
frames: Optional[List[np.ndarray]] = None
|
| 760 |
+
) -> Union[float, None]:
|
| 761 |
+
"""
|
| 762 |
+
Hip turn @ impact: TARGET-RELATIVE degrees (positive = open to target).
|
| 763 |
+
Fixed to use target line reference instead of camera-relative angles.
|
| 764 |
+
"""
|
| 765 |
+
PLAUSIBLE = 70.0
|
| 766 |
+
|
| 767 |
+
if not world_landmarks:
|
| 768 |
+
# Fallback: calculate from 2D pose data only
|
| 769 |
+
return _calculate_hip_turn_2d_fallback(pose_data, swing_phases, _impact_frame(pose_data, swing_phases))
|
| 770 |
+
|
| 771 |
+
# Get setup frames for target axis and baseline
|
| 772 |
+
setup_frames = swing_phases.get("setup", [])[:10]
|
| 773 |
+
if not setup_frames:
|
| 774 |
+
# Fallback: use first available frames as "setup"
|
| 775 |
+
all_frames = sorted(world_landmarks.keys())
|
| 776 |
+
if all_frames:
|
| 777 |
+
setup_frames = all_frames[:min(5, len(all_frames))]
|
| 778 |
+
else:
|
| 779 |
+
return _calculate_hip_turn_2d_fallback(pose_data, swing_phases, _impact_frame(pose_data, swing_phases))
|
| 780 |
+
|
| 781 |
+
# Build stable target axis from toe line
|
| 782 |
+
target_hat = _target_hat_from_toes_world(world_landmarks, setup_frames, handedness='right')
|
| 783 |
+
|
| 784 |
+
# Impact frame detection
|
| 785 |
+
impact_f = _impact_frame(world_landmarks, swing_phases)
|
| 786 |
+
if impact_f is None or impact_f not in world_landmarks:
|
| 787 |
+
# Fallback: use 2D calculation
|
| 788 |
+
return _calculate_hip_turn_2d_fallback(pose_data, swing_phases, _impact_frame(pose_data, swing_phases))
|
| 789 |
+
|
| 790 |
+
# Get pelvis direction at impact
|
| 791 |
+
impact_pelvis_hat = pelvis_hat(world_landmarks[impact_f])
|
| 792 |
+
|
| 793 |
+
if impact_pelvis_hat is None:
|
| 794 |
+
# Fallback: use 2D calculation
|
| 795 |
+
return _calculate_hip_turn_2d_fallback(pose_data, swing_phases, impact_f)
|
| 796 |
+
|
| 797 |
+
# ±1 frame median for stability at impact
|
| 798 |
+
impact_frames_nearby = [f for f in range(impact_f-1, impact_f+2) if f in world_landmarks]
|
| 799 |
+
if len(impact_frames_nearby) > 1:
|
| 800 |
+
impact_rels = []
|
| 801 |
+
for f in impact_frames_nearby:
|
| 802 |
+
ph = pelvis_hat(world_landmarks[f])
|
| 803 |
+
if ph is not None:
|
| 804 |
+
impact_rels.append(signed_angle2(target_hat, ph))
|
| 805 |
+
if impact_rels:
|
| 806 |
+
impact_rel = np.median(impact_rels)
|
| 807 |
+
else:
|
| 808 |
+
impact_rel = signed_angle2(target_hat, impact_pelvis_hat)
|
| 809 |
+
else:
|
| 810 |
+
impact_rel = signed_angle2(target_hat, impact_pelvis_hat)
|
| 811 |
+
|
| 812 |
+
# Hip turn = absolute open angle vs target at impact (not delta from setup)
|
| 813 |
+
hip_turn_abs = wrap180(impact_rel)
|
| 814 |
+
|
| 815 |
+
# 3D confidence check
|
| 816 |
+
conf_3d, pelvis_change_pct = _conf3d(world_landmarks, impact_f, setup_frames)
|
| 817 |
+
|
| 818 |
+
# Use 3D if high confidence and stable pelvis
|
| 819 |
+
if conf_3d >= 0.75 and pelvis_change_pct <= 15.0:
|
| 820 |
+
final_turn = hip_turn_abs
|
| 821 |
+
else:
|
| 822 |
+
# Try 2D fallback
|
| 823 |
+
turn_2d = _calculate_hip_turn_2d_fallback(pose_data, swing_phases, impact_f)
|
| 824 |
+
if turn_2d is not None:
|
| 825 |
+
_, valid_H, _ = _build_ground_homography(smooth_landmarks(pose_data), setup_frames)
|
| 826 |
+
if valid_H and abs(hip_turn_abs - turn_2d) <= 20.0:
|
| 827 |
+
final_turn = turn_2d
|
| 828 |
+
dbg(f"hip_turn - using 2D fallback: {turn_2d:.1f}°")
|
| 829 |
+
else:
|
| 830 |
+
final_turn = hip_turn_abs
|
| 831 |
+
else:
|
| 832 |
+
final_turn = hip_turn_abs
|
| 833 |
+
if conf_3d < 0.75 or pelvis_change_pct > 15.0:
|
| 834 |
+
dbg(f"hip_turn - 3D unreliable (conf={conf_3d:.3f}, pelvis_change={pelvis_change_pct:.1f}%)")
|
| 835 |
+
|
| 836 |
+
# Plausible check - but ALWAYS return something
|
| 837 |
+
if abs(final_turn) > PLAUSIBLE:
|
| 838 |
+
if conf_3d < 0.8:
|
| 839 |
+
dbg(f"hip_turn - VALUE {final_turn:.1f}° EXCEEDS PLAUSIBLE RANGE, clamping")
|
| 840 |
+
# Clamp to reasonable range rather than returning None
|
| 841 |
+
final_turn = PLAUSIBLE if final_turn > 0 else -PLAUSIBLE
|
| 842 |
+
return float(final_turn)
|
| 843 |
+
|
| 844 |
+
|
| 845 |
+
def _build_ground_homography(sm, setup_frames):
|
| 846 |
+
"""Build ground-plane homography using RANSAC"""
|
| 847 |
+
try:
|
| 848 |
+
import cv2
|
| 849 |
+
|
| 850 |
+
# Find best setup frame with all required landmarks
|
| 851 |
+
best_frame = None
|
| 852 |
+
for f in setup_frames:
|
| 853 |
+
lm = sm.get(f)
|
| 854 |
+
if lm and len(lm) > 31:
|
| 855 |
+
# Check we have feet landmarks + hips for ground plane
|
| 856 |
+
if (lm[27] and lm[28] and lm[31] and lm[32] and lm[23] and lm[24] and
|
| 857 |
+
all(lm[i][2] > 0.5 for i in [27,28,31,32,23,24])):
|
| 858 |
+
best_frame = f
|
| 859 |
+
break
|
| 860 |
+
|
| 861 |
+
if best_frame is None:
|
| 862 |
+
return None, False, "no_landmarks"
|
| 863 |
+
|
| 864 |
+
lm = sm[best_frame]
|
| 865 |
+
|
| 866 |
+
# 4 ground points: heel tips + ball center + back marker
|
| 867 |
+
img_pts = np.array([
|
| 868 |
+
[lm[31][0], lm[31][1]], # L heel tip
|
| 869 |
+
[lm[32][0], lm[32][1]], # R heel tip
|
| 870 |
+
[(lm[31][0] + lm[32][0]) / 2, min(lm[31][1], lm[32][1]) + 40], # Ball center (estimated)
|
| 871 |
+
[(lm[23][0] + lm[24][0]) / 2, (lm[23][1] + lm[24][1]) / 2 + 20] # Back marker (hip depth)
|
| 872 |
+
], dtype=np.float32)
|
| 873 |
+
|
| 874 |
+
# Ground coordinates (inches)
|
| 875 |
+
ground_pts = np.array([
|
| 876 |
+
[-10, 0], [10, 0], [0, 30], [0, -8]
|
| 877 |
+
], dtype=np.float32)
|
| 878 |
+
|
| 879 |
+
# RANSAC homography fitting
|
| 880 |
+
H, mask = cv2.findHomography(img_pts, ground_pts, cv2.RANSAC,
|
| 881 |
+
ransacReprojThreshold=2.0)
|
| 882 |
+
|
| 883 |
+
if H is None:
|
| 884 |
+
return None, False, "ransac_failed"
|
| 885 |
+
|
| 886 |
+
|
| 887 |
+
# Validation criteria - only check for non-singular matrix
|
| 888 |
+
det_H = abs(np.linalg.det(H[:2,:2]))
|
| 889 |
+
valid_H = det_H > 1e-4
|
| 890 |
+
|
| 891 |
+
reason = "ok" if valid_H else "singular"
|
| 892 |
+
|
| 893 |
+
return H, valid_H, reason
|
| 894 |
+
|
| 895 |
+
except Exception as e:
|
| 896 |
+
return None, False, "exception"
|
| 897 |
+
|
| 898 |
+
def _calculate_hip_turn_2d_fallback(pose_data: Dict[int, List], swing_phases: Dict[str, List], impact_frame: int) -> Optional[float]:
|
| 899 |
+
"""
|
| 900 |
+
TRUE 2D ground-plane fallback using homography
|
| 901 |
+
Only used if homography is valid and consistent
|
| 902 |
+
"""
|
| 903 |
+
sm = smooth_landmarks(pose_data)
|
| 904 |
+
setup_frames = swing_phases.get("setup", [])[:5]
|
| 905 |
+
if not setup_frames:
|
| 906 |
+
# Use first available frames as setup
|
| 907 |
+
all_frames = sorted(pose_data.keys())
|
| 908 |
+
if all_frames:
|
| 909 |
+
setup_frames = all_frames[:min(5, len(all_frames))]
|
| 910 |
+
else:
|
| 911 |
+
return None
|
| 912 |
+
|
| 913 |
+
# Handle None impact frame
|
| 914 |
+
if impact_frame is None:
|
| 915 |
+
# Use last available frame as impact
|
| 916 |
+
all_frames = sorted(pose_data.keys())
|
| 917 |
+
if all_frames:
|
| 918 |
+
impact_frame = all_frames[-1]
|
| 919 |
+
else:
|
| 920 |
+
return None
|
| 921 |
+
|
| 922 |
+
# Build ground-plane homography
|
| 923 |
+
H, valid_H, reason = _build_ground_homography(sm, setup_frames)
|
| 924 |
+
if not valid_H:
|
| 925 |
+
print(f"DEBUG: 2D_fallback - INVALID HOMOGRAPHY: {reason}")
|
| 926 |
+
return None
|
| 927 |
+
|
| 928 |
+
try:
|
| 929 |
+
import cv2
|
| 930 |
+
|
| 931 |
+
def warp_pts(pts_img):
|
| 932 |
+
"""Project image points to ground plane"""
|
| 933 |
+
pts_array = np.array(pts_img, dtype=np.float32).reshape(1, -1, 2)
|
| 934 |
+
ground_pts = cv2.perspectiveTransform(pts_array, H)[0]
|
| 935 |
+
return ground_pts
|
| 936 |
+
|
| 937 |
+
# Get ground-plane toe direction for target axis
|
| 938 |
+
toe_vecs_ground = []
|
| 939 |
+
for f in setup_frames:
|
| 940 |
+
lm = sm.get(f)
|
| 941 |
+
if lm and len(lm) > 32 and lm[31] and lm[32]:
|
| 942 |
+
toe_img = [[lm[31][0], lm[31][1]], [lm[32][0], lm[32][1]]]
|
| 943 |
+
toe_ground = warp_pts(toe_img)
|
| 944 |
+
toe_vec = toe_ground[1] - toe_ground[0] # Right - Left
|
| 945 |
+
norm = np.linalg.norm(toe_vec)
|
| 946 |
+
if norm > 1e-6:
|
| 947 |
+
toe_vecs_ground.append(toe_vec / norm)
|
| 948 |
+
|
| 949 |
+
if not toe_vecs_ground:
|
| 950 |
+
return None
|
| 951 |
+
|
| 952 |
+
toe_hat_2d = np.median(toe_vecs_ground, axis=0)
|
| 953 |
+
# Target is parallel to toe line (not perpendicular)
|
| 954 |
+
target_hat_2d = toe_hat_2d
|
| 955 |
+
|
| 956 |
+
# Get pelvis directions in ground plane
|
| 957 |
+
def pelvis_hat_ground(frame_idx):
|
| 958 |
+
lm = sm.get(frame_idx)
|
| 959 |
+
if not lm or len(lm) <= 24 or not lm[23] or not lm[24]:
|
| 960 |
+
return None
|
| 961 |
+
hip_img = [[lm[23][0], lm[23][1]], [lm[24][0], lm[24][1]]]
|
| 962 |
+
hip_ground = warp_pts(hip_img)
|
| 963 |
+
pelvis_vec = hip_ground[1] - hip_ground[0] # Right - Left
|
| 964 |
+
norm = np.linalg.norm(pelvis_vec)
|
| 965 |
+
return pelvis_vec / norm if norm > 1e-6 else None
|
| 966 |
+
|
| 967 |
+
# Impact pelvis direction
|
| 968 |
+
impact_pelvis_hat = pelvis_hat_ground(impact_frame)
|
| 969 |
+
|
| 970 |
+
if impact_pelvis_hat is None:
|
| 971 |
+
return None
|
| 972 |
+
|
| 973 |
+
# Absolute target-relative angle at impact (not delta from setup)
|
| 974 |
+
impact_rel = signed_angle2(target_hat_2d, impact_pelvis_hat)
|
| 975 |
+
turn_2d = wrap180(impact_rel)
|
| 976 |
+
|
| 977 |
+
|
| 978 |
+
return float(turn_2d)
|
| 979 |
+
|
| 980 |
+
except Exception:
|
| 981 |
+
return None
|
| 982 |
+
|
| 983 |
+
|
| 984 |
+
|
| 985 |
+
|
| 986 |
+
|
| 987 |
+
|
| 988 |
+
|
| 989 |
+
|
| 990 |
+
def calculate_hip_sway_at_top(pose_data: Dict[int, List], swing_phases: Dict[str, List],
|
| 991 |
+
world_landmarks: Optional[Dict[int, List]] = None) -> Union[float, None]:
|
| 992 |
+
"""Calculate hip sway at top using 2D homography projection along toe/target axis"""
|
| 993 |
+
smoothed_pose_data = smooth_landmarks(pose_data)
|
| 994 |
+
|
| 995 |
+
setup_frames = _address_frames(swing_phases)
|
| 996 |
+
if not setup_frames:
|
| 997 |
+
return None
|
| 998 |
+
|
| 999 |
+
# Find top frame
|
| 1000 |
+
backswing_frames = swing_phases.get("backswing", [])
|
| 1001 |
+
if not backswing_frames:
|
| 1002 |
+
return None
|
| 1003 |
+
|
| 1004 |
+
top_frame = _pick_top_frame_hinge(smoothed_pose_data, backswing_frames, handed='right')
|
| 1005 |
+
if top_frame is None:
|
| 1006 |
+
top_frame = backswing_frames[-1]
|
| 1007 |
+
|
| 1008 |
+
# Use 2D homography as primary method (world landmarks can re-center per frame)
|
| 1009 |
+
H, valid_H, _ = _build_ground_homography(smoothed_pose_data, setup_frames)
|
| 1010 |
+
if not valid_H:
|
| 1011 |
+
# ALWAYS DISPLAY SOMETHING - fallback to simple pixel measurement if homography fails
|
| 1012 |
+
return _simple_pixel_sway_fallback(smoothed_pose_data, setup_frames, top_frame)
|
| 1013 |
+
|
| 1014 |
+
try:
|
| 1015 |
+
import cv2
|
| 1016 |
+
|
| 1017 |
+
def pelvis_center_ground(frame_idx):
|
| 1018 |
+
lm = smoothed_pose_data.get(frame_idx)
|
| 1019 |
+
if not lm or not lm[23] or not lm[24]:
|
| 1020 |
+
return None
|
| 1021 |
+
hip_img = np.array([[lm[23][0], lm[23][1]], [lm[24][0], lm[24][1]]], dtype=np.float32)
|
| 1022 |
+
g = cv2.perspectiveTransform(hip_img.reshape(1,-1,2), H)[0]
|
| 1023 |
+
return (g[0] + g[1]) / 2
|
| 1024 |
+
|
| 1025 |
+
# Toe direction (ground) = target axis
|
| 1026 |
+
toe_vecs = []
|
| 1027 |
+
for f in setup_frames:
|
| 1028 |
+
lm = smoothed_pose_data.get(f)
|
| 1029 |
+
if lm and lm[31] and lm[32]:
|
| 1030 |
+
toe_img = np.array([[lm[31][0], lm[31][1]], [lm[32][0], lm[32][1]]], dtype=np.float32)
|
| 1031 |
+
toe_g = cv2.perspectiveTransform(toe_img.reshape(1,-1,2), H)[0]
|
| 1032 |
+
v = toe_g[1] - toe_g[0]
|
| 1033 |
+
n = np.linalg.norm(v)
|
| 1034 |
+
if n > 1e-6:
|
| 1035 |
+
toe_vecs.append(v/n)
|
| 1036 |
+
|
| 1037 |
+
if not toe_vecs:
|
| 1038 |
+
return None
|
| 1039 |
+
|
| 1040 |
+
target_hat_g = np.median(toe_vecs, axis=0) # unit 2D vector in ground coords
|
| 1041 |
+
|
| 1042 |
+
# Setup center (median)
|
| 1043 |
+
setup_centers = [pelvis_center_ground(f) for f in setup_frames]
|
| 1044 |
+
setup_centers = [c for c in setup_centers if c is not None]
|
| 1045 |
+
if not setup_centers:
|
| 1046 |
+
return None
|
| 1047 |
+
setup_ctr = np.median(setup_centers, axis=0)
|
| 1048 |
+
|
| 1049 |
+
# Top center
|
| 1050 |
+
top_ctr = pelvis_center_ground(top_frame)
|
| 1051 |
+
if top_ctr is None:
|
| 1052 |
+
return None
|
| 1053 |
+
|
| 1054 |
+
# Project movement onto toe/target axis
|
| 1055 |
+
delta = top_ctr - setup_ctr
|
| 1056 |
+
sway_inches = float(np.dot(delta, target_hat_g)) # ground_pts are in inches already
|
| 1057 |
+
return sway_inches
|
| 1058 |
+
|
| 1059 |
+
except Exception:
|
| 1060 |
+
# ALWAYS DISPLAY SOMETHING - fallback if homography projection fails
|
| 1061 |
+
return _simple_pixel_sway_fallback(smoothed_pose_data, setup_frames, top_frame)
|
| 1062 |
+
|
| 1063 |
+
|
| 1064 |
+
|
| 1065 |
+
|
| 1066 |
+
def angle_between(u, v):
|
| 1067 |
+
"""Calculate angle between two vectors in degrees (0-180)"""
|
| 1068 |
+
u, v = np.array(u, float), np.array(v, float)
|
| 1069 |
+
nu, nv = np.linalg.norm(u), np.linalg.norm(v)
|
| 1070 |
+
if nu == 0 or nv == 0:
|
| 1071 |
+
return float('nan')
|
| 1072 |
+
c = np.clip(np.dot(u, v) / (nu * nv), -1, 1)
|
| 1073 |
+
return math.degrees(math.acos(c)) # 0..180
|
| 1074 |
+
|
| 1075 |
+
|
| 1076 |
+
|
| 1077 |
+
def _hinge2d(lm, handed='right'):
|
| 1078 |
+
"""Simple 2D hinge angle: elbow-wrist vs wrist-index (robust, repeatable)"""
|
| 1079 |
+
if not lm:
|
| 1080 |
+
return None
|
| 1081 |
+
EL, WR = (13, 15) if handed=='right' else (14, 16) # Lead arm
|
| 1082 |
+
IM = 19 if handed=='right' else 20
|
| 1083 |
+
if not lm[EL] or not lm[WR] or not lm[IM]:
|
| 1084 |
+
return None
|
| 1085 |
+
|
| 1086 |
+
u = np.array([lm[WR][0]-lm[EL][0], lm[WR][1]-lm[EL][1]], float) # forearm
|
| 1087 |
+
v = np.array([lm[IM][0]-lm[WR][0], lm[IM][1]-lm[WR][1]], float) # hand
|
| 1088 |
+
nu, nv = np.linalg.norm(u), np.linalg.norm(v)
|
| 1089 |
+
if nu < 1e-6 or nv < 1e-6:
|
| 1090 |
+
return None
|
| 1091 |
+
|
| 1092 |
+
ang = math.degrees(math.acos(np.clip(np.dot(u,v)/(nu*nv), -1, 1))) # 0..180
|
| 1093 |
+
return 180.0 - ang # more hinge → larger value
|
| 1094 |
+
|
| 1095 |
+
|
| 1096 |
+
|
| 1097 |
+
|
| 1098 |
+
|
| 1099 |
+
|
| 1100 |
+
|
| 1101 |
+
def _pick_top_frame_hinge(sm, backswing_frames, handed='right'):
|
| 1102 |
+
"""Pick frame with maximum hinge in backswing"""
|
| 1103 |
+
if not backswing_frames:
|
| 1104 |
+
return None
|
| 1105 |
+
|
| 1106 |
+
# Top = max hinge in backswing
|
| 1107 |
+
top_vals = [(f, _hinge2d(sm.get(f), handed)) for f in backswing_frames]
|
| 1108 |
+
top_vals = [(f, h) for f, h in top_vals if h is not None]
|
| 1109 |
+
if not top_vals:
|
| 1110 |
+
return None
|
| 1111 |
+
|
| 1112 |
+
top_frame, hinge_top = max(top_vals, key=lambda x: x[1])
|
| 1113 |
+
return top_frame
|
| 1114 |
+
|
| 1115 |
+
|
| 1116 |
+
|
| 1117 |
+
|
| 1118 |
+
|
| 1119 |
+
|
| 1120 |
+
|
| 1121 |
+
|
| 1122 |
+
def calculate_wrist_hinge_at_top(pose_data: Dict[int, List], swing_phases: Dict[str, List],
|
| 1123 |
+
frames: Optional[List[np.ndarray]] = None) -> Union[Dict[str, float], None]:
|
| 1124 |
+
"""Calculate wrist hinge using simple 2D angle method"""
|
| 1125 |
+
sm = smooth_landmarks(pose_data)
|
| 1126 |
+
|
| 1127 |
+
backswing_frames = swing_phases.get("backswing", [])
|
| 1128 |
+
if not backswing_frames:
|
| 1129 |
+
# Fallback: use any available frames as "backswing"
|
| 1130 |
+
all_frames = sorted(pose_data.keys())
|
| 1131 |
+
if all_frames:
|
| 1132 |
+
# Use middle third of available frames as "backswing"
|
| 1133 |
+
mid_start = len(all_frames) // 3
|
| 1134 |
+
mid_end = 2 * len(all_frames) // 3
|
| 1135 |
+
backswing_frames = all_frames[mid_start:mid_end] or all_frames
|
| 1136 |
+
else:
|
| 1137 |
+
return None # Truly no data
|
| 1138 |
+
|
| 1139 |
+
# Top = max hinge in backswing
|
| 1140 |
+
top_frame = _pick_top_frame_hinge(sm, backswing_frames, handed='right')
|
| 1141 |
+
if top_frame is None:
|
| 1142 |
+
top_frame = backswing_frames[-1] # Use last frame as fallback
|
| 1143 |
+
|
| 1144 |
+
hinge_top = _hinge2d(sm.get(top_frame), handed='right')
|
| 1145 |
+
if hinge_top is None:
|
| 1146 |
+
# Try to find any frame with valid hinge data
|
| 1147 |
+
for frame_idx in backswing_frames:
|
| 1148 |
+
test_hinge = _hinge2d(sm.get(frame_idx), handed='right')
|
| 1149 |
+
if test_hinge is not None:
|
| 1150 |
+
hinge_top = test_hinge
|
| 1151 |
+
break
|
| 1152 |
+
if hinge_top is None:
|
| 1153 |
+
return None # Truly no wrist data available
|
| 1154 |
+
|
| 1155 |
+
# Address median for delta calculation
|
| 1156 |
+
setup_frames = _address_frames(swing_phases)
|
| 1157 |
+
addr_vals = [_hinge2d(sm.get(f), handed='right') for f in setup_frames[:5]]
|
| 1158 |
+
addr_vals = [h for h in addr_vals if h is not None]
|
| 1159 |
+
hinge_addr = np.median(addr_vals) if addr_vals else None
|
| 1160 |
+
|
| 1161 |
+
result = {"radial_deviation_deg": hinge_top}
|
| 1162 |
+
if hinge_addr is not None:
|
| 1163 |
+
delta = hinge_top - hinge_addr
|
| 1164 |
+
result["delta_deg"] = delta
|
| 1165 |
+
|
| 1166 |
+
# Quality check for typical range
|
| 1167 |
+
if not (15.0 <= delta <= 50.0):
|
| 1168 |
+
result["definition_suspect"] = True
|
| 1169 |
+
|
| 1170 |
+
return result
|
| 1171 |
+
|
| 1172 |
+
|
| 1173 |
+
def compute_front_facing_metrics(pose_data: Dict[int, List], swing_phases: Dict[str, List],
|
| 1174 |
+
world_landmarks: Optional[Dict[int, List]] = None,
|
| 1175 |
+
frames: Optional[List[np.ndarray]] = None) -> Dict[str, Dict[str, Union[float, None]]]:
|
| 1176 |
+
"""Compute the 4 required front-facing golf swing metrics"""
|
| 1177 |
+
|
| 1178 |
+
# Calculate individual metrics
|
| 1179 |
+
shoulder_tilt_impact = calculate_shoulder_tilt_at_impact(
|
| 1180 |
+
pose_data, swing_phases, world_landmarks, frames=frames
|
| 1181 |
+
)
|
| 1182 |
+
hip_turn_impact = calculate_hip_turn_at_impact(pose_data, swing_phases, world_landmarks, frames)
|
| 1183 |
+
hip_sway_top = calculate_hip_sway_at_top(pose_data, swing_phases, world_landmarks)
|
| 1184 |
+
wrist_hinge_result = calculate_wrist_hinge_at_top(pose_data, swing_phases, frames)
|
| 1185 |
+
|
| 1186 |
+
# Extract wrist hinge values
|
| 1187 |
+
wrist_radial_dev = wrist_hinge_result.get('radial_deviation_deg') if wrist_hinge_result else None
|
| 1188 |
+
wrist_hinge_delta = wrist_hinge_result.get('delta_deg') if wrist_hinge_result else None
|
| 1189 |
+
definition_suspect = wrist_hinge_result.get('definition_suspect', False) if wrist_hinge_result else False
|
| 1190 |
+
|
| 1191 |
+
# ALWAYS DISPLAY A METRIC - use delta first, then absolute as fallback
|
| 1192 |
+
wrist_final = wrist_hinge_delta
|
| 1193 |
+
if wrist_final is None and wrist_radial_dev is not None:
|
| 1194 |
+
wrist_final = wrist_radial_dev # Use absolute as fallback
|
| 1195 |
+
|
| 1196 |
+
front_facing_metrics = {
|
| 1197 |
+
'shoulder_tilt_impact_deg': {'value': shoulder_tilt_impact},
|
| 1198 |
+
'hip_turn_impact_deg': {'value': hip_turn_impact},
|
| 1199 |
+
'hip_sway_top_inches': {'value': hip_sway_top},
|
| 1200 |
+
'wrist_hinge_top_deg': {'value': wrist_final}
|
| 1201 |
+
}
|
| 1202 |
+
|
| 1203 |
+
return front_facing_metrics
|