Calculate head pose in euler angles using landmarks
retval, rvec, tvec = cv2.solvePnP(landmarks_3D,
landmarks_2D,
camera_matrix,
camera_distortion)
if not calibrated_fg:
bias_rvec = rvec
bias_rvec_matrix = cv2.Rodrigues(bias_rvec)[0]
calibrated_fg = True
raw_rvec_matrix = cv2.Rodrigues(rvec)[0]
rvec_matrix = np.dot(np.linalg.inv(bias_rvec_matrix),
raw_rvec_matrix)
head_pose = np.hstack((rvec_matrix, tvec))
R = np.vstack((head_pose, [0, 0, 0, 1]))
sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
singular = sy < 1e-6
if not singular:
x = math.atan2(R[2, 1], R[2, 2])
y = math.atan2(-R[2, 0], sy)
z = math.atan2(R[1, 0], R[0, 0])
else:
x = math.atan2(-R[1, 2], R[1, 1])
y = math.atan2(-R[2, 0], sy)
z = 0
euler_angles = np.array([x, y, z]) / np.pi * 180
roll_predicted = euler_angles[0]
pitch_predicted = euler_angles[1]
yaw_predicted = euler_angles[2]
print("Frame Index: %d Landmark-based Estimated "
"[roll, pitch, yaw] ..... [%f\t%f\t%f]\n" %
(frame_num, roll_predicted, pitch_predicted, yaw_predicted))
txt_out.write(str(frame_num) + '\t%f\t%f\t%f\n' % (
yaw_predicted, pitch_predicted, roll_predicted))