posturepy.py

import math
import cv2 as cv
import numpy as np
import logging as lg

lg.basicConfig(level=lg.WARNING, format='[%(levelname)s] %(message)s')


REAL_IRIS_DIAMETER_MM = 11.7

def euclidean(p1, p2):
    return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)

def calculate_focal_length_px_using_eyes_iris(image, distance_from_camera_in_mm, face_mesh_result):

    focal_length_px = -1
    confidence = "low"
    annotated_image = image.copy()
    h, w = image.shape[:2]

    if face_mesh_result.multi_face_landmarks:
        face = face_mesh_result.multi_face_landmarks[0]
        left_iris_lms = [face.landmark[i] for i in [474, 476]]
        right_iris_lms = [face.landmark[i] for i in [469, 471]]

        left_pts = [(int(lm.x * w), int(lm.y * h)) for lm in left_iris_lms]
        right_pts = [(int(lm.x * w), int(lm.y * h)) for lm in right_iris_lms]

        left_diameter = euclidean(*left_pts)
        right_diameter = euclidean(*right_pts)

        if left_diameter > 0 and right_diameter > 0:
            avg_pixel_diameter = (left_diameter + right_diameter) / 2
            focal_length_px = (distance_from_camera_in_mm * avg_pixel_diameter) / REAL_IRIS_DIAMETER_MM

            avg_diameter = (left_diameter + right_diameter) / 2
            rel_diff = abs(left_diameter - right_diameter) / avg_diameter

            if rel_diff < 0.13:
                confidence = "high"
            elif rel_diff < 0.55:
                confidence = "medium"
            else:
                confidence = "low"

            cv.line(annotated_image, left_pts[0], left_pts[1], (0, 255, 0), 2)
            cv.circle(annotated_image, left_pts[0], 3, (255, 0, 0), -1)
            cv.circle(annotated_image, left_pts[1], 3, (255, 0, 0), -1)
            cv.line(annotated_image, right_pts[0], right_pts[1], (0, 255, 0), 2)
            cv.circle(annotated_image, right_pts[0], 3, (255, 0, 0), -1)
            cv.circle(annotated_image, right_pts[1], 3, (255, 0, 0), -1)

            cv.putText(annotated_image, f"Confidence: {confidence}", (30, 50),
                       cv.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
        else:
            lg.warning(f'Invalid iris diameter.\nLeft: {left_diameter:.2f} px, Right: {right_diameter:.2f} px')
    else:
        lg.warning('Could not detect eyes — try looking at the camera.')

    return focal_length_px, annotated_image, confidence

def estimate_distance_from_iris(image, focal_length_px, face_mesh_result):
    distance_mm = -1
    left_diameter_px = -1
    right_diameter_px = -1
    confidence = "low"
    annotated_image = image.copy()
    h, w = image.shape[:2]

    if face_mesh_result.multi_face_landmarks:
        face = face_mesh_result.multi_face_landmarks[0]
        right_pts = [(int(face.landmark[i].x * w), int(face.landmark[i].y * h)) for i in [474, 476]]
        left_pts  = [(int(face.landmark[i].x * w), int(face.landmark[i].y * h)) for i in [469, 471]]

        right_diameter_px = euclidean(*right_pts)
        left_diameter_px  = euclidean(*left_pts)

        if right_diameter_px >= 2 and left_diameter_px >= 2:
            avg_diameter = (right_diameter_px + left_diameter_px) / 2
            distance_mm = (REAL_IRIS_DIAMETER_MM * focal_length_px) / avg_diameter

            # Resolution-independent confidence check
            rel_diff = abs(left_diameter_px - right_diameter_px) / avg_diameter
            if rel_diff < 0.13:
                confidence = "high"
            elif rel_diff < 0.55:
                confidence = "medium"
            else:
                confidence = "low"

            # Visuals
            cv.line(annotated_image, right_pts[0], right_pts[1], (0, 255, 0), 2)
            cv.line(annotated_image, left_pts[0], left_pts[1], (0, 255, 0), 2)
            cv.putText(annotated_image, f"Distance: {distance_mm:.1f} mm", (30, 50),
                       cv.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2)
            cv.putText(annotated_image, f"Confidence: {confidence}", (30, 80),
                       cv.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
        else:
            lg.warning(f"Iris diameters too small: Left={left_diameter_px:.2f}, Right={right_diameter_px:.2f}")
    else:
        lg.warning("No face detected in estimate_distance_from_iris.")

    return distance_mm, annotated_image, left_diameter_px, right_diameter_px, confidence

def estimate_distance_from_shoulders(image, focal_length_px, real_shoulder_width_mm, pose_result):
    distance_mm = -1
    confidence = "low"
    annotated_image = image.copy()
    h, w = image.shape[:2]

    if pose_result.pose_landmarks:
        landmarks = pose_result.pose_landmarks.landmark
        l_shoulder = landmarks[11]
        r_shoulder = landmarks[12]

        if l_shoulder.visibility > 0.5 and r_shoulder.visibility > 0.5:
            p1 = (int(l_shoulder.x * w), int(l_shoulder.y * h))
            p2 = (int(r_shoulder.x * w), int(r_shoulder.y * h))
            pixel_shoulder_width = euclidean(p1, p2)

            dx = abs(l_shoulder.x - r_shoulder.x)
            dy = abs(l_shoulder.y - r_shoulder.y)
            angle = math.degrees(math.atan2(dy, dx))  # tilt angle

            if pixel_shoulder_width / w > 0.015:
                z_diff = abs(l_shoulder.z - r_shoulder.z)

                # 🔹 Evaluate confidence
                if z_diff < 0.25 or angle < 5:
                    confidence = "high"
                elif z_diff < 0.4 or angle < 15:
                    confidence = "medium"
                else:
                    confidence = "low"

                # 🔹 Estimate distance
                distance_mm = (real_shoulder_width_mm * focal_length_px) / pixel_shoulder_width

                # 🔹 Draw
                cv.line(annotated_image, p1, p2, (0, 255, 0), 2)
                cv.putText(annotated_image, f"Shoulder Distance: {distance_mm:.1f} mm", (30, 50),
                           cv.FONT_HERSHEY_SIMPLEX, 0.8, (0, 0, 255), 2)
                cv.putText(annotated_image, f"Confidence: {confidence}", (30, 80),
                           cv.FONT_HERSHEY_SIMPLEX, 0.7, (255, 0, 0), 2)
            else:
                lg.warning("Shoulder width too small.")
        else:
            lg.warning("One or both shoulders not visible.")
    else:
        lg.warning("No pose detected.")

    return distance_mm, annotated_image, confidence

def extract_features(image, face_mesh, pose, focal_length_px, keypoints, shoulders_width):
    """
Extracts a comprehensive set of features from a single image using MediaPipe FaceMesh and Pose with segmentation.

The extracted features include:
- Pose keypoints (x, y) normalized relative to the chest midpoint and their visibility scores
- Chin position normalized to the chest midpoint, and Euclidean distance from chin to chest
- Head-to-camera distance estimated using iris-based triangulation, with associated confidence level
- Shoulder-to-camera distance estimated using known real shoulder width and focal length, with confidence level
- Segmentation ratio (proportion of the subject’s mask area in the frame)
- Segmented image with blurred background based on pose segmentation mask

Parameters
----------
image : np.ndarray
    The input image in BGR format.
face_mesh : mediapipe.solutions.face_mesh.FaceMesh
    Initialized MediaPipe FaceMesh instance.
pose : mediapipe.solutions.pose.Pose
    Initialized MediaPipe Pose instance with enable_segmentation=True.
focal_length_px : float
    Calibrated focal length of the camera in pixels.
keypoints : List[int]
    Pose landmark indices to extract for normalized coordinates and visibility.
shoulders_width : float
    Real shoulder width of the subject in millimeters.

Returns
-------
features : dict
    Dictionary of all extracted numeric features and confidence labels.
head_annotated : np.ndarray
    Copy of the original image annotated with iris landmarks and distance text.
shoulder_annotated : np.ndarray
    Copy of the original image annotated with shoulder landmarks and distance text.
segmented_image : np.ndarray
    Copy of the original image with background blurred using the segmentation mask.
"""

    segmented_image = image.copy()

    h, w = image.shape[:2]
    features = {}
    rgb = cv.cvtColor(image, cv.COLOR_BGR2RGB)


    face_result = face_mesh.process(rgb)
    pose_result = pose.process(rgb)

    # Pose Keypoints
    chest = None
    if pose_result.pose_landmarks:
        landmarks = pose_result.pose_landmarks.landmark
        l_sh = landmarks[11]
        r_sh = landmarks[12]

        cx = (l_sh.x + r_sh.x) / 2
        cy = (l_sh.y + r_sh.y) / 2
        chest = (cx, cy)

        for idx in keypoints:
            kp = landmarks[idx]
            features[f'x{idx}'] = kp.x - cx
            features[f'y{idx}'] = kp.y - cy
            features[f'v{idx}'] = kp.visibility
    else:
        lg.warning("Pose landmarks not detected.")

    # Chin from FaceMesh
    if face_result.multi_face_landmarks and chest:
        chin = face_result.multi_face_landmarks[0].landmark[152]
        dx = chin.x - chest[0]
        dy = chin.y - chest[1]
        features['chin_x'] = dx
        features['chin_y'] = dy
        features['chin_to_chest_dist'] = math.sqrt(dx**2 + dy**2)
    else:
        if not face_result.multi_face_landmarks:
            lg.warning('Failed to detect face landmarks to add chin coordinates , in extract_features.')
        if not chest:
            lg.warning('Chest center not computed (missing shoulders) in extract_features.')
        features['chin_x'] = -1
        features['chin_y'] = -1
        features['chin_to_chest_dist'] = -1

    # Head Distance
    head_dist, head_annotated, _, _, confidence = estimate_distance_from_iris(image, focal_length_px, face_result)
    if head_dist == -1:
        lg.warning("Failed to estimate head distance from iris in extract_features.")
    features['head_distance_mm'] = head_dist
    features['head_distance_confidence'] = confidence

    # Shoulder Distance
    shoulder_dist, shoulder_annotated, shoulder_confidence = estimate_distance_from_shoulders(
        image, focal_length_px, shoulders_width, pose_result
    )
    if shoulder_dist == -1:
        lg.warning("Failed to estimate shoulder distance.")
    features['shoulder_distance_mm'] = shoulder_dist
    features['shoulder_distance_confidence'] = shoulder_confidence
    features['shoulder_width'] = shoulders_width

    # Segmentation Ratio (from pose result)
    try:
        mask = pose_result.segmentation_mask
        binary_mask = (mask > 0.5).astype(np.uint8)
        segmentation_ratio = np.sum(binary_mask) / binary_mask.size
        features['segmentation_ratio'] = segmentation_ratio
        # Create blurred background segmented image
        condition = mask > 0.5
        blurred = cv.GaussianBlur(image, (55, 55), 0)
        segmented_image = np.where(condition[..., None], image, blurred)

    except Exception as e:
        lg.warning(f"Failed to compute segmentation ratio: {e}")
        features['segmentation_ratio'] = -1
        segmented_image = None


    return features, head_annotated, shoulder_annotated , segmented_image