AprilTags from camera (12MP)

1#!/usr/bin/env python3
2
3import cv2
4import depthai as dai
5import time
6
7FULL_RES = (4000, 3000) # 12MP
8PREVIEW_SIZE = (1332, 1000) # 1/3 of 12MP, to preserve bandwidth
9
10with dai.Pipeline() as pipeline:
11    hostCamera = pipeline.create(dai.node.Camera).build()
12    aprilTagNode = pipeline.create(dai.node.AprilTag)
13    outputCam = hostCamera.requestOutput(FULL_RES)
14    outputCam.link(aprilTagNode.inputImage)
15    outQueue = aprilTagNode.out.createOutputQueue()
16
17    # We use ImageManip instead of creating a new smaller output because of the syncing,
18    # ATM, AprilTags don't contain timestamps, so we can't sync them with frames
19    manip = pipeline.create(dai.node.ImageManip)
20    manip.initialConfig.setOutputSize(PREVIEW_SIZE[0], PREVIEW_SIZE[1], dai.ImageManipConfig.ResizeMode.STRETCH)
21    manip.setMaxOutputFrameSize(2162688)
22    outputCam.link(manip.inputImage)
23    frameQ = manip.out.createOutputQueue()
24
25    color = (0, 255, 0)
26    startTime = time.monotonic()
27    counter = 0
28    fps = 0.0
29    pipeline.start()
30
31    while pipeline.isRunning():
32        aprilTagMessage = outQueue.get()
33        assert(isinstance(aprilTagMessage, dai.AprilTags))
34        aprilTags = aprilTagMessage.aprilTags
35
36        counter += 1
37        currentTime = time.monotonic()
38        if (currentTime - startTime) > 1:
39            fps = counter / (currentTime - startTime)
40            counter = 0
41            startTime = currentTime
42
43        def rescale(p: dai.Point2f):
44            return (int(p.x / FULL_RES[0] * PREVIEW_SIZE[0]),
45                    int(p.y / FULL_RES[1] * PREVIEW_SIZE[1]))
46
47        passthroughImage: dai.ImgFrame = frameQ.get()
48        frame = passthroughImage.getCvFrame()
49        for tag in aprilTags:
50            topLeft = rescale(tag.topLeft)
51            topRight = rescale(tag.topRight)
52            bottomRight = rescale(tag.bottomRight)
53            bottomLeft = rescale(tag.bottomLeft)
54
55            center = (int((topLeft[0] + bottomRight[0]) / 2), int((topLeft[1] + bottomRight[1]) / 2))
56
57            cv2.line(frame, topLeft, topRight, color, 2, cv2.LINE_AA, 0)
58            cv2.line(frame, topRight,bottomRight, color, 2, cv2.LINE_AA, 0)
59            cv2.line(frame, bottomRight,bottomLeft, color, 2, cv2.LINE_AA, 0)
60            cv2.line(frame, bottomLeft,topLeft, color, 2, cv2.LINE_AA, 0)
61
62            idStr = "ID: " + str(tag.id)
63            cv2.putText(frame, idStr, center, cv2.FONT_HERSHEY_TRIPLEX, 0.5, color)
64
65            cv2.putText(frame, f"fps: {fps:.1f}", (200, 20), cv2.FONT_HERSHEY_TRIPLEX, 1, color)
66
67        cv2.imshow("detections", frame)
68
69        if cv2.waitKey(1) == ord("q"):
70            break