chenemii commited on
Commit
e195886
·
1 Parent(s): 91badaf

Add front facing metrics module

Browse files
Files changed (1) hide show
  1. 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