"""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") # Outer eye corners (landmarks 33 and 263) are model points 2 and 3. eye_distance = math.hypot(image_points[3][0] - image_points[2][0], image_points[3][1] - image_points[2][1]) 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, eye_distance_px=eye_distance) def result_from_angles( self, angles: tuple[float, float, float], reprojection_error: float | None = None, eye_distance_px: float | None = None, ) -> HeadPoseEstimation: """Validate calculated angles and construct a normalized result. When ``max_reprojection_error_ratio`` is set, a pose whose reprojection error exceeds that multiple of ``eye_distance_px`` is rejected: it is a degenerate solution (typically a very small or leaving face), not a noisy measurement. """ normalized = self.normalize_angles(angles) ratio = self.config.max_reprojection_error_ratio if (ratio > 0.0 and reprojection_error is not None and eye_distance_px is not None and eye_distance_px > 0.0 and reprojection_error > ratio * eye_distance_px): return HeadPoseEstimation(False, *normalized, reprojection_error=reprojection_error, failure_reason="reprojection_error_too_large") 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) @staticmethod def facial_matrix_to_euler(matrix) -> tuple[float, float, float]: import cv2 import numpy as np m = np.asarray(matrix, dtype=np.float64).reshape(4, 4) rotation = m[:3, :3] rotation = rotation / np.linalg.norm(rotation, axis=0) # スケール成分を除去 angles = cv2.RQDecomp3x3(rotation)[0] return float(angles[0]), float(angles[1]), float(angles[2]) def estimate_from_matrix(self, matrix) -> HeadPoseEstimation: if not self.config.enabled: return HeadPoseEstimation(False, failure_reason="head_pose_disabled") try: angles = self.facial_matrix_to_euler(matrix) except Exception: return HeadPoseEstimation(False, failure_reason="facial_matrix_failed") return self.result_from_angles(angles) # 再投影誤差は無いので棄却判定はスキップされる