133 lines
6.7 KiB
Python
133 lines
6.7 KiB
Python
"""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) # 再投影誤差は無いので棄却判定はスキップされる
|