疲劳检测 （四） 基于landmark的姿态检测

1）定义一个具有n个关键点的3D脸部模型，以6个关键点的3D脸部模型为例（左眼角，右眼角，鼻尖，左嘴角，右嘴角，下颌）：

model_points = np.array([(0.0, 0.0, 0.0),             # Nose tip
                         (0.0, -330.0, -65.0),        # Chin
                         (-225.0, 170.0, -135.0),     # Left eye left corner
                         (225.0, 170.0, -135.0),      # Right eye right corne
                         (-150.0, -150.0, -125.0),    # Left Mouth corner
                         (150.0, -150.0, -125.0)]      # Right mouth corner

2）采用人脸检测以及面部关键点检测得到2D人脸关键点；

3）采用Opencv的solvePnP函数解出旋转向量；

K = [6.5308391993466671e+002, 0.0, 3.1950000000000000e+002,
     0.0, 6.5308391993466671e+002, 2.3950000000000000e+002,
     0.0, 0.0, 1.0]
D = [7.0834633684407095e-002, 6.9140193737175351e-002, 0.0, 0.0, -1.3073460323689292e+000]

# # 相机内参矩阵
camera_matrix = np.array(K).reshape(3, 3).astype(np.float32)

# # 相机畸变系数
dist_coeffs = np.array(D).reshape(5, 1).astype(np.float32)

(success, rotation_vector, translation_vector) = cv2.solvePnP(model_points, image_points, camera_matrix,dist_coeffs, flags=4)  # cv2.CV_ITERATIVE

4）将旋转向量转换为欧拉角；

# calculate rotation angles
theta = cv2.norm(rotation_vector, cv2.NORM_L2)

# transformed to quaterniond
w = math.cos(theta / 2)
x = math.sin(theta / 2) * rotation_vector[0][0] / theta
y = math.sin(theta / 2) * rotation_vector[1][0] / theta
z = math.sin(theta / 2) * rotation_vector[2][0] / theta

ysqr = y * y

# pitch (x-axis rotation)
t0 = 2.0 * (w * x + y * z)
t1 = 1.0 - 2.0 * (x * x + ysqr)
pitch = math.atan2(t0, t1)

# yaw (y-axis rotation)
t2 = 2.0 * (w * y - z * x)
if t2 > 1.0:
    t2 = 1.0
if t2 < -1.0:
    t2 = -1.0
yaw = math.asin(t2)

# roll (z-axis rotation)
t3 = 2.0 * (w * z + x * y)
t4 = 1.0 - 2.0 * (ysqr + z * z)
roll = math.atan2(t3, t4)

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

# 单位转换：将弧度转换为度
# Y = int((pitch / math.pi) * 180)
# X = int((yaw / math.pi) * 180)
# Z = int((roll / math.pi) * 180)

https://blog.csdn.net/u014090429/article/details/100762308

https://blog.csdn.net/Taily_Duan/article/details/52946501  （C++：计算旋转角度/人脸转正）