Draw 3d cube on apriltags

Discussion related to "under the hood" OpenMV topics.
jgpeiro
Posts: 2
Joined: Thu Nov 01, 2018 8:00 am

Draw 3d cube on apriltags

Postby jgpeiro » Thu Nov 01, 2018 8:22 am

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?

Code: Select all

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 )
User avatar
kwagyeman
Posts: 2401
Joined: Sun May 24, 2015 2:10 pm

Re: Draw 3d cube on apriltags

Postby kwagyeman » Thu Nov 01, 2018 11:20 am

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.
Nyamekye,
jgpeiro
Posts: 2
Joined: Thu Nov 01, 2018 8:00 am

Re: Draw 3d cube on apriltags

Postby jgpeiro » Thu Nov 01, 2018 5:38 pm

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?

Image

Code: Select all

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)

User avatar
kwagyeman
Posts: 2401
Joined: Sun May 24, 2015 2:10 pm

Re: Draw 3d cube on apriltags

Postby kwagyeman » Fri Nov 02, 2018 1:19 am

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.
Nyamekye,

Return to “Technical Discussion”

Who is online

Users browsing this forum: No registered users and 5 guests