Relative Rotation Between Pose (rvec)

I want to find the relative rotation angles between two Aruco markers, using python and cv2. I'm referring to my markers as the 'test' marker and the 'reference' marker. I have su

Solution 1:

I am not sure if I understand right. But unlike you, while I was getting reference I was using basic vector math: AB = AC - BC A and B are the markers, C is the camera. so if I invert reference tvec&rvec, add to first markers, I got the relative point. To that, I wrote two functions:

def inversePerspective(rvec, tvec):
""" Applies perspective transform for given rvec and tvec. """
    R, _ = cv2.Rodrigues(rvec)
    R = np.matrix(R).T
    invTvec = np.dot(R, np.matrix(-tvec))
    invRvec, _ = cv2.Rodrigues(R)
    return invRvec, invTvec



def relativePosition(rvec1, tvec1, rvec2, tvec2):
""" Get relative position for rvec2 & tvec2. Compose the returned rvec & tvec to use composeRT with rvec2 & tvec2 """
    rvec1, tvec1 = rvec1.reshape((3, 1)), tvec1.reshape((3, 1))
    rvec2, tvec2 = rvec2.reshape((3, 1)), tvec2.reshape((3, 1))

    # Inverse the second marker, the right one in the image
    invRvec, invTvec = inversePerspective(rvec2, tvec2)

    info = cv2.composeRT(rvec1, tvec1, invRvec, invTvec)
    composedRvec, composedTvec = info[0], info[1]

    composedRvec = composedRvec.reshape((3, 1))
    composedTvec = composedTvec.reshape((3, 1))
    return composedRvec, composedTvec

I basically used to refer a point. I was using one of the markers rotation in that case but maybe it can help you. I actually wrote it in my blog but it is in turkish. Sorry , I didn't have time to write an english one: My blog post

And also you can see my source code for that, I hope it can help you: My source code.

Sorry if I misunderstand your problem in there. I didn't test my code for accurate angle reference, I tested it for the translation.

Solution 2:

I doubt you should be using cv2.decomposeProjectionMatrix() on R_test_to_ref, because it is a 3x3 rotation matrix, not a projection matrix. One reference for converting a 3x3 rotation matrix to Euler angles in Python is here, code copied below:

# Code from https://www.learnopencv.com/rotation-matrix-to-euler-angles/
# Calculates rotation matrix to euler angles
# The result is the same as MATLAB except the order
# of the euler angles ( x and z are swapped ).
def rotationMatrixToEulerAngles(R) :

    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

    return np.array([x, y, z])

What is your use for the Euler angles? You don't need Euler angles for OpenGL, for example, you can use matrices directly. Euler angles have their problems and in many cases you will have more luck with a rotation vector or quaternion as a compact representation of the rotation.