Cacher is the code snippet organizer for pro developers

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))