Roll, Pitch, Yaw ROS right hand notation from Aruco marker rvec

edit

I'm trying to get the RPY of an Aruco marker from the camera view using the ROS notation. ROS axis notations are right hand, where positive x points north, y west and z upwards. I'm following this post http://answers.opencv.org/question/16... but I can't get it to work properly for ROS notation. This is my implementation:

def rpy_decomposition(self, rvec):

    R, _ = cv2.Rodrigues(rvec)

    sin_x = math.sqrt(R[2, 0] * R[2, 0] + R[2, 1] * R[2, 1])
    singular = sin_x < 1e-6
    if not singular:
        z1 = math.atan2(R[2, 0], R[2, 1])  # around z1-axis
        x = math.atan2(sin_x, R[2, 2])  # around x-axis
        z2 = math.atan2(R[0, 2], -R[1, 2])  # around z2-axis
    else:  # gimbal lock
        z1 = 0  # around z1-axis
        x = math.atan2(sin_x, R[2, 2])  # around x-axis
        z2 = 0  # around z2-axis

    z2 = -(2*math.pi -z2)%(2*math.pi)

    return z1, x, z2

I can't really find a working code in Python or C++. Thanks