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/161369/retrieve-yaw-pitch-roll-from-rvec/ 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