3

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?

Steve Osborne
  • 680
  • 4
  • 12

2 Answers2

3

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 enter image description here 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.

aliyasineser
  • 186
  • 3
  • 15
1

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.

Chungzuwalla
  • 1,038
  • 6
  • 17