Move aruco detected point along marker axes

I’m trying to make a simple project of a cube with 6 different aruco markers on each side. The goal is to obtain and display a single translation and rotation vector pair which will always show the center of the cube.

For now I managed to rotate all the rvecs so each recognized aruco axes will match the common cube rotation, which works just fine. Modifying the tvecs turned out to be a bit harder. Lets focus on the top side marker.
I tried to subtract the offset vector from the tvec as following:

rvec, tvec, _ = aruco.estimatePoseSingleMarkers(...)
tvec = tvec - [0, -markerSideLength / 2, 0] # Moving -half cube down by Y axis

When looking to the side of the marker the offset looks almost fine (image 1), but when the marker is parallel to the camera plane (image 2) we can see the issue:

I quickly understood that the offset I’m subtracting is moving the tvec along the camera Y axis and not the marker Y axis. This could also be seen by that the marker Y axis should be green and not blue, as on previous screenshot (so I actually need to move it along blue Z).

So I need to move the tvec along the marker axis. I searched for a few days and tried to use the translation matrices and cv2.composeRT, but I was only able to get the same invalid or completely non-working result. I can see I need to somehow mix the marker rotation in to make this work, but I can’t understand how to do that.

Any ideas?

you should define an “aruco board” with the aruco API. a cube is also a “board”.

tvecs are relative to the camera, not the object. the rvec and tvec together represent a transformation from marker space to camera space.

in camera space, adding/subtracting translations makes no sense if you expect anything to happen relative to the object.

you should work with 4x4 matrices. then you can insert a translation before (ahead of) the rotation and translation from marker into camera space.

1 Like

Thanks! I remember I’ve seen the arucoBoard implementation for similar task somewhere, but haven’t decided to try it yet. Even though it should work, I will probably still try to make this barebones, have to dig into the 4x4 transformation matrices though.

I tried using this topic on stackoverflow, there’s a nice function which first does the translation and then rotation, just as you mentioned. But it actually did the same as the regular vector subtraction (or maybe I misunderstood how to use it) :frowning:

Btw I used SciPy Rotation to rotate rvec and it worked nicely. Sadly there’s no implementation for translation vector.

Ok, awesome, I figured it out! I’ll post the code I used for rotations and translations in case someone needs it:

# Estimate pose of the marker
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
                corners, markerSideLength, calib_matrix, dist_coeffs)
rvec = rvec[0][0]
tvec = tvec[0][0]

# Get the original marker position in 4x4 matrix representation
transformMatrix = getTransformMatrix(rvec, tvec)

# Get the transform matrix we want to apply to the obtained marker position
# E.g. rotate 180 deg (PI) along Y and then translate -10 cm along X
relativeTransformMatrix = relativeTransformMatrix([0, np.pi, 0], [-0.1, 0, 0])

# Now apply the transform to the original matrix by simply dot multiplying them
transformMatrix = np.dot(transformMatrix, relativeTransformMatrix)

# Extract rotation matrix and translation vector out of result and then display
rmat = transformMatrix[:3, :3]
tmat = transformMatrix[:3, 3:]
aruco.drawAxis(img, calib_matrix, dist_coeffs, rmat, tmat, 0.1)

Here are the utility functions I wrote:

def getTranslationMatrix(tvec):
    T = np.identity(n=4)
    T[0:3, 3] = tvec
    return T

def getTransformMatrix(rvec, tvec):
    mat = getTranslationMatrix(tvec)
    mat[:3, :3] = cv2.Rodrigues(rvec)[0]
    return mat

And the one from the Stackoverflow:

def relativeTransformMatrix(rotation, translation):
    xC, xS = cos(rotation[0]), sin(rotation[0])
    yC, yS = cos(rotation[1]), sin(rotation[1])
    zC, zS = cos(rotation[2]), sin(rotation[2])
    dX = translation[0]
    dY = translation[1]
    dZ = translation[2]
    Translate_matrix = np.array([[1, 0, 0, dX],
                                 [0, 1, 0, dY],
                                 [0, 0, 1, dZ],
                                 [0, 0, 0, 1]])
    Rotate_X_matrix = np.array([[1, 0, 0, 0],
                                [0, xC, -xS, 0],
                                [0, xS, xC, 0],
                                [0, 0, 0, 1]])
    Rotate_Y_matrix = np.array([[yC, 0, yS, 0],
                                [0, 1, 0, 0],
                                [-yS, 0, yC, 0],
                                [0, 0, 0, 1]])
    Rotate_Z_matrix = np.array([[zC, -zS, 0, 0],
                                [zS, zC, 0, 0],
                                [0, 0, 1, 0],
                                [0, 0, 0, 1]])
    return np.dot(Rotate_Z_matrix, np.dot(Rotate_Y_matrix, np.dot(Rotate_X_matrix, Translate_matrix)))

Thanks crackwitz for your help!

there’s code to be found on this forum. browse the calib3d tag. you’ll find functions to convert between rvec,tvec and 4x4 pose matrix, you’ll find utility functions that produce primitive transformations, and you’ll find how to use numpy’s @ operator for best readability.

I highly recommend working with axis-angle representations for rotations, rather than “euler angles”. they’re always confusing someone, especially the order in which the rotations are applied. it’s trivial to compose individual rotations too, in any order, when you have 4x4 matrices.

I hope not! because it’s supposed to first apply the rotation, to rotate marker-local geometry into alignment with the camera frame (while keeping it at its origin, like a rotation of the wrist), and then translate into camera frame.

cv.Rodrigues does the same. ignore the jacobian return part. edit: I see you’ve found that one