kinemotion 0.17.0__py3-none-any.whl

kinemotion/core/filtering.py

@@ -0,0 +1,345 @@
+"""Advanced filtering techniques for robust trajectory processing."""
+
+import numpy as np
+from scipy.signal import medfilt
+
+
+def detect_outliers_ransac(
+    positions: np.ndarray,
+    window_size: int = 15,
+    threshold: float = 0.02,
+    min_inliers: float = 0.7,
+) -> np.ndarray:
+    """
+    Detect outlier positions using RANSAC-based polynomial fitting.
+
+    Uses a sliding window approach to detect positions that deviate significantly
+    from a polynomial fit of nearby points. This catches MediaPipe tracking glitches
+    where landmarks jump to incorrect positions.
+
+    Args:
+        positions: 1D array of position values (e.g., y-coordinates)
+        window_size: Size of sliding window for local fitting
+        threshold: Distance threshold to consider a point an inlier
+        min_inliers: Minimum fraction of points that must be inliers
+
+    Returns:
+        Boolean array: True for outliers, False for valid points
+    """
+    n = len(positions)
+    is_outlier = np.zeros(n, dtype=bool)
+
+    if n < window_size:
+        return is_outlier
+
+    # Ensure window size is odd
+    if window_size % 2 == 0:
+        window_size += 1
+
+    half_window = window_size // 2
+
+    for i in range(n):
+        # Define window around current point
+        start = max(0, i - half_window)
+        end = min(n, i + half_window + 1)
+        window_positions = positions[start:end]
+        window_indices = np.arange(start, end)
+
+        if len(window_positions) < 3:
+            continue
+
+        # Fit polynomial (quadratic) to window
+        # Use polyfit with degree 2 (parabolic motion)
+        try:
+            coeffs = np.polyfit(window_indices, window_positions, deg=2)
+            predicted = np.polyval(coeffs, window_indices)
+
+            # Calculate residuals
+            residuals = np.abs(window_positions - predicted)
+
+            # Point is outlier if its residual is large
+            local_idx = i - start
+            if local_idx < len(residuals) and residuals[local_idx] > threshold:
+                # Also check if most other points are inliers (RANSAC criterion)
+                inliers = np.sum(residuals <= threshold)
+                if inliers / len(residuals) >= min_inliers:
+                    is_outlier[i] = True
+        except np.linalg.LinAlgError:
+            # Polyfit failed, skip this window
+            continue
+
+    return is_outlier
+
+
+def detect_outliers_median(
+    positions: np.ndarray, window_size: int = 5, threshold: float = 0.03
+) -> np.ndarray:
+    """
+    Detect outliers using median filtering.
+
+    Points that deviate significantly from the local median are marked as outliers.
+    More robust to noise than mean-based methods.
+
+    Args:
+        positions: 1D array of position values
+        window_size: Size of median filter window (must be odd)
+        threshold: Deviation threshold to mark as outlier
+
+    Returns:
+        Boolean array: True for outliers, False for valid points
+    """
+    if len(positions) < window_size:
+        return np.zeros(len(positions), dtype=bool)
+
+    # Ensure window size is odd
+    if window_size % 2 == 0:
+        window_size += 1
+
+    # Apply median filter
+    median_filtered = medfilt(positions, kernel_size=window_size)
+
+    # Calculate absolute deviation from median
+    deviations = np.abs(positions - median_filtered)
+
+    # Mark as outlier if deviation exceeds threshold
+    is_outlier = deviations > threshold
+
+    return is_outlier
+
+
+def remove_outliers(
+    positions: np.ndarray,
+    outlier_mask: np.ndarray,
+    method: str = "interpolate",
+) -> np.ndarray:
+    """
+    Replace outlier values with interpolated or median values.
+
+    Args:
+        positions: Original position array
+        outlier_mask: Boolean array indicating outliers
+        method: "interpolate" or "median"
+            - interpolate: Linear interpolation from neighboring valid points
+            - median: Replace with local median of valid points
+
+    Returns:
+        Position array with outliers replaced
+    """
+    positions_clean = positions.copy()
+
+    if not np.any(outlier_mask):
+        return positions_clean
+
+    outlier_indices = np.nonzero(outlier_mask)[0]
+
+    for idx in outlier_indices:
+        if method == "interpolate":
+            # Find nearest valid points before and after
+            valid_before = np.nonzero(~outlier_mask[:idx])[0]
+            valid_after = np.nonzero(~outlier_mask[idx + 1 :])[0]
+
+            if len(valid_before) > 0 and len(valid_after) > 0:
+                # Linear interpolation between nearest valid points
+                idx_before = valid_before[-1]
+                idx_after = valid_after[0] + idx + 1
+
+                # Interpolate
+                t = (idx - idx_before) / (idx_after - idx_before)
+                positions_clean[idx] = (
+                    positions[idx_before] * (1 - t) + positions[idx_after] * t
+                )
+            elif len(valid_before) > 0:
+                # Use last valid value
+                positions_clean[idx] = positions[valid_before[-1]]
+            elif len(valid_after) > 0:
+                # Use next valid value
+                positions_clean[idx] = positions[valid_after[0] + idx + 1]
+
+        elif method == "median":
+            # Replace with median of nearby valid points
+            window_size = 5
+            start = max(0, idx - window_size)
+            end = min(len(positions), idx + window_size + 1)
+
+            window_valid = ~outlier_mask[start:end]
+            if np.any(window_valid):
+                positions_clean[idx] = np.median(positions[start:end][window_valid])
+
+    return positions_clean
+
+
+def reject_outliers(
+    positions: np.ndarray,
+    use_ransac: bool = True,
+    use_median: bool = True,
+    ransac_window: int = 15,
+    ransac_threshold: float = 0.02,
+    median_window: int = 5,
+    median_threshold: float = 0.03,
+    interpolate: bool = True,
+) -> tuple[np.ndarray, np.ndarray]:
+    """
+    Comprehensive outlier rejection using multiple methods.
+
+    Combines RANSAC-based and median-based outlier detection for robust
+    identification of tracking glitches.
+
+    Args:
+        positions: 1D array of position values
+        use_ransac: Enable RANSAC-based outlier detection
+        use_median: Enable median-based outlier detection
+        ransac_window: Window size for RANSAC
+        ransac_threshold: Deviation threshold for RANSAC
+        median_window: Window size for median filter
+        median_threshold: Deviation threshold for median
+        interpolate: Replace outliers with interpolated values
+
+    Returns:
+        Tuple of (cleaned_positions, outlier_mask)
+        - cleaned_positions: Positions with outliers replaced
+        - outlier_mask: Boolean array marking outliers
+    """
+    outlier_mask = np.zeros(len(positions), dtype=bool)
+
+    # Detect outliers using RANSAC
+    if use_ransac:
+        ransac_outliers = detect_outliers_ransac(
+            positions, window_size=ransac_window, threshold=ransac_threshold
+        )
+        outlier_mask |= ransac_outliers
+
+    # Detect outliers using median filtering
+    if use_median:
+        median_outliers = detect_outliers_median(
+            positions, window_size=median_window, threshold=median_threshold
+        )
+        outlier_mask |= median_outliers
+
+    # Remove/replace outliers
+    if interpolate:
+        cleaned_positions = remove_outliers(
+            positions, outlier_mask, method="interpolate"
+        )
+    else:
+        cleaned_positions = positions.copy()
+
+    return cleaned_positions, outlier_mask
+
+
+def adaptive_smooth_window(
+    positions: np.ndarray,
+    base_window: int = 5,
+    velocity_threshold: float = 0.02,
+    min_window: int = 3,
+    max_window: int = 11,
+) -> np.ndarray:
+    """
+    Determine adaptive smoothing window size based on local motion velocity.
+
+    Uses larger windows during slow motion (ground contact) and smaller windows
+    during fast motion (flight) to preserve details where needed while smoothing
+    where safe.
+
+    Args:
+        positions: 1D array of position values
+        base_window: Base window size (default: 5)
+        velocity_threshold: Velocity below which to use larger window
+        min_window: Minimum window size (for fast motion)
+        max_window: Maximum window size (for slow motion)
+
+    Returns:
+        Array of window sizes for each frame
+    """
+    n = len(positions)
+    windows = np.full(n, base_window, dtype=int)
+
+    if n < 2:
+        return windows
+
+    # Compute local velocity (simple diff)
+    velocities = np.abs(np.diff(positions, prepend=positions[0]))
+
+    # Smooth velocity to avoid spurious changes
+    if n >= 5:
+        from scipy.signal import medfilt
+
+        velocities = medfilt(velocities, kernel_size=5)
+
+    # Assign window sizes based on velocity
+    for i in range(n):
+        if velocities[i] < velocity_threshold / 2:
+            # Very slow motion - use maximum window
+            windows[i] = max_window
+        elif velocities[i] < velocity_threshold:
+            # Slow motion - use larger window
+            windows[i] = (base_window + max_window) // 2
+        else:
+            # Fast motion - use smaller window
+            windows[i] = min_window
+
+    # Ensure windows are odd
+    windows = np.where(windows % 2 == 0, windows + 1, windows)
+
+    return windows
+
+
+def bilateral_temporal_filter(
+    positions: np.ndarray,
+    window_size: int = 9,
+    sigma_spatial: float = 3.0,
+    sigma_intensity: float = 0.02,
+) -> np.ndarray:
+    """
+    Apply bilateral filter in temporal domain for edge-preserving smoothing.
+
+    Unlike Savitzky-Golay which smooths uniformly across all frames, the bilateral
+    filter preserves sharp transitions (like landing/takeoff) while smoothing within
+    smooth regions (flight phase, ground contact).
+
+    The filter weights each neighbor by both:
+    1. Temporal distance (like regular smoothing)
+    2. Intensity similarity (preserves edges)
+
+    Args:
+        positions: 1D array of position values
+        window_size: Temporal window size (must be odd)
+        sigma_spatial: Std dev for spatial (temporal) Gaussian kernel
+        sigma_intensity: Std dev for intensity (position difference) kernel
+
+    Returns:
+        Filtered position array
+    """
+    n = len(positions)
+    filtered = np.zeros(n)
+
+    # Ensure window size is odd
+    if window_size % 2 == 0:
+        window_size += 1
+
+    half_window = window_size // 2
+
+    for i in range(n):
+        # Define window
+        start = max(0, i - half_window)
+        end = min(n, i + half_window + 1)
+
+        # Get window positions
+        window_pos = positions[start:end]
+        center_pos = positions[i]
+
+        # Compute spatial (temporal) weights
+        temporal_indices = np.arange(start - i, end - i)
+        spatial_weights = np.exp(-(temporal_indices**2) / (2 * sigma_spatial**2))
+
+        # Compute intensity (position difference) weights
+        intensity_diff = window_pos - center_pos
+        intensity_weights = np.exp(-(intensity_diff**2) / (2 * sigma_intensity**2))
+
+        # Combined weights (bilateral)
+        weights = spatial_weights * intensity_weights
+        weights /= np.sum(weights)  # Normalize
+
+        # Weighted average
+        filtered[i] = np.sum(weights * window_pos)
+
+    return filtered

kinemotion/core/pose.py

@@ -0,0 +1,259 @@
+"""Pose tracking using MediaPipe Pose."""
+
+import cv2
+import mediapipe as mp
+import numpy as np
+
+
+class PoseTracker:
+    """Tracks human pose landmarks in video frames using MediaPipe."""
+
+    def __init__(
+        self,
+        min_detection_confidence: float = 0.5,
+        min_tracking_confidence: float = 0.5,
+    ):
+        """
+        Initialize the pose tracker.
+
+        Args:
+            min_detection_confidence: Minimum confidence for pose detection
+            min_tracking_confidence: Minimum confidence for pose tracking
+        """
+        self.mp_pose = mp.solutions.pose
+        self.pose = self.mp_pose.Pose(
+            min_detection_confidence=min_detection_confidence,
+            min_tracking_confidence=min_tracking_confidence,
+            model_complexity=1,
+        )
+
+    def process_frame(
+        self, frame: np.ndarray
+    ) -> dict[str, tuple[float, float, float]] | None:
+        """
+        Process a single frame and extract pose landmarks.
+
+        Args:
+            frame: BGR image frame
+
+        Returns:
+            Dictionary mapping landmark names to (x, y, visibility) tuples,
+            or None if no pose detected. Coordinates are normalized (0-1).
+        """
+        # Convert BGR to RGB
+        rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+
+        # Process the frame
+        results = self.pose.process(rgb_frame)
+
+        if not results.pose_landmarks:
+            return None
+
+        # Extract key landmarks for feet tracking and CoM estimation
+        landmarks = {}
+        landmark_names = {
+            # Feet landmarks
+            self.mp_pose.PoseLandmark.LEFT_ANKLE: "left_ankle",
+            self.mp_pose.PoseLandmark.RIGHT_ANKLE: "right_ankle",
+            self.mp_pose.PoseLandmark.LEFT_HEEL: "left_heel",
+            self.mp_pose.PoseLandmark.RIGHT_HEEL: "right_heel",
+            self.mp_pose.PoseLandmark.LEFT_FOOT_INDEX: "left_foot_index",
+            self.mp_pose.PoseLandmark.RIGHT_FOOT_INDEX: "right_foot_index",
+            # Torso landmarks for CoM estimation
+            self.mp_pose.PoseLandmark.LEFT_HIP: "left_hip",
+            self.mp_pose.PoseLandmark.RIGHT_HIP: "right_hip",
+            self.mp_pose.PoseLandmark.LEFT_SHOULDER: "left_shoulder",
+            self.mp_pose.PoseLandmark.RIGHT_SHOULDER: "right_shoulder",
+            # Additional landmarks for better CoM estimation
+            self.mp_pose.PoseLandmark.NOSE: "nose",
+            self.mp_pose.PoseLandmark.LEFT_KNEE: "left_knee",
+            self.mp_pose.PoseLandmark.RIGHT_KNEE: "right_knee",
+        }
+
+        for landmark_id, name in landmark_names.items():
+            lm = results.pose_landmarks.landmark[landmark_id]
+            landmarks[name] = (lm.x, lm.y, lm.visibility)
+
+        return landmarks
+
+    def close(self) -> None:
+        """Release resources."""
+        self.pose.close()
+
+
+def _add_head_segment(
+    segments: list,
+    weights: list,
+    visibilities: list,
+    landmarks: dict[str, tuple[float, float, float]],
+    vis_threshold: float,
+) -> None:
+    """Add head segment (8% body mass) if visible."""
+    if "nose" in landmarks:
+        x, y, vis = landmarks["nose"]
+        if vis > vis_threshold:
+            segments.append((x, y))
+            weights.append(0.08)
+            visibilities.append(vis)
+
+
+def _add_trunk_segment(
+    segments: list,
+    weights: list,
+    visibilities: list,
+    landmarks: dict[str, tuple[float, float, float]],
+    vis_threshold: float,
+) -> None:
+    """Add trunk segment (50% body mass) if visible."""
+    trunk_keys = ["left_shoulder", "right_shoulder", "left_hip", "right_hip"]
+    trunk_pos = [
+        (x, y, vis)
+        for key in trunk_keys
+        if key in landmarks
+        for x, y, vis in [landmarks[key]]
+        if vis > vis_threshold
+    ]
+    if len(trunk_pos) >= 2:
+        trunk_x = float(np.mean([p[0] for p in trunk_pos]))
+        trunk_y = float(np.mean([p[1] for p in trunk_pos]))
+        trunk_vis = float(np.mean([p[2] for p in trunk_pos]))
+        segments.append((trunk_x, trunk_y))
+        weights.append(0.50)
+        visibilities.append(trunk_vis)
+
+
+def _add_limb_segment(
+    segments: list,
+    weights: list,
+    visibilities: list,
+    landmarks: dict[str, tuple[float, float, float]],
+    side: str,
+    proximal_key: str,
+    distal_key: str,
+    segment_weight: float,
+    vis_threshold: float,
+) -> None:
+    """Add a limb segment (thigh or lower leg) if both endpoints visible."""
+    prox_full = f"{side}_{proximal_key}"
+    dist_full = f"{side}_{distal_key}"
+
+    if prox_full in landmarks and dist_full in landmarks:
+        px, py, pvis = landmarks[prox_full]
+        dx, dy, dvis = landmarks[dist_full]
+        if pvis > vis_threshold and dvis > vis_threshold:
+            seg_x = (px + dx) / 2
+            seg_y = (py + dy) / 2
+            seg_vis = (pvis + dvis) / 2
+            segments.append((seg_x, seg_y))
+            weights.append(segment_weight)
+            visibilities.append(seg_vis)
+
+
+def _add_foot_segment(
+    segments: list,
+    weights: list,
+    visibilities: list,
+    landmarks: dict[str, tuple[float, float, float]],
+    side: str,
+    vis_threshold: float,
+) -> None:
+    """Add foot segment (1.5% body mass per foot) if visible."""
+    foot_keys = [f"{side}_ankle", f"{side}_heel", f"{side}_foot_index"]
+    foot_pos = [
+        (x, y, vis)
+        for key in foot_keys
+        if key in landmarks
+        for x, y, vis in [landmarks[key]]
+        if vis > vis_threshold
+    ]
+    if foot_pos:
+        foot_x = float(np.mean([p[0] for p in foot_pos]))
+        foot_y = float(np.mean([p[1] for p in foot_pos]))
+        foot_vis = float(np.mean([p[2] for p in foot_pos]))
+        segments.append((foot_x, foot_y))
+        weights.append(0.015)
+        visibilities.append(foot_vis)
+
+
+def compute_center_of_mass(
+    landmarks: dict[str, tuple[float, float, float]],
+    visibility_threshold: float = 0.5,
+) -> tuple[float, float, float]:
+    """
+    Compute approximate center of mass (CoM) from body landmarks.
+
+    Uses biomechanical segment weights based on Dempster's body segment parameters:
+    - Head: 8% of body mass (represented by nose)
+    - Trunk (shoulders to hips): 50% of body mass
+    - Thighs: 2 × 10% = 20% of body mass
+    - Legs (knees to ankles): 2 × 5% = 10% of body mass
+    - Feet: 2 × 1.5% = 3% of body mass
+
+    The CoM is estimated as a weighted average of these segments, with
+    weights corresponding to their proportion of total body mass.
+
+    Args:
+        landmarks: Dictionary of landmark positions (x, y, visibility)
+        visibility_threshold: Minimum visibility to include landmark in calculation
+
+    Returns:
+        (x, y, visibility) tuple for estimated CoM position
+        visibility = average visibility of all segments used
+    """
+    segments: list = []
+    weights: list = []
+    visibilities: list = []
+
+    # Add body segments
+    _add_head_segment(segments, weights, visibilities, landmarks, visibility_threshold)
+    _add_trunk_segment(segments, weights, visibilities, landmarks, visibility_threshold)
+
+    # Add bilateral limb segments
+    for side in ["left", "right"]:
+        _add_limb_segment(
+            segments,
+            weights,
+            visibilities,
+            landmarks,
+            side,
+            "hip",
+            "knee",
+            0.10,
+            visibility_threshold,
+        )
+        _add_limb_segment(
+            segments,
+            weights,
+            visibilities,
+            landmarks,
+            side,
+            "knee",
+            "ankle",
+            0.05,
+            visibility_threshold,
+        )
+        _add_foot_segment(
+            segments, weights, visibilities, landmarks, side, visibility_threshold
+        )
+
+    # Fallback if no segments found
+    if not segments:
+        if "left_hip" in landmarks and "right_hip" in landmarks:
+            lh_x, lh_y, lh_vis = landmarks["left_hip"]
+            rh_x, rh_y, rh_vis = landmarks["right_hip"]
+            return ((lh_x + rh_x) / 2, (lh_y + rh_y) / 2, (lh_vis + rh_vis) / 2)
+        return (0.5, 0.5, 0.0)
+
+    # Normalize weights and compute weighted average
+    total_weight = sum(weights)
+    normalized_weights = [w / total_weight for w in weights]
+
+    com_x = float(
+        sum(p[0] * w for p, w in zip(segments, normalized_weights, strict=True))
+    )
+    com_y = float(
+        sum(p[1] * w for p, w in zip(segments, normalized_weights, strict=True))
+    )
+    com_visibility = float(np.mean(visibilities)) if visibilities else 0.0
+
+    return (com_x, com_y, com_visibility)