AprilTags from camera

1#!/usr/bin/env python3
2
3import cv2
4import depthai as dai
5import time
6
7with dai.Pipeline() as pipeline:
8    hostCamera = pipeline.create(dai.node.Camera).build()
9    aprilTagNode = pipeline.create(dai.node.AprilTag)
10    hostCamera.requestOutput((1920, 1080)).link(aprilTagNode.inputImage)
11    passthroughOutputQueue = aprilTagNode.passthroughInputImage.createOutputQueue()
12    outQueue = aprilTagNode.out.createOutputQueue()
13
14    color = (0, 255, 0)
15    startTime = time.monotonic()
16    counter = 0
17    fps = 0.0
18
19    pipeline.start()
20    while pipeline.isRunning():
21        aprilTagMessage = outQueue.get()
22        assert(isinstance(aprilTagMessage, dai.AprilTags))
23        aprilTags = aprilTagMessage.aprilTags
24
25        counter += 1
26        currentTime = time.monotonic()
27        if (currentTime - startTime) > 1:
28            fps = counter / (currentTime - startTime)
29            counter = 0
30            startTime = currentTime
31
32        passthroughImage: dai.ImgFrame = passthroughOutputQueue.get()
33        frame = passthroughImage.getCvFrame()
34
35        def to_int(tag):
36            return (int(tag.x), int(tag.y))
37
38        for tag in aprilTags:
39            topLeft = to_int(tag.topLeft)
40            topRight = to_int(tag.topRight)
41            bottomRight = to_int(tag.bottomRight)
42            bottomLeft = to_int(tag.bottomLeft)
43
44            center = (int((topLeft[0] + bottomRight[0]) / 2), int((topLeft[1] + bottomRight[1]) / 2))
45
46            cv2.line(frame, topLeft, topRight, color, 2, cv2.LINE_AA, 0)
47            cv2.line(frame, topRight,bottomRight, color, 2, cv2.LINE_AA, 0)
48            cv2.line(frame, bottomRight,bottomLeft, color, 2, cv2.LINE_AA, 0)
49            cv2.line(frame, bottomLeft,topLeft, color, 2, cv2.LINE_AA, 0)
50
51            idStr = "ID: " + str(tag.id)
52            cv2.putText(frame, idStr, center, cv2.FONT_HERSHEY_TRIPLEX, 0.5, color)
53
54            cv2.putText(frame, f"fps: {fps:.1f}", (200, 20), cv2.FONT_HERSHEY_TRIPLEX, 0.5, color)
55
56        cv2.imshow("detections", frame)
57        if cv2.waitKey(1) == ord("q"):
58            break