apriltag video capture example

apriltag_video_capture.py

# https://github.com/AprilRobotics/apriltag
# example image: https://github.com/AprilRobotics/apriltag-imgs/blob/master/tagStandard41h12/mosaic.png

import cv2
from apriltag import apriltag

cam = cv2.VideoCapture(0)
cv2.namedWindow("detections")
img_counter = 0

while True:
    ret, frame = cam.read()
    if not ret:
        print("failed to grab frame")
        break

    image = frame
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    detector = apriltag("tag16h5")

    detections = detector.detect(gray)
    for r in detections:
        # draw the bounding box of the detection
        (lb, rb, rt, lt) = r['lb-rb-rt-lt']
        rb = (int(rb[0]), int(rb[1]))
        rt = (int(rt[0]), int(rt[1]))
        lt = (int(lt[0]), int(lt[1]))
        lb = (int(lb[0]), int(lb[1]))
        cv2.line(image, lb, rb, (0, 255, 0), 2)
        cv2.line(image, rb, rt, (0, 255, 0), 2)
        cv2.line(image, rt, lt, (0, 255, 0), 2)
        cv2.line(image, lt, lb, (0, 255, 0), 2)

        # draw the center point
        center = r['center']
        (cx, cy) = (int(center[0]), int(center[1]))
        cv2.circle(image, (cx, cy), 5, (0, 0, 255), -1)

        # draw the tag id
        tag_id = r['id']
        cv2.putText(image, '{}'.format(tag_id), (lb[0], lb[1] - 15),
                    cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)

        # hamming = r['hamming']
        # margin = r ['margin']

    cv2.imshow("detections", image)

    k = cv2.waitKey(1)
    if k%256 == 27:
        # esc pressed
        print("quit")
        break
    elif k%256 == 32:
        # space pressed
        img_name = "detection_result_{}.png".format(img_counter)
        cv2.imwrite(img_name, image)
        print("save {}".format(img_name))
        img_counter += 1

cam.release()
cv2.destroyAllWindows()