import sensor, time, image

# 重置传感器
sensor.reset()

# 传感器设置
sensor.set_contrast(3)
sensor.set_gainceiling(16)
sensor.set_framesize(sensor.VGA)
sensor.set_windowing((320, 240))
sensor.set_pixformat(sensor.GRAYSCALE)

sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False, value=100)

#画出特征点
def draw_keypoints(img, kpts):
    if kpts:
        print(kpts)
        img.draw_keypoints(kpts)
        img = sensor.snapshot()
        time.sleep_ms(1000)


kpts1 = image.load_descriptor("/desc.orb")
kpts2 = image.load_descriptor("/desc0.orb")


clock = time.clock()

while (True):
    clock.tick()
    img = sensor.snapshot()

    kpts0 = img.find_keypoints(max_keypoints=150, threshold=10, normalized=True)

    if (kpts0):

        match = image.match_descriptor(kpts0, kpts1, threshold=85)

        if (match.count()>10):

            img.draw_rectangle(match.rect())
            img.draw_cross(match.cx(), match.cy(), size=10)

        print(kpts2, "matched 1:%d dt:%d"%(match.count(), match.theta()))

        match = image.match_descriptor(kpts0, kpts2, threshold=85)
        
        if (match.count()>10):
        
            img.draw_rectangle(match.rect())
            img.draw_cross(match.cx(), match.cy(), size=10)
        
        print(kpts2, "matched 2:%d dt:%d"%(match.count(), match.theta()))


    img.draw_string(0, 0, "FPS:%.2f"%(clock.fps()))