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

asked 2019-02-06 08:40:35 -0500

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

edit retag flag offensive close merge delete