|
| 1 | +#!/usr/bin/env python3 |
| 2 | + |
| 3 | +import cv2 |
| 4 | +import depthai as dai |
| 5 | +import time |
| 6 | + |
| 7 | +# Create pipeline |
| 8 | +pipeline = dai.Pipeline() |
| 9 | + |
| 10 | +# Define sources and outputs |
| 11 | +monoLeft = pipeline.create(dai.node.MonoCamera) |
| 12 | +aprilTag = pipeline.create(dai.node.AprilTag) |
| 13 | + |
| 14 | +xoutMono = pipeline.create(dai.node.XLinkOut) |
| 15 | +xoutAprilTag = pipeline.create(dai.node.XLinkOut) |
| 16 | + |
| 17 | +xoutMono.setStreamName("mono") |
| 18 | +xoutAprilTag.setStreamName("aprilTagData") |
| 19 | + |
| 20 | +# Properties |
| 21 | +monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) |
| 22 | +monoLeft.setBoardSocket(dai.CameraBoardSocket.LEFT) |
| 23 | + |
| 24 | +aprilTag.initialConfig.setFamily(dai.AprilTagConfig.Family.TAG_36H11) |
| 25 | + |
| 26 | +# Linking |
| 27 | +aprilTag.passthroughInputImage.link(xoutMono.input) |
| 28 | +monoLeft.out.link(aprilTag.inputImage) |
| 29 | +aprilTag.out.link(xoutAprilTag.input) |
| 30 | +# always take the latest frame as apriltag detections are slow |
| 31 | +aprilTag.inputImage.setBlocking(False) |
| 32 | +aprilTag.inputImage.setQueueSize(1) |
| 33 | + |
| 34 | +# advanced settings, configurable at runtime |
| 35 | +aprilTagConfig = aprilTag.initialConfig.get() |
| 36 | +aprilTagConfig.quadDecimate = 4 |
| 37 | +aprilTagConfig.quadSigma = 0 |
| 38 | +aprilTagConfig.refineEdges = True |
| 39 | +aprilTagConfig.decodeSharpening = 0.25 |
| 40 | +aprilTagConfig.maxHammingDistance = 1 |
| 41 | +aprilTagConfig.quadThresholds.minClusterPixels = 5 |
| 42 | +aprilTagConfig.quadThresholds.maxNmaxima = 10 |
| 43 | +aprilTagConfig.quadThresholds.criticalDegree = 10 |
| 44 | +aprilTagConfig.quadThresholds.maxLineFitMse = 10 |
| 45 | +aprilTagConfig.quadThresholds.minWhiteBlackDiff = 5 |
| 46 | +aprilTagConfig.quadThresholds.deglitch = False |
| 47 | +aprilTag.initialConfig.set(aprilTagConfig) |
| 48 | + |
| 49 | +# Connect to device and start pipeline |
| 50 | +with dai.Device(pipeline) as device: |
| 51 | + |
| 52 | + # Output queue will be used to get the mono frames from the outputs defined above |
| 53 | + monoQueue = device.getOutputQueue("mono", 8, False) |
| 54 | + aprilTagQueue = device.getOutputQueue("aprilTagData", 8, False) |
| 55 | + |
| 56 | + color = (0, 255, 0) |
| 57 | + |
| 58 | + startTime = time.monotonic() |
| 59 | + counter = 0 |
| 60 | + fps = 0 |
| 61 | + |
| 62 | + while(True): |
| 63 | + inFrame = monoQueue.get() |
| 64 | + |
| 65 | + counter+=1 |
| 66 | + current_time = time.monotonic() |
| 67 | + if (current_time - startTime) > 1 : |
| 68 | + fps = counter / (current_time - startTime) |
| 69 | + counter = 0 |
| 70 | + startTime = current_time |
| 71 | + |
| 72 | + monoFrame = inFrame.getFrame() |
| 73 | + frame = cv2.cvtColor(monoFrame, cv2.COLOR_GRAY2BGR) |
| 74 | + |
| 75 | + aprilTagData = aprilTagQueue.get().aprilTags |
| 76 | + for aprilTag in aprilTagData: |
| 77 | + topLeft = aprilTag.topLeft |
| 78 | + topRight = aprilTag.topRight |
| 79 | + bottomRight = aprilTag.bottomRight |
| 80 | + bottomLeft = aprilTag.bottomLeft |
| 81 | + |
| 82 | + center = (int((topLeft.x + bottomRight.x) / 2), int((topLeft.y + bottomRight.y) / 2)) |
| 83 | + |
| 84 | + cv2.line(frame, (int(topLeft.x), int(topLeft.y)), (int(topRight.x), int(topRight.y)), color, 2, cv2.LINE_AA, 0) |
| 85 | + cv2.line(frame, (int(topRight.x), int(topRight.y)), (int(bottomRight.x), int(bottomRight.y)), color, 2, cv2.LINE_AA, 0) |
| 86 | + cv2.line(frame, (int(bottomRight.x), int(bottomRight.y)), (int(bottomLeft.x), int(bottomLeft.y)), color, 2, cv2.LINE_AA, 0) |
| 87 | + cv2.line(frame, (int(bottomLeft.x), int(bottomLeft.y)), (int(topLeft.x), int(topLeft.y)), color, 2, cv2.LINE_AA, 0) |
| 88 | + |
| 89 | + idStr = "ID: " + str(aprilTag.id) |
| 90 | + cv2.putText(frame, idStr, center, cv2.FONT_HERSHEY_TRIPLEX, 0.5, color) |
| 91 | + |
| 92 | + cv2.putText(frame, "Fps: {:.2f}".format(fps), (2, frame.shape[0] - 4), cv2.FONT_HERSHEY_TRIPLEX, 0.4, (255,255,255)) |
| 93 | + |
| 94 | + cv2.imshow("mono", frame) |
| 95 | + |
| 96 | + if cv2.waitKey(1) == ord('q'): |
| 97 | + break |
0 commit comments