rvec_to_yawpitchroll.py

import cv2
import numpy as np


def rvec_to_yawpitchroll(rvec):
    """
    Convert rotation vector to yaw, pitch, roll
    Args:
        rvec: pose rotation vector (rvec can be obtained by
        [rotation_x, rotation_y, rotation_z] in head_pose_tacker_poses.csv)

    Returns:
        yaw, pitch, roll in degrees
    """

    rvec = np.asarray(rvec, dtype=np.float64)
    rmat = cv2.Rodrigues(rvec)[0]

    sy = np.sqrt(rmat[0, 0] * rmat[0, 0] + rmat[1, 0] * rmat[1, 0])
    if sy < 1e-6:
        x = np.arctan2(-rmat[1, 2], rmat[1, 1])
        y = np.arctan2(-rmat[2, 0], sy)
        z = 0
    else:
        x = np.arctan2(rmat[2, 1], rmat[2, 2])
        y = np.arctan2(-rmat[2, 0], sy)
        z = np.arctan2(rmat[1, 0], rmat[0, 0])

    yaw, pitch, roll = np.rad2deg([y, x, z])
    return yaw, pitch, roll