AprilTags Example

This example shows the power of the OpenMV Cam to detect April Tags

on the OpenMV Cam M7. The M4 versions cannot detect April Tags.

import sensor, image, time, math, pyb
from pyb import LED, UART

#led灯的定义
LedRed = LED(1)
LedGreen = LED(2)
LedBlue = LED(3)

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
sensor.skip_frames(100)
sensor.set_auto_gain(False) # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False) # must turn this off to prevent image washout...
clock = time.clock()
uart = UART(3,9600,timeout_char=1000)
uart.init(9600, bits=8, parity=None, stop=1, timeout_char=1000)

#apriltag 坐标信息存储列表
apriltags = [[0,0],[0,0],[0,0],[0,0]]

#坐标系的原点和宽高 x y w h
coordinate = [1,1,1,1]

#定义各个可能需要被识别的色块
red_threshold = (40, 49, 30, 78, 32, 61)
yellow_threshold = (62, 69, 0, 18, 12, 72)
blue_threshold = (17, 38, -5, 34, -50, -22)

#查询所有apritag，并将有用的apriltag保存到列表内
#img:相机拍摄照片后的image对象
#TAG16H5
def search_apritags(img):
for tag in img.find_apriltags(families=image.TAG16H5):
img.draw_rectangle(tag.rect(), color = (255, 0, 0)) #用红色框出apriltag
img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0)) #用绿色十字标出apriltag的中心
#将apriltag信息存放到apriltags列表内
if tag.id() >= 0 and tag.id() <= 3:
apriltags[tag.id()] = [tag.cx(), tag.cy()]

#检测apriltags内是否保存有四个坐标点
def check_apriltags():
for i in range(0,4):
if apriltags[i][0] == 0 and apriltags[i][1] == 0:
return 0
return 1

ROI=(10,10,20,20)

#确定原点坐标以及背景区域的宽高像素值
#num:原点坐标apritag的ID号

目前使用2号apritag为原点坐标

2 0

3 1

def make_coordinate(num=2):
if check_apriltags() == 1:
coordinate[0] = apriltags[num][0]
coordinate[1] = apriltags[num][1]
coordinate[2] = (int)((apriltags[0][0] - apriltags[2][0] + apriltags[1][0] - apriltags[3][0])/2 + 0.5)
coordinate[3] = (int)((apriltags[3][1] - apriltags[2][1] + apriltags[1][1] - apriltags[0][1])/2 + 0.5)
print(coordinate)

#寻找色块 red 1 blue 2 yellow 4
#发送数据 一次只发送一个目标点的信息
def find_color(img, roi):
blobs = img.find_blobs([red_threshold, blue_threshold, yellow_threshold],roi=roi)
if blobs:
for b in blobs:
if b.w() > 10 and b.h() > 10:
img.draw_rectangle(b[0:4])
img.draw_cross(b[5],b[6])
for b in blobs:
if b.w() > 10 and b.h() > 10:
com_sendbyte(b.cx(),b.cy(),b.code())
break

#整合数据、串口发送
#帧头 背景宽高位 背景宽低位 背景高高位 背景高低位 颜色 y高位 y低位 x高位 x低位 校验和
def com_sendbyte(x, y, color):
bwh = (coordinate[2] & 0xFF00) >> 8
bwl = coordinate[2] & 0x00FF
bhh = (coordinate[3] & 0xFF00) >> 8
bhl = coordinate[3] & 0x00FF

target_x = x - coordinate[0]
target_y = y - coordinate[1]
txh = (target_x & 0xFF00) >> 8
txl = target_x & 0x00FF
tyh = (target_y & 0xFF00) >> 8
tyl = target_y & 0x00FF

check = txh + txl + tyh + tyl + bwh + bwl + bhh + bhl + color

print(bwh, bwl, bhh, bhl, color, txh, txl, tyh, tyl, check)
arr = bytearray([bwh, bwl, bhh, bhl, color, txh, txl, tyh, tyl, check])
uart.write(arr)

find_aprtime = pyb.millis()
find_blobtime = pyb.millis()
while(True):
img = sensor.snapshot().lens_corr(strength = 1.2, zoom = 1.2)
ROI = tuple(coordinate)
img.draw_rectangle(ROI)
if pyb.millis() - find_aprtime > 1000:
find_aprtime = pyb.millis()
search_apritags(img) #寻找坐标点
make_coordinate() #建立坐标系
if pyb.millis() - find_blobtime > 100:
find_blobtime = pyb.millis()
find_color(img,ROI)