MAiX Qube 奮闘記07 AprilTagは使える!!

今回はAprilTagの認識に挑戦!
やってみたらAprilTagの認識力はＱＲコードの認識力とは異次元。
とにかく高速で、明るさやコントラストが悪条件でも強い。
タグの向きも取得できる。
これいろいろ使えそう。

下記サンプルコードは画像の中のAprilTagを赤い枠で囲み向きを矢印で表示するというもの。

AprilTagの画像はMaixPyIDEのツールで作成しています。

SearchTag.py

# Welcome to the MaixPy IDE!
# 1. Conenct board to computer
# 2. Select board at the top of MaixPy IDE: `tools->Select Board`
# 3. Click the connect buttion below to connect board
# 4. Click on the green run arrow button below to run the script!

import sensor, image, time, lcd,math
#time.sleep(1)
#lcd.init(type=2, invert=True,freq=15000000,width=240, height=240) # cube ips
lcd.init(type=2, invert=True) # cube ips
#lcd.mirror(False)
#lcd.rotation(1)
lcd.rotation(2)
lcd.clear(236, 36, 36)
#lcd.draw_string(10, 10, "hello maixpy", lcd.WHITE, lcd.RED)
#time.sleep(0.1)
#lcd.clear(236, 36, 36)
#lcd.draw_string(10, 10, "hello maixpy", lcd.WHITE, lcd.RED)
#lcd.init(freq=15000000)
sensor.reset()                      # Reset and initialize the sensor. It will
                                    # run automatically, call sensor.run(0) to stop
sensor.set_pixformat(sensor.RGB565) # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QQVGA)   # Set frame size to QVGA (320x240)
sensor.skip_frames(time = 2000)     # Wait for settings take effect.
sensor.set_vflip(True)
sensor.set_hmirror(True)
clock = time.clock()                # Create a clock object to track the FPS.

tag_families = 0
tag_families |= image.TAG16H5 # comment out to disable this family
tag_families |= image.TAG25H7 # comment out to disable this family
tag_families |= image.TAG25H9 # comment out to disable this family
tag_families |= image.TAG36H10 # comment out to disable this family
tag_families |= image.TAG36H11 # comment out to disable this family (default family)
tag_families |= image.ARTOOLKIT # comment out to disable this family
def family_name(tag):
    if(tag.family() == image.TAG16H5):
        return "TAG16H5"
    if(tag.family() == image.TAG25H7):
        return "TAG25H7"
    if(tag.family() == image.TAG25H9):
        return "TAG25H9"
    if(tag.family() == image.TAG36H10):
        return "TAG36H10"
    if(tag.family() == image.TAG36H11):
        return "TAG36H11"
    if(tag.family() == image.ARTOOLKIT):
        return "ARTOOLKIT"

while(True):
    clock.tick()                    # Update the FPS clock.
    img = sensor.snapshot()
    for tag in img.find_apriltags(families=tag_families): # defaults to TAG36H11 without "families".
        img.draw_rectangle(tag.rect(), color = (255, 0, 0),thickness=2)
        img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
        #矢印を描画
        circlex,circley = tag.cx()+int(math.sin(tag.rotation())*40),tag.cy()+int(math.cos(tag.rotation())*40)
        img.draw_arrow(tag.cx(), tag.cy(),circlex,circley, color = (255, 0, 0),thickness=2)
        print_args = (family_name(tag), tag.id(), (180 * tag.rotation()) / math.pi)
        print("Tag Family %s, Tag ID %d, rotation %5.2f (degrees)" % print_args)
        img.draw_string(tag.cx(), tag.cy(), "ID:"+str(tag.id()), lcd.GREEN, scale=2)
        #print_args = (family_name(tag), tag.id(), (180 * tag.rotation()) / math.pi)
        #print("Tag Family %s, Tag ID %d, rotation %f (degrees)" % print_args)
        #img.draw_string(5, 30, "Hello World!%d"%clock.fps(), color = (50, 50, 100), scale = 2, mono_space = False,
            #char_rotation = 0, char_hmirror = False, char_vflip = False,
            #string_rotation = 0, string_hmirror = False, string_vflip = False)

    lcd.display(img)                # Display on LCD

    print(clock.fps())