Phase 11

src/head_pose.py

@@ -0,0 +1,100 @@
+"""OpenCV solvePnP head-pose estimation with no MediaPipe dependency."""
+
+from __future__ import annotations
+
+import math
+
+from .config_loader import HeadPoseSettings
+from .models import FaceLandmark, HeadPoseEstimation
+
+
+class HeadPoseEstimator:
+    """Estimate approximate pitch, yaw, and roll from six facial landmarks."""
+
+    LANDMARK_INDICES = (1, 152, 33, 263, 61, 291)
+    # Arbitrary units: only relative shape matters. Y is positive downward to
+    # match OpenCV image coordinates; using a Y-up model can make a frontal
+    # face solve as an approximately 180-degree rotation.
+    MODEL_POINTS = (
+        (0.0, 0.0, 0.0), (0.0, 63.6, -12.5), (-43.3, -32.7, -26.0),
+        (43.3, -32.7, -26.0), (-28.9, 28.9, -24.1), (28.9, 28.9, -24.1),
+    )
+
+    def __init__(self, config: HeadPoseSettings) -> None:
+        self.config = config
+
+    def build_image_points(self, landmarks: list[FaceLandmark]) -> list[tuple[float, float]] | None:
+        by_index = {landmark.index: landmark for landmark in landmarks}
+        if any(index not in by_index for index in self.LANDMARK_INDICES):
+            return None
+        return [(by_index[index].x, by_index[index].y) for index in self.LANDMARK_INDICES]
+
+    @staticmethod
+    def build_camera_matrix(frame_width: int, frame_height: int) -> list[list[float]]:
+        focal = float(frame_width)
+        return [[focal, 0.0, frame_width / 2.0], [0.0, focal, frame_height / 2.0], [0.0, 0.0, 1.0]]
+
+    @staticmethod
+    def rotation_matrix_to_euler(rotation: list[list[float]]) -> tuple[float, float, float]:
+        """Return X/Y/Z Euler rotations as pitch/yaw/roll in degrees."""
+        sy = math.sqrt(rotation[0][0] ** 2 + rotation[1][0] ** 2)
+        singular = sy < 1e-6
+        if not singular:
+            pitch = math.atan2(rotation[2][1], rotation[2][2])
+            yaw = math.atan2(-rotation[2][0], sy)
+            roll = math.atan2(rotation[1][0], rotation[0][0])
+        else:
+            pitch = math.atan2(-rotation[1][2], rotation[1][1])
+            yaw = math.atan2(-rotation[2][0], sy)
+            roll = 0.0
+        return tuple(float(math.degrees(value)) for value in (pitch, yaw, roll))
+
+    def estimate(self, landmarks: list[FaceLandmark], frame_width: int, frame_height: int) -> HeadPoseEstimation:
+        if not self.config.enabled:
+            return HeadPoseEstimation(False, failure_reason="head_pose_disabled")
+        image_points = self.build_image_points(landmarks)
+        if image_points is None:
+            return HeadPoseEstimation(False, failure_reason="insufficient_landmarks")
+        try:
+            import cv2
+            import numpy as np
+
+            model = np.asarray(self.MODEL_POINTS, dtype=np.float64)
+            image = np.asarray(image_points, dtype=np.float64)
+            camera = np.asarray(self.build_camera_matrix(frame_width, frame_height), dtype=np.float64)
+            distortion = np.zeros((4, 1), dtype=np.float64)
+            success, rotation_vector, translation_vector = cv2.solvePnP(
+                model, image, camera, distortion, flags=cv2.SOLVEPNP_ITERATIVE,
+            )
+            if not success:
+                return HeadPoseEstimation(False, failure_reason="solvepnp_failed")
+            rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
+            angles = self.rotation_matrix_to_euler(rotation_matrix.tolist())
+            projected, _ = cv2.projectPoints(model, rotation_vector, translation_vector, camera, distortion)
+            error = float(np.sqrt(np.mean(np.sum((projected.reshape(-1, 2) - image) ** 2, axis=1))))
+        except Exception:
+            return HeadPoseEstimation(False, failure_reason="solvepnp_failed")
+        return self.result_from_angles(angles, error)
+
+    def result_from_angles(
+        self, angles: tuple[float, float, float], reprojection_error: float | None = None,
+    ) -> HeadPoseEstimation:
+        """Validate calculated angles and construct a normalized result."""
+        normalized = self.normalize_angles(angles)
+        if any(not math.isfinite(value) or abs(value) > self.config.max_abs_angle for value in normalized):
+            return HeadPoseEstimation(False, *normalized, reprojection_error=reprojection_error,
+                                      failure_reason="angle_out_of_range")
+        return HeadPoseEstimation(True, *normalized, reprojection_error=reprojection_error)
+
+    @staticmethod
+    def normalize_angles(angles: tuple[float, float, float]) -> tuple[float, float, float]:
+        """Canonicalize roll for upright subjects while preserving pitch and yaw.
+
+        A planar face can be returned by solvePnP with its in-plane axes reversed,
+        producing roll near +/-180 degrees for an upright frontal face. Roll is an
+        axial orientation here, so the equivalent value in [-90, 90) is used.
+        """
+        pitch, yaw, roll = angles
+        if math.isfinite(roll):
+            roll = (roll + 90.0) % 180.0 - 90.0
+        return float(pitch), float(yaw), float(roll)