def get_head_pose(shape): # 头部姿态估计

# （像素坐标集合）填写 2D 参考点

# 17 左眉左上角 /21 左眉右角 /22 右眉左上角 /26 右眉右上角 /36 左眼左上角 /39 左眼右上角 /42 右眼左上角 /

# 45 右眼右上角 /31 鼻子左上角 /35 鼻子右上角 /48 左上角 /54 嘴右上角 /57 嘴中央下角 /8 下巴角

image_pts = np.float32([shape[17], shape[21], shape[22], shape[26], shape[36],

shape[39], shape[42], shape[45], shape[31], shape[35],

shape[48], shape[54], shape[57], shape[8]])

# solvePnP 计算姿势——求解旋转和平移矩阵：

# rotation_vec 表示旋转矩阵， translation_vec 表示平移矩阵， cam_matrix 与 K 矩阵对应， dist_coeffs 与 D 矩阵对应。

_, rotation_vec, translation_vec = cv2.solvePnP(object_pts, image_pts, cam_matrix, dist_coeffs)

# projectPoints 重新投影误差：外汇跟单gendan5.com原 2d 点和重投影 2d 点的距离（输入 3d 点、相机内参、相机畸变、 r 、 t ，输出重投影 2d 点）

reprojectdst, _ = cv2.projectPoints(reprojectsrc, rotation_vec, translation_vec, cam_matrix, dist_coeffs)

reprojectdst = tuple(map(tuple, reprojectdst.reshape(8, 2))) # 以 8 行 2 列显示

# 计算欧拉角 calc euler angle

rotation_mat, _ = cv2.Rodrigues(rotation_vec) # 罗德里格斯公式（将旋转矩阵转换为旋转向量）

pose_mat = cv2.hconcat((rotation_mat, translation_vec)) # 水平拼接， vconcat 垂直拼接

# decomposeProjectionMatrix 将投影矩阵分解为旋转矩阵和相机矩阵

_, _, _, _, _, _, euler_angle = cv2.decomposeProjectionMatrix(pose_mat)

pitch, yaw, roll = [math.radians(_) for _ in euler_angle]

pitch = math.degrees(math.asin(math.sin(pitch)))

roll = -math.degrees(math.asin(math.sin(roll)))

yaw = math.degrees(math.asin(math.sin(yaw)))

print('pitch:{}, yaw:{}, roll:{}'.format(pitch, yaw, roll))

return reprojectdst, euler_angle # 投影误差，欧拉角

def eye_aspect_ratio(eye):

# 垂直眼标志（ X ， Y ）坐标

A = dist.euclidean(eye[1], eye[5]) # 计算两个集合之间的欧式距离

B = dist.euclidean(eye[2], eye[4])

# 计算水平之间的欧几里得距离

# 水平眼标志（ X ， Y ）坐标

C = dist.euclidean(eye[0], eye[3])

# 眼睛长宽比的计算

ear = (A + B) / (2.0 * C)

# 返回眼睛的长宽比

return ear

def mouth_aspect_ratio(mouth): # 嘴部

A = np.linalg.norm(mouth[2] - mouth[9]) # 51, 59

B = np.linalg.norm(mouth[4] - mouth[7]) # 53, 57

C = np.linalg.norm(mouth[0] - mouth[6]) # 49, 55

mar = (A + B) / (2.0 * C)

return mar