Move aruco detected point along marker axes

Python
,
1

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:

image
image1280×552 69.3 KB

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?

2

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
3

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.

4

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!

5

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

6

hello，i would like to know how i could define the cube as an aruco board ?I want to realize this function in C++. Thanks a lot

7

first, look at OpenCV: Detection of ArUco Boards

then…

  • a “board” is a configuration of various markers in 3D space. only one marker needs to be visible at all.
  • a board has an origin
  • the markers that make up the board are defined relative to the board’s origin
  • cv.aruco.Board_create() takes some arguments
    • 3d array: number of markers, four corners per marker, 3 coordinates per corner
    • the aruco dictionary (4x4_1000 or something)
    • a list of ids, corresponding to the markers in first array
    • use np.array([[-0.5, +0.5, 0], [+0.5, +0.5, 0], [+0.5, -0.5, 0], [-0.5, -0.5, 0]]) as the “model” of a marker, scale according to its length, etc.
  • to construct a Board object, you’re gonna want to have some utility functions to scale, rotate, and translate points
10

Thanks for your reply. I still met a problem in the process of creating the board, for example, my camera can see at most three sides of the cube, so will it work when I use the transition matrix to find the angular coordinates of the three opposite sides?After that I start to create the board

11

I don’t understand what you’re trying to do there. I see no need for it.

12

managed to assemble a code that gives exactly same result as working example from above

    #we take some rvec and rotate the frame as we wish
    #here I rotated it into frame space of some robot arm model
    rotation_matrix, _ = cv2.Rodrigues(rvec)

    rotate_x = np.radians(90)
    rotate_y = np.radians(0)
    rotate_z1 = np.radians(-90)
    rotate_z2 = np.radians(90)

    rotation_matrix_x = np.array([[1, 0, 0],
                                  [0, np.cos(rotate_x), -np.sin(rotate_x)],
                                  [0, np.sin(rotate_x), np.cos(rotate_x)]])

    # Create the rotation matrices for the desired rotations
    rotation_matrix_y = np.array([[np.cos(rotate_y), 0, np.sin(rotate_y)],
                                  [0, 1, 0],
                                  [-np.sin(rotate_y), 0, np.cos(rotate_y)]])

    rotation_matrix_z1 = np.array([[np.cos(rotate_z1), -np.sin(rotate_z1), 0],
                                  [np.sin(rotate_z1), np.cos(rotate_z1), 0],
                                  [0, 0, 1]])

    rotation_matrix_z2 = np.array([[np.cos(rotate_z2), -np.sin(rotate_z2), 0],
                                  [np.sin(rotate_z2), np.cos(rotate_z2), 0],
                                  [0, 0, 1]])

    # Apply the rotations to the rotation matrix
    rotation_matrix = rotation_matrix @ rotation_matrix_z1 @ rotation_matrix_x @ rotation_matrix_z2

    rvec_tmp, _ = cv2.Rodrigues(rotation_matrix)

    rvec2, tvec2 = rvec_tmp.reshape((3, 1)), tvec.reshape((3, 1))

    # move frame farther by z from aruco space to camera space
    offset = np.array([[0.12820000000000964], [-0.0320000015199281], [0.33650000083446163]], dtype=np.float32)
    #I create a frame in camera space with my intended offset
    #and move it intro my desired aruco space described by frame I created in first half of code
    info = cv2.composeRT(np.array([[0], [0], [0]], dtype=np.float32), offset,
                         rvec2, tvec2)
    composedRvec, composedTvec = info[0], info[1]
    composedRvec1 = composedRvec.reshape((3, 1))
    composedTvec1 = composedTvec.reshape((3, 1))