kinemotion 0.17.0__py3-none-any.whl

kinemotion/dropjump/analysis.py

@@ -0,0 +1,790 @@
+"""Ground contact detection logic for drop-jump analysis."""
+
+from enum import Enum
+
+import numpy as np
+
+from ..core.smoothing import (
+    compute_acceleration_from_derivative,
+    compute_velocity_from_derivative,
+)
+
+
+class ContactState(Enum):
+    """States for foot-ground contact."""
+
+    IN_AIR = "in_air"
+    ON_GROUND = "on_ground"
+    UNKNOWN = "unknown"
+
+
+def calculate_adaptive_threshold(
+    positions: np.ndarray,
+    fps: float,
+    baseline_duration: float = 3.0,
+    multiplier: float = 1.5,
+    smoothing_window: int = 5,
+    polyorder: int = 2,
+) -> float:
+    """
+    Calculate adaptive velocity threshold based on baseline motion characteristics.
+
+    Analyzes the first few seconds of video (assumed to be relatively stationary,
+    e.g., athlete standing on box) to determine the noise floor, then sets threshold
+    as a multiple of this baseline noise.
+
+    This adapts to:
+    - Different camera distances (closer = more pixel movement)
+    - Different lighting conditions (affects tracking quality)
+    - Different frame rates (higher fps = smoother motion)
+    - Video compression artifacts
+
+    Args:
+        positions: Array of vertical positions (0-1 normalized)
+        fps: Video frame rate
+        baseline_duration: Duration in seconds to analyze for baseline (default: 3.0s)
+        multiplier: Factor above baseline noise to set threshold (default: 1.5x)
+        smoothing_window: Window size for velocity computation
+        polyorder: Polynomial order for Savitzky-Golay filter (default: 2)
+
+    Returns:
+        Adaptive velocity threshold value
+
+    Example:
+        At 30fps with 3s baseline:
+        - Analyzes first 90 frames
+        - Computes velocity for this "stationary" period
+        - 95th percentile velocity = 0.012 (noise level)
+        - Threshold = 0.012 × 1.5 = 0.018
+    """
+    if len(positions) < 2:
+        return 0.02  # Fallback to default
+
+    # Calculate number of frames for baseline analysis
+    baseline_frames = int(fps * baseline_duration)
+    baseline_frames = min(baseline_frames, len(positions))
+
+    if baseline_frames < smoothing_window:
+        return 0.02  # Not enough data, use default
+
+    # Extract baseline period (assumed relatively stationary)
+    baseline_positions = positions[:baseline_frames]
+
+    # Compute velocity for baseline period using derivative
+    baseline_velocities = compute_velocity_from_derivative(
+        baseline_positions, window_length=smoothing_window, polyorder=polyorder
+    )
+
+    # Calculate noise floor as 95th percentile of baseline velocities
+    # Using 95th percentile instead of max to be robust against outliers
+    noise_floor = float(np.percentile(np.abs(baseline_velocities), 95))
+
+    # Set threshold as multiplier of noise floor
+    # Minimum threshold to avoid being too sensitive
+    adaptive_threshold = max(noise_floor * multiplier, 0.005)
+
+    # Maximum threshold to ensure we still detect contact
+    adaptive_threshold = min(adaptive_threshold, 0.05)
+
+    return adaptive_threshold
+
+
+def _find_stable_baseline(
+    positions: np.ndarray,
+    min_stable_frames: int,
+    stability_threshold: float = 0.01,
+    debug: bool = False,
+) -> tuple[int, float]:
+    """Find first stable period and return baseline position.
+
+    Returns:
+        Tuple of (baseline_start_frame, baseline_position). Returns (-1, 0.0) if not found.
+    """
+    stable_window = min_stable_frames
+
+    for start_idx in range(0, len(positions) - stable_window, 5):
+        window = positions[start_idx : start_idx + stable_window]
+        window_std = float(np.std(window))
+
+        if window_std < stability_threshold:
+            baseline_start = start_idx
+            baseline_position = float(np.median(window))
+
+            if debug:
+                end_frame = baseline_start + stable_window - 1
+                print("[detect_drop_start] Found stable period:")
+                print(f"  frames {baseline_start}-{end_frame}")
+                print(f"  baseline_position: {baseline_position:.4f}")
+                print(f"  baseline_std: {window_std:.4f} < {stability_threshold:.4f}")
+
+            return baseline_start, baseline_position
+
+    if debug:
+        print(
+            f"[detect_drop_start] No stable period found "
+            f"(variance always > {stability_threshold:.4f})"
+        )
+    return -1, 0.0
+
+
+def _find_drop_from_baseline(
+    positions: np.ndarray,
+    baseline_start: int,
+    baseline_position: float,
+    stable_window: int,
+    position_change_threshold: float,
+    smoothing_window: int,
+    debug: bool = False,
+) -> int:
+    """Find drop start after stable baseline period.
+
+    Returns:
+        Drop frame index, or 0 if not found.
+    """
+    search_start = baseline_start + stable_window
+    window_size = max(3, smoothing_window)
+
+    for i in range(search_start, len(positions) - window_size):
+        window_positions = positions[i : i + window_size]
+        avg_position = float(np.mean(window_positions))
+        position_change = avg_position - baseline_position
+
+        if position_change > position_change_threshold:
+            drop_frame = max(baseline_start, i - window_size)
+
+            if debug:
+                print(f"[detect_drop_start] Drop detected at frame {drop_frame}")
+                print(
+                    f"  position_change: {position_change:.4f} > "
+                    f"{position_change_threshold:.4f}"
+                )
+                print(
+                    f"  avg_position: {avg_position:.4f} vs "
+                    f"baseline: {baseline_position:.4f}"
+                )
+
+            return drop_frame
+
+    if debug:
+        print("[detect_drop_start] No drop detected after stable period")
+    return 0
+
+
+def detect_drop_start(
+    positions: np.ndarray,
+    fps: float,
+    min_stationary_duration: float = 1.0,
+    position_change_threshold: float = 0.02,
+    smoothing_window: int = 5,
+    debug: bool = False,
+) -> int:
+    """
+    Detect when the drop jump actually starts by finding stable period then detecting drop.
+
+    Strategy:
+    1. Scan forward to find first STABLE period (low variance over N frames)
+    2. Use that stable period as baseline
+    3. Detect when position starts changing significantly from baseline
+
+    This handles videos where athlete steps onto box at start (unstable beginning).
+
+    Args:
+        positions: Array of vertical positions (0-1 normalized, y increases downward)
+        fps: Video frame rate
+        min_stationary_duration: Minimum duration (seconds) of stable period (default: 1.0s)
+        position_change_threshold: Position change indicating start of drop
+            (default: 0.02 = 2% of frame)
+        smoothing_window: Window for computing position variance
+        debug: Print debug information (default: False)
+
+    Returns:
+        Frame index where drop starts (or 0 if no clear stable period found)
+
+    Example:
+        - Frames 0-14: Stepping onto box (noisy, unstable)
+        - Frames 15-119: Standing on box (stable, low variance)
+        - Frame 119: Drop begins (position changes significantly)
+        - Returns: 119
+    """
+    min_stable_frames = int(fps * min_stationary_duration)
+    if len(positions) < min_stable_frames + 30:
+        if debug:
+            print(
+                f"[detect_drop_start] Video too short: {len(positions)} < "
+                f"{min_stable_frames + 30}"
+            )
+        return 0
+
+    # Find stable baseline period
+    baseline_start, baseline_position = _find_stable_baseline(
+        positions, min_stable_frames, debug=debug
+    )
+
+    if baseline_start < 0:
+        return 0
+
+    # Find drop from baseline
+    return _find_drop_from_baseline(
+        positions,
+        baseline_start,
+        baseline_position,
+        min_stable_frames,
+        position_change_threshold,
+        smoothing_window,
+        debug,
+    )
+
+
+def detect_ground_contact(
+    foot_positions: np.ndarray,
+    velocity_threshold: float = 0.02,
+    min_contact_frames: int = 3,
+    visibility_threshold: float = 0.5,
+    visibilities: np.ndarray | None = None,
+    window_length: int = 5,
+    polyorder: int = 2,
+) -> list[ContactState]:
+    """
+    Detect when feet are in contact with ground based on vertical motion.
+
+    Uses derivative-based velocity calculation via Savitzky-Golay filter for smooth,
+    accurate velocity estimates. This is consistent with the velocity calculation used
+    throughout the pipeline for sub-frame interpolation and curvature analysis.
+
+    Args:
+        foot_positions: Array of foot y-positions (normalized, 0-1, where 1 is bottom)
+        velocity_threshold: Threshold for vertical velocity to consider stationary
+        min_contact_frames: Minimum consecutive frames to confirm contact
+        visibility_threshold: Minimum visibility score to trust landmark
+        visibilities: Array of visibility scores for each frame
+        window_length: Window size for velocity derivative calculation (must be odd)
+        polyorder: Polynomial order for Savitzky-Golay filter (default: 2)
+
+    Returns:
+        List of ContactState for each frame
+    """
+    n_frames = len(foot_positions)
+    states = [ContactState.UNKNOWN] * n_frames
+
+    if n_frames < 2:
+        return states
+
+    # Compute vertical velocity using derivative-based method
+    # This provides smoother, more accurate velocity estimates than frame-to-frame differences
+    # and is consistent with the velocity calculation used for sub-frame interpolation
+    velocities = compute_velocity_from_derivative(
+        foot_positions, window_length=window_length, polyorder=polyorder
+    )
+
+    # Detect potential contact frames based on low velocity
+    is_stationary = np.abs(velocities) < velocity_threshold
+
+    # Apply visibility filter
+    if visibilities is not None:
+        is_visible = visibilities > visibility_threshold
+        is_stationary = is_stationary & is_visible
+
+    # Apply minimum contact duration filter
+    contact_frames = []
+    current_run = []
+
+    for i, stationary in enumerate(is_stationary):
+        if stationary:
+            current_run.append(i)
+        else:
+            if len(current_run) >= min_contact_frames:
+                contact_frames.extend(current_run)
+            current_run = []
+
+    # Don't forget the last run
+    if len(current_run) >= min_contact_frames:
+        contact_frames.extend(current_run)
+
+    # Set states
+    for i in range(n_frames):
+        if visibilities is not None and visibilities[i] < visibility_threshold:
+            states[i] = ContactState.UNKNOWN
+        elif i in contact_frames:
+            states[i] = ContactState.ON_GROUND
+        else:
+            states[i] = ContactState.IN_AIR
+
+    return states
+
+
+def find_contact_phases(
+    contact_states: list[ContactState],
+) -> list[tuple[int, int, ContactState]]:
+    """
+    Identify continuous phases of contact/flight.
+
+    Args:
+        contact_states: List of ContactState for each frame
+
+    Returns:
+        List of (start_frame, end_frame, state) tuples for each phase
+    """
+    phases: list[tuple[int, int, ContactState]] = []
+    if not contact_states:
+        return phases
+
+    current_state = contact_states[0]
+    phase_start = 0
+
+    for i in range(1, len(contact_states)):
+        if contact_states[i] != current_state:
+            # Phase transition
+            phases.append((phase_start, i - 1, current_state))
+            current_state = contact_states[i]
+            phase_start = i
+
+    # Don't forget the last phase
+    phases.append((phase_start, len(contact_states) - 1, current_state))
+
+    return phases
+
+
+def interpolate_threshold_crossing(
+    vel_before: float,
+    vel_after: float,
+    velocity_threshold: float,
+) -> float:
+    """
+    Find fractional offset where velocity crosses threshold between two frames.
+
+    Uses linear interpolation assuming velocity changes linearly between frames.
+
+    Args:
+        vel_before: Velocity at frame boundary N (absolute value)
+        vel_after: Velocity at frame boundary N+1 (absolute value)
+        velocity_threshold: Threshold value
+
+    Returns:
+        Fractional offset from frame N (0.0 to 1.0)
+    """
+    # Handle edge cases
+    if abs(vel_after - vel_before) < 1e-9:  # Velocity not changing
+        return 0.5
+
+    # Linear interpolation: at what fraction t does velocity equal threshold?
+    # vel(t) = vel_before + t * (vel_after - vel_before)
+    # Solve for t when vel(t) = threshold:
+    # threshold = vel_before + t * (vel_after - vel_before)
+    # t = (threshold - vel_before) / (vel_after - vel_before)
+
+    t = (velocity_threshold - vel_before) / (vel_after - vel_before)
+
+    # Clamp to [0, 1] range
+    return float(max(0.0, min(1.0, t)))
+
+
+def _interpolate_phase_start(
+    start_idx: int,
+    state: ContactState,
+    velocities: np.ndarray,
+    velocity_threshold: float,
+) -> float:
+    """Interpolate start boundary of a phase with sub-frame precision.
+
+    Returns:
+        Fractional start frame, or float(start_idx) if no interpolation.
+    """
+    if start_idx <= 0 or start_idx >= len(velocities):
+        return float(start_idx)
+
+    vel_before = velocities[start_idx - 1]
+    vel_at = velocities[start_idx]
+
+    # Check threshold crossing based on state
+    is_landing = (
+        state == ContactState.ON_GROUND and vel_before > velocity_threshold > vel_at
+    )
+    is_takeoff = (
+        state == ContactState.IN_AIR and vel_before < velocity_threshold < vel_at
+    )
+
+    if is_landing or is_takeoff:
+        offset = interpolate_threshold_crossing(vel_before, vel_at, velocity_threshold)
+        return (start_idx - 1) + offset
+
+    return float(start_idx)
+
+
+def _interpolate_phase_end(
+    end_idx: int,
+    state: ContactState,
+    velocities: np.ndarray,
+    velocity_threshold: float,
+    max_idx: int,
+) -> float:
+    """Interpolate end boundary of a phase with sub-frame precision.
+
+    Returns:
+        Fractional end frame, or float(end_idx) if no interpolation.
+    """
+    if end_idx >= max_idx - 1 or end_idx + 1 >= len(velocities):
+        return float(end_idx)
+
+    vel_at = velocities[end_idx]
+    vel_after = velocities[end_idx + 1]
+
+    # Check threshold crossing based on state
+    is_takeoff = (
+        state == ContactState.ON_GROUND and vel_at < velocity_threshold < vel_after
+    )
+    is_landing = (
+        state == ContactState.IN_AIR and vel_at > velocity_threshold > vel_after
+    )
+
+    if is_takeoff or is_landing:
+        offset = interpolate_threshold_crossing(vel_at, vel_after, velocity_threshold)
+        return end_idx + offset
+
+    return float(end_idx)
+
+
+def find_interpolated_phase_transitions(
+    foot_positions: np.ndarray,
+    contact_states: list[ContactState],
+    velocity_threshold: float,
+    smoothing_window: int = 5,
+) -> list[tuple[float, float, ContactState]]:
+    """
+    Find contact phases with sub-frame interpolation for precise timing.
+
+    Uses derivative-based velocity from smoothed trajectory for interpolation.
+    This provides much smoother velocity estimates than frame-to-frame differences,
+    leading to more accurate threshold crossing detection.
+
+    Args:
+        foot_positions: Array of foot y-positions (normalized, 0-1)
+        contact_states: List of ContactState for each frame
+        velocity_threshold: Threshold used for contact detection
+        smoothing_window: Window size for velocity smoothing (must be odd)
+
+    Returns:
+        List of (start_frame, end_frame, state) tuples with fractional frame indices
+    """
+    phases = find_contact_phases(contact_states)
+    if not phases or len(foot_positions) < 2:
+        return []
+
+    velocities = compute_velocity_from_derivative(
+        foot_positions, window_length=smoothing_window, polyorder=2
+    )
+
+    interpolated_phases: list[tuple[float, float, ContactState]] = []
+
+    for start_idx, end_idx, state in phases:
+        start_frac = _interpolate_phase_start(
+            start_idx, state, velocities, velocity_threshold
+        )
+        end_frac = _interpolate_phase_end(
+            end_idx, state, velocities, velocity_threshold, len(foot_positions)
+        )
+        interpolated_phases.append((start_frac, end_frac, state))
+
+    return interpolated_phases
+
+
+def refine_transition_with_curvature(
+    foot_positions: np.ndarray,
+    estimated_frame: float,
+    transition_type: str,
+    search_window: int = 3,
+    smoothing_window: int = 5,
+    polyorder: int = 2,
+) -> float:
+    """
+    Refine phase transition timing using trajectory curvature analysis.
+
+    Looks for characteristic acceleration patterns near estimated transition:
+    - Landing: Large acceleration spike (rapid deceleration on impact)
+    - Takeoff: Acceleration change (transition from static to upward motion)
+
+    Args:
+        foot_positions: Array of foot y-positions (normalized, 0-1)
+        estimated_frame: Initial estimate of transition frame (from velocity)
+        transition_type: Type of transition ("landing" or "takeoff")
+        search_window: Number of frames to search around estimate
+        smoothing_window: Window size for acceleration computation
+        polyorder: Polynomial order for Savitzky-Golay filter (default: 2)
+
+    Returns:
+        Refined fractional frame index
+    """
+    if len(foot_positions) < smoothing_window:
+        return estimated_frame
+
+    # Compute acceleration (second derivative)
+    acceleration = compute_acceleration_from_derivative(
+        foot_positions, window_length=smoothing_window, polyorder=polyorder
+    )
+
+    # Define search range around estimated transition
+    est_int = int(estimated_frame)
+    search_start = max(0, est_int - search_window)
+    search_end = min(len(acceleration), est_int + search_window + 1)
+
+    if search_end <= search_start:
+        return estimated_frame
+
+    # Extract acceleration in search window
+    accel_window = acceleration[search_start:search_end]
+
+    if len(accel_window) == 0:
+        return estimated_frame
+
+    if transition_type == "landing":
+        # Landing: Look for large magnitude acceleration (impact deceleration)
+        # Find frame with maximum absolute acceleration
+        peak_idx = np.argmax(np.abs(accel_window))
+        refined_frame = float(search_start + peak_idx)
+
+    elif transition_type == "takeoff":
+        # Takeoff: Look for acceleration magnitude change
+        # Find frame with large acceleration change (derivative of acceleration)
+        if len(accel_window) < 2:
+            return estimated_frame
+
+        accel_diff = np.abs(np.diff(accel_window))
+        peak_idx = np.argmax(accel_diff)
+        refined_frame = float(search_start + peak_idx)
+
+    else:
+        return estimated_frame
+
+    # Blend with original estimate (don't stray too far)
+    # 70% curvature-based, 30% velocity-based
+    blend_factor = 0.7
+    refined_frame = blend_factor * refined_frame + (1 - blend_factor) * estimated_frame
+
+    return refined_frame
+
+
+def find_interpolated_phase_transitions_with_curvature(
+    foot_positions: np.ndarray,
+    contact_states: list[ContactState],
+    velocity_threshold: float,
+    smoothing_window: int = 5,
+    polyorder: int = 2,
+    use_curvature: bool = True,
+) -> list[tuple[float, float, ContactState]]:
+    """
+    Find contact phases with sub-frame interpolation and curvature refinement.
+
+    Combines three methods for maximum accuracy:
+    1. Velocity thresholding (coarse integer frame detection)
+    2. Velocity interpolation (sub-frame precision)
+    3. Curvature analysis (refinement based on acceleration patterns)
+
+    Args:
+        foot_positions: Array of foot y-positions (normalized, 0-1)
+        contact_states: List of ContactState for each frame
+        velocity_threshold: Threshold used for contact detection
+        smoothing_window: Window size for velocity/acceleration smoothing
+        polyorder: Polynomial order for Savitzky-Golay filter (default: 2)
+        use_curvature: Whether to apply curvature-based refinement
+
+    Returns:
+        List of (start_frame, end_frame, state) tuples with fractional frame indices
+    """
+    # Get interpolated phases using velocity
+    interpolated_phases = find_interpolated_phase_transitions(
+        foot_positions, contact_states, velocity_threshold, smoothing_window
+    )
+
+    if not use_curvature or len(interpolated_phases) == 0:
+        return interpolated_phases
+
+    # Refine phase boundaries using curvature analysis
+    refined_phases: list[tuple[float, float, ContactState]] = []
+
+    for start_frac, end_frac, state in interpolated_phases:
+        refined_start = start_frac
+        refined_end = end_frac
+
+        if state == ContactState.ON_GROUND:
+            # Refine landing (start of ground contact)
+            refined_start = refine_transition_with_curvature(
+                foot_positions,
+                start_frac,
+                "landing",
+                search_window=3,
+                smoothing_window=smoothing_window,
+                polyorder=polyorder,
+            )
+            # Refine takeoff (end of ground contact)
+            refined_end = refine_transition_with_curvature(
+                foot_positions,
+                end_frac,
+                "takeoff",
+                search_window=3,
+                smoothing_window=smoothing_window,
+                polyorder=polyorder,
+            )
+
+        elif state == ContactState.IN_AIR:
+            # For flight phases, takeoff is at start, landing is at end
+            refined_start = refine_transition_with_curvature(
+                foot_positions,
+                start_frac,
+                "takeoff",
+                search_window=3,
+                smoothing_window=smoothing_window,
+                polyorder=polyorder,
+            )
+            refined_end = refine_transition_with_curvature(
+                foot_positions,
+                end_frac,
+                "landing",
+                search_window=3,
+                smoothing_window=smoothing_window,
+                polyorder=polyorder,
+            )
+
+        refined_phases.append((refined_start, refined_end, state))
+
+    return refined_phases
+
+
+def find_landing_from_acceleration(
+    positions: np.ndarray,
+    accelerations: np.ndarray,
+    takeoff_frame: int,
+    fps: float,
+    search_duration: float = 0.7,
+) -> int:
+    """
+    Find landing frame by detecting impact acceleration after takeoff.
+
+    Similar to CMJ landing detection, looks for maximum positive acceleration
+    (deceleration on ground impact) after the jump peak.
+
+    Args:
+        positions: Array of vertical positions (normalized 0-1)
+        accelerations: Array of accelerations (second derivative)
+        takeoff_frame: Frame at takeoff (end of ground contact)
+        fps: Video frame rate
+        search_duration: Duration in seconds to search for landing (default: 0.7s)
+
+    Returns:
+        Landing frame index (integer)
+    """
+    # Find peak height (minimum y value = highest point)
+    search_start = takeoff_frame
+    search_end = min(len(positions), takeoff_frame + int(fps * search_duration))
+
+    if search_end <= search_start:
+        return min(len(positions) - 1, takeoff_frame + int(fps * 0.3))
+
+    flight_positions = positions[search_start:search_end]
+    peak_idx = int(np.argmin(flight_positions))
+    peak_frame = search_start + peak_idx
+
+    # After peak, look for landing (impact with ground)
+    # Landing is detected by maximum positive acceleration (deceleration on impact)
+    landing_search_start = peak_frame + 2
+    landing_search_end = min(len(accelerations), landing_search_start + int(fps * 0.6))
+
+    if landing_search_end <= landing_search_start:
+        return min(len(positions) - 1, peak_frame + int(fps * 0.2))
+
+    # Find impact: maximum positive acceleration after peak
+    landing_accelerations = accelerations[landing_search_start:landing_search_end]
+    impact_idx = int(np.argmax(landing_accelerations))
+    impact_frame = landing_search_start + impact_idx
+
+    # After acceleration peak, look for position stabilization (full ground contact)
+    # Check where vertical position stops decreasing (athlete stops compressing)
+    stabilization_search_start = impact_frame
+    stabilization_search_end = min(len(positions), impact_frame + int(fps * 0.2))
+
+    landing_frame = impact_frame
+    if stabilization_search_end > stabilization_search_start + 3:
+        # Find where position reaches maximum (lowest point) and starts stabilizing
+        search_positions = positions[
+            stabilization_search_start:stabilization_search_end
+        ]
+
+        # Look for the frame where position reaches its maximum (deepest landing)
+        max_pos_idx = int(np.argmax(search_positions))
+
+        # Landing is just after max position (athlete at deepest landing compression)
+        landing_frame = stabilization_search_start + max_pos_idx
+        landing_frame = min(len(positions) - 1, landing_frame)
+
+    return landing_frame
+
+
+def compute_average_foot_position(
+    landmarks: dict[str, tuple[float, float, float]],
+) -> tuple[float, float]:
+    """
+    Compute average foot position from ankle and foot landmarks.
+
+    Args:
+        landmarks: Dictionary of landmark positions
+
+    Returns:
+        (x, y) average foot position in normalized coordinates
+    """
+    foot_keys = [
+        "left_ankle",
+        "right_ankle",
+        "left_heel",
+        "right_heel",
+        "left_foot_index",
+        "right_foot_index",
+    ]
+
+    x_positions = []
+    y_positions = []
+
+    for key in foot_keys:
+        if key in landmarks:
+            x, y, visibility = landmarks[key]
+            if visibility > 0.5:  # Only use visible landmarks
+                x_positions.append(x)
+                y_positions.append(y)
+
+    if not x_positions:
+        return (0.5, 0.5)  # Default to center if no visible feet
+
+    return (float(np.mean(x_positions)), float(np.mean(y_positions)))
+
+
+def extract_foot_positions_and_visibilities(
+    smoothed_landmarks: list[dict[str, tuple[float, float, float]] | None],
+) -> tuple[np.ndarray, np.ndarray]:
+    """
+    Extract vertical positions and average visibilities from smoothed landmarks.
+
+    This utility function eliminates code duplication between CLI and programmatic usage.
+
+    Args:
+        smoothed_landmarks: Smoothed landmark sequence from tracking
+
+    Returns:
+        Tuple of (vertical_positions, visibilities) as numpy arrays
+    """
+    position_list: list[float] = []
+    visibilities_list: list[float] = []
+
+    for frame_landmarks in smoothed_landmarks:
+        if frame_landmarks:
+            _, foot_y = compute_average_foot_position(frame_landmarks)
+            position_list.append(foot_y)
+
+            # Average visibility of foot landmarks
+            foot_vis = []
+            for key in ["left_ankle", "right_ankle", "left_heel", "right_heel"]:
+                if key in frame_landmarks:
+                    foot_vis.append(frame_landmarks[key][2])
+            visibilities_list.append(float(np.mean(foot_vis)) if foot_vis else 0.0)
+        else:
+            # Fill missing frames with last known position or default
+            position_list.append(position_list[-1] if position_list else 0.5)
+            visibilities_list.append(0.0)
+
+    return np.array(position_list), np.array(visibilities_list)