## 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

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

### Re: Draw 3d cube on apriltags

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

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?

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)

``````
kwagyeman
Posts: 4271
Joined: Sun May 24, 2015 2:10 pm

### Re: Draw 3d cube on apriltags

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

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

### Re: Draw 3d cube on apriltags

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.