1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130
| import cv2 import numpy as np from typing import Tuple
class HeadPoseEstimator: """ 头部姿态估计器 基于 solvePnP 算法 """ def __init__(self, camera_matrix: np.ndarray = None, dist_coeffs: np.ndarray = None): """ Args: camera_matrix: 相机内参矩阵 (3, 3) dist_coeffs: 畸变系数 """ if camera_matrix is None: self.camera_matrix = np.array([ [800, 0, 640], [0, 800, 400], [0, 0, 1] ], dtype=np.float64) else: self.camera_matrix = camera_matrix self.dist_coeffs = dist_coeffs if dist_coeffs is not None else np.zeros((4, 1)) self.model_points = np.array([ (0.0, 0.0, 0.0), (0.0, -330.0, -65.0), (-225.0, 170.0, -135.0), (225.0, 170.0, -135.0), (-150.0, -150.0, -125.0), (150.0, -150.0, -125.0) ], dtype=np.float64) self.landmark_indices = [30, 8, 36, 45, 48, 54] def estimate(self, landmarks: np.ndarray) -> Tuple[np.ndarray, np.ndarray, dict]: """ 估计头部姿态 Args: landmarks: (68, 2) 面部关键点 Returns: (rotation_vector, translation_vector, euler_angles) """ image_points = landmarks[self.landmark_indices].astype(np.float64) success, rotation_vector, translation_vector = cv2.solvePnP( self.model_points, image_points, self.camera_matrix, self.dist_coeffs, flags=cv2.SOLVEPNP_ITERATIVE ) if not success: return None, None, None rotation_matrix, _ = cv2.Rodrigues(rotation_vector) sy = np.sqrt(rotation_matrix[0, 0]**2 + rotation_matrix[1, 0]**2) if sy < 1e-6: pitch = np.arctan2(-rotation_matrix[1, 2], rotation_matrix[1, 1]) yaw = np.arctan2(-rotation_matrix[2, 0], sy) roll = 0 else: pitch = np.arctan2(rotation_matrix[2, 1], rotation_matrix[2, 2]) yaw = np.arctan2(-rotation_matrix[2, 0], sy) roll = np.arctan2(rotation_matrix[1, 0], rotation_matrix[0, 0]) euler_angles = { 'pitch': np.degrees(pitch), 'yaw': np.degrees(yaw), 'roll': np.degrees(roll) } return rotation_vector, translation_vector, euler_angles def is_looking_forward(self, euler_angles: dict, yaw_threshold: float = 30.0, pitch_threshold: float = 20.0) -> bool: """ 判断是否看向前方 Args: euler_angles: 欧拉角字典 yaw_threshold: 偏航角阈值(度) pitch_threshold: 俯仰角阈值(度) Returns: 是否看向前方 """ if euler_angles is None: return False yaw_ok = abs(euler_angles['yaw']) < yaw_threshold pitch_ok = abs(euler_angles['pitch']) < pitch_threshold return yaw_ok and pitch_ok
if __name__ == "__main__": estimator = HeadPoseEstimator() landmarks = np.random.randint(200, 400, (68, 2)) rot_vec, trans_vec, euler = estimator.estimate(landmarks) if euler: print(f"Pitch (俯仰): {euler['pitch']:.1f}°") print(f"Yaw (偏航): {euler['yaw']:.1f}°") print(f"Roll (翻滚): {euler['roll']:.1f}°") is_forward = estimator.is_looking_forward(euler) print(f"看向前方: {is_forward}")
|