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

1#include <chrono>
2#include <iostream>
3#include <memory>
4#include <opencv2/opencv.hpp>
5
6#include "depthai/depthai.hpp"
7
8int main() {
9    // Create device
10    std::shared_ptr<dai::Device> device = std::make_shared<dai::Device>();
11
12    // Create pipeline
13    dai::Pipeline pipeline(device);
14
15    // Create nodes
16    auto cameraNode = pipeline.create<dai::node::Camera>()->build();
17    auto aprilTagNode = pipeline.create<dai::node::AprilTag>();
18
19    // Configure nodes
20    auto outputCam = cameraNode->requestOutput(std::make_pair(1280, 720));
21    outputCam->link(aprilTagNode->inputImage);
22
23    // Create output queues
24    auto passthroughOutputQueue = aprilTagNode->passthroughInputImage.createOutputQueue();
25    auto outQueue = aprilTagNode->out.createOutputQueue();
26
27    // Start pipeline
28    pipeline.start();
29
30    // Variables for FPS calculation
31    auto startTime = std::chrono::steady_clock::now();
32    int counter = 0;
33    float fps = 0.0f;
34    const cv::Scalar color(0, 255, 0);
35
36    while(true) {
37        auto aprilTagMessage = outQueue->get<dai::AprilTags>();
38        if(aprilTagMessage == nullptr) continue;
39
40        // FPS calculation
41        counter++;
42        auto currentTime = std::chrono::steady_clock::now();
43        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(currentTime - startTime).count();
44        if(elapsed > 1000) {
45            fps = counter * 1000.0f / elapsed;
46            counter = 0;
47            startTime = currentTime;
48        }
49
50        auto frame = passthroughOutputQueue->get<dai::ImgFrame>();
51        if(frame == nullptr) continue;
52
53        cv::Mat cvFrame = frame->getCvFrame();
54
55        // Helper function to convert points to integers
56        auto to_int = [](const dai::Point2f& p) { return cv::Point(static_cast<int>(p.x), static_cast<int>(p.y)); };
57
58        for(const auto& tag : aprilTagMessage->aprilTags) {
59            auto topLeft = to_int(tag.topLeft);
60            auto topRight = to_int(tag.topRight);
61            auto bottomRight = to_int(tag.bottomRight);
62            auto bottomLeft = to_int(tag.bottomLeft);
63
64            cv::Point center((topLeft.x + bottomRight.x) / 2, (topLeft.y + bottomRight.y) / 2);
65
66            // Draw tag boundaries
67            cv::line(cvFrame, topLeft, topRight, color, 2, cv::LINE_AA, 0);
68            cv::line(cvFrame, topRight, bottomRight, color, 2, cv::LINE_AA, 0);
69            cv::line(cvFrame, bottomRight, bottomLeft, color, 2, cv::LINE_AA, 0);
70            cv::line(cvFrame, bottomLeft, topLeft, color, 2, cv::LINE_AA, 0);
71
72            // Draw tag ID
73            std::string idStr = "ID: " + std::to_string(tag.id);
74            cv::putText(cvFrame, idStr, center, cv::FONT_HERSHEY_TRIPLEX, 0.5, color);
75
76            // Draw FPS
77            cv::putText(cvFrame, "fps: " + std::to_string(fps).substr(0, 4), cv::Point(200, 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, color);
78        }
79
80        cv::imshow("detections", cvFrame);
81
82        if(cv::waitKey(1) == 'q') {
83            break;
84        }
85    }
86
87    return 0;
88}