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

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    // Constants
16    const cv::Size FULL_RES(4000, 3000);      // 12MP
17    const cv::Size PREVIEW_SIZE(1332, 1000);  // 1/3 of 12MP
18
19    // Create nodes
20    auto hostCamera = pipeline.create<dai::node::Camera>()->build();
21    auto aprilTagNode = pipeline.create<dai::node::AprilTag>();
22    auto manip = pipeline.create<dai::node::ImageManip>();
23
24    // Configure nodes
25    auto outputCam = hostCamera->requestOutput(std::make_pair(FULL_RES.width, FULL_RES.height));
26    outputCam->link(aprilTagNode->inputImage);
27
28    // Configure ImageManip
29    manip->initialConfig->setOutputSize(PREVIEW_SIZE.width, PREVIEW_SIZE.height, dai::ImageManipConfig::ResizeMode::STRETCH);
30    manip->setMaxOutputFrameSize(2162688);
31    outputCam->link(manip->inputImage);
32
33    // Create output queues
34    auto outQueue = aprilTagNode->out.createOutputQueue();
35    auto frameQ = manip->out.createOutputQueue();
36
37    // Start pipeline
38    pipeline.start();
39
40    // Variables for FPS calculation
41    auto startTime = std::chrono::steady_clock::now();
42    int counter = 0;
43    float fps = 0.0f;
44    const cv::Scalar color(0, 255, 0);
45
46    while(true) {
47        auto aprilTagMessage = outQueue->get<dai::AprilTags>();
48        if(aprilTagMessage == nullptr) continue;
49
50        // FPS calculation
51        counter++;
52        auto currentTime = std::chrono::steady_clock::now();
53        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(currentTime - startTime).count();
54        if(elapsed > 1000) {
55            fps = counter * 1000.0f / elapsed;
56            counter = 0;
57            startTime = currentTime;
58        }
59
60        // Helper function to rescale points
61        auto rescale = [&FULL_RES, &PREVIEW_SIZE](const dai::Point2f& p) {
62            return cv::Point(static_cast<int>(p.x / FULL_RES.width * PREVIEW_SIZE.width), static_cast<int>(p.y / FULL_RES.height * PREVIEW_SIZE.height));
63        };
64
65        auto frame = frameQ->get<dai::ImgFrame>();
66        if(frame == nullptr) continue;
67
68        cv::Mat cvFrame = frame->getCvFrame();
69        for(const auto& tag : aprilTagMessage->aprilTags) {
70            auto topLeft = rescale(tag.topLeft);
71            auto topRight = rescale(tag.topRight);
72            auto bottomRight = rescale(tag.bottomRight);
73            auto bottomLeft = rescale(tag.bottomLeft);
74
75            cv::Point center((topLeft.x + bottomRight.x) / 2, (topLeft.y + bottomRight.y) / 2);
76
77            // Draw tag boundaries
78            cv::line(cvFrame, topLeft, topRight, color, 2, cv::LINE_AA, 0);
79            cv::line(cvFrame, topRight, bottomRight, color, 2, cv::LINE_AA, 0);
80            cv::line(cvFrame, bottomRight, bottomLeft, color, 2, cv::LINE_AA, 0);
81            cv::line(cvFrame, bottomLeft, topLeft, color, 2, cv::LINE_AA, 0);
82
83            // Draw tag ID
84            std::string idStr = "ID: " + std::to_string(tag.id);
85            cv::putText(cvFrame, idStr, center, cv::FONT_HERSHEY_TRIPLEX, 0.5, color);
86
87            // Draw FPS
88            cv::putText(cvFrame, "fps: " + std::to_string(fps).substr(0, 4), cv::Point(200, 20), cv::FONT_HERSHEY_TRIPLEX, 1, color);
89        }
90
91        cv::imshow("detections", cvFrame);
92
93        if(cv::waitKey(1) == 'q') {
94            break;
95        }
96    }
97
98    return 0;
99}