Relative rotation between pose (rvec)

Question

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 successfully retrieved the pose of the markers using cv2.aruco.estimatePoseSingleMarkers. This gives me a "test_rvec" for the test marker and a "ref_rvec" for the reference marker.

As I understand it, rvec (same format as used by cv2.solvePnP, which I believe aruco uses under the covers) is the rotation of the marker relative to the camera. So, to get the rotation of the test marker relative to the reference marker, I do:

R_ref_to_cam = cv2.Rodrigues(ref_rvec)[0] #reference to camera
R_test_to_cam = cv2.Rodrigues(test_rvec)[0] #test to camera
R_cam_to_ref = np.transpose(R_ref_to_cam) #inverse of reference to camera
R_test_to_ref = np.matmul(R_test_to_cam,R_cam_to_ref) #test to reference

Then I use cv2.decomposeProjectionMatrix to compute the euler angles of the resulting matrix (R_test_to_ref).

In testing, with both markers flat on my desk and with the same orientation, with the camera pointed straight down, I get X=0, Y=0, Z=0 as expected, since the relative orientation between the markers is zero.

However, if I rotate one marker 90 degrees in the "z" direction (still keeping it flat on my desk), I get X=30, Y=30, Z=90. I would expect to see two of the axes report as 90 degrees and the third (rotational) axis report 0 degrees. What am I doing wrong?

Answer 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.