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: 4271
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: 4271
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,
kakaguotao
Posts: 2
Joined: Sun Mar 15, 2020 2:59 pm

Re: Draw 3d cube on apriltags

Postby kakaguotao » Sun Mar 15, 2020 3:01 pm

Thanks.
Can we do solve pnp on OpneMV?
User avatar
iabdalkader
Posts: 1193
Joined: Sun May 24, 2015 3:53 pm

Re: Draw 3d cube on apriltags

Postby iabdalkader » Sun Mar 15, 2020 4:42 pm

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 https://github.com/jalawson/ulinalg

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

Return to “Technical Discussion”

Who is online

Users browsing this forum: No registered users and 4 guests