# 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) 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
tvec = tvec

# 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)
return mat

``````

And the one from the Stackoverflow:

``````def relativeTransformMatrix(rotation, translation):
xC, xS = cos(rotation), sin(rotation)
yC, yS = cos(rotation), sin(rotation)
zC, zS = cos(rotation), sin(rotation)
dX = translation
dY = translation
dZ = translation
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)))
``````

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.
`cv.Rodrigues` does the same. ignore the jacobian return part. edit: I see you’ve found that one