Draw 3d cube on apriltags

Hi,

I want to use apriltags rotation and translation to draw a virtual 3d cube over it.
It is possible implement something like OpenCV projectPoints function?

points = ([0,0,0], [1,0,0], [1,1,0], [0,1,0]...)
fx, fy = camera focal length
cx, cy = camera center
tag = img.find_apriltags()
for point in points:
    m = mat_mult( scale( fx, fy, 1 ), point )
    m = mat_mult( rotate( tag.rx, tag.ry, tag.rz ), m )
    m = mat_mult( translate( tag.tx, tag.ty, tag.tz ), m )
    x, y = m[0][0], m[0][1]
    img.draw_pixel( x, y )

We just have the draw line method and the four tag corners. If you want to do the 3D cube thing you need to perform the matrix math yourself. Um, I understand this looks cool, but, I don’t know the use otherwise.

I can draw 3d cube with this code, but I cant draw this cube over real apriltag cause I cant relate obtained rotation and translation values.
It is possible? How can I do?




import sensor, image, time, math

def mat_mult( A, B ):
    rows_A = len(A)
    cols_A = len(A[0])
    rows_B = len(B)
    cols_B = len(B[0])

    if( cols_A != rows_B ):
        print( "Cannot multiply the two matrices. Incorrect dimensions." )
        return None

    C = [[0.0 for row in range(cols_B)] for col in range(rows_A)]
    for i in range(rows_A):
        for j in range(cols_B):
            for k in range(cols_A):
                C[i][j] += A[i][k] * B[k][j]

    return C

def scale( x, y, z ):
    m = [
        [ x, 0, 0, 0 ],
        [ 0, y, 0, 0 ],
        [ 0, 0, z, 0 ],
        [ 0, 0, 0, 1 ],
    ]
    return m

def rotate_x( a ):
    s = math.sin( a )
    c = math.cos( a )
    m = [
        [ 1, 0, 0, 0 ],
        [ 0, c,-s, 0 ],
        [ 0, s, c, 0 ],
        [ 0, 0, 0, 1 ],
    ]
    return m

def rotate_y( a ):
    s = math.sin( a )
    c = math.cos( a )
    m = [
        [ c, 0, s, 0 ],
        [ 0, 1, 0, 0 ],
        [-s, 0, c, 0 ],
        [ 0, 0, 0, 1 ],
    ]
    return m

def rotate_z( a ):
    s = math.sin( a )
    c = math.cos( a )
    m = [
        [ c,-s, 0, 0 ],
        [ s, c, 0, 0 ],
        [ 0, 0, 1, 0 ],
        [ 0, 0, 0, 1 ],
    ]
    return m

def rotate( rx, ry, rz ):
    m = rotate_z( rz )
    m = mat_mult( rotate_y( ry ), m )
    m = mat_mult( rotate_x( rx ), m )
    return m

def translate( x, y, z ):
    m = [
        [ 1, 0, 0, x ],
        [ 0, 1, 0, y ],
        [ 0, 0, 1, z ],
        [ 0, 0, 0, 1 ],
    ]
    return m

def point( x, y, z ):
    m = [ [x], [y], [z], [1] ]
    return m

points_pairs = [
    [point( 0, 0, 0 ), point( 0, 1, 0 )], 
    [point( 0, 1, 0 ), point( 1, 1, 0 )], 
    [point( 1, 1, 0 ), point( 1, 0, 0 )], 
    [point( 1, 0, 0 ), point( 0, 0, 0 )], 

    [point( 0, 0, 1 ), point( 0, 1, 1 )], 
    [point( 0, 1, 1 ), point( 1, 1, 1 )], 
    [point( 1, 1, 1 ), point( 1, 0, 1 )], 
    [point( 1, 0, 1 ), point( 0, 0, 1 )], 

    [point( 0, 0, 0 ), point( 0, 0, 1 )], 
    [point( 0, 1, 0 ), point( 0, 1, 1 )], 
    [point( 1, 1, 0 ), point( 1, 1, 1 )], 
    [point( 1, 0, 0 ), point( 1, 0, 1 )], 
]

colors = [
    [255, 0, 0], [255, 0, 0], [255, 0, 0], [255, 0, 0],
    [0, 255, 0], [0, 255, 0], [0, 255, 0], [0, 255, 0],
    [0, 0, 255], [0, 0, 255], [0, 0, 255], [0, 0, 255],
]

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
#sensor.skip_frames(time = 2000)
img = sensor.snapshot()

sx, sy, sz = 100, 100, 100
rx, ry, rz = 0, math.pi/6, math.pi/6
tx, ty, tz = 320/2, 240/2, 100

for i in range( 1000 ):
    
    rx = i*math.pi/100
    img.clear()
    
    for point_pair, color in zip( points_pairs, colors ):
        m = point_pair[0]
        m = mat_mult( scale( sx, sy, sz ), m )
        m = mat_mult( rotate( rx, ry, rz ), m )
        m = mat_mult( translate( tx, ty, tz ), m )
        m0 = m
        
        m = point_pair[1]
        m = mat_mult( scale( sx, sy, sz ), m )
        m = mat_mult( rotate( rx, ry, rz ), m )
        m = mat_mult( translate( tx, ty, tz ), m )
        m1 = m
        
        x0, y0, x1, y1 = int( m0[0][0] ), int( m0[1][0] ), int( m1[0][0] ), int( m1[1][0] )
        img.draw_line( x0, y0, x1, y1, color=color, thickness=2 )

    sensor.flush()
    #time.sleep(1000)

Hi, the AprilTag outputs it’s 6dof orientation and centorid in the field of view. Given your code can you accept the centrod of the tag to rotate the cube around as a start? If so, then you just need to use the tag x, y, and z, rotation to position things.

Alternatively, use the corners() method of he tag to get a sorted list of the corners of the tag.

It looks like you got most of the code to.draw the 3d stuff below. Try setting the corners of the tag to the X positions of the box and see what happens. Then project out from those corners where the top of the box would be.

It looks like you are almost there.

Thanks.
Can we do solve pnp on OpneMV?

Hi, we have some matrix / linear algebra modules built-in if you’d like to use them instead (should simplify your code a bit) here’s the doc GitHub - jalawson/ulinalg: Small size matrix handling module with a few linear algebra operations specifically for MicroPython (Python3)

When you’re done, if you don’t mind please share the final script, and I’ll add this to the examples.