AprilTags on image replay

1#!/usr/bin/env python3
2
3import cv2
4import depthai as dai
5import time
6from pathlib import Path
7
8# Get the absolute path of the current script's directory
9script_dir = Path(__file__).resolve().parent
10examplesRoot = (script_dir / Path('../')).resolve()  # This resolves the parent directory correctly
11models = examplesRoot / 'models'
12tagImage = models / 'april_tags.jpg'
13
14class ImageReplay(dai.node.ThreadedHostNode):
15    def __init__(self):
16        dai.node.ThreadedHostNode.__init__(self)
17        self.output =  self.createOutput()
18        frame = cv2.imread(str(tagImage.resolve()))
19        frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
20        imgFrame = dai.ImgFrame()
21        imgFrame.setData(frame)
22        imgFrame.setWidth(frame.shape[1])
23        imgFrame.setHeight(frame.shape[0])
24        imgFrame.setType(dai.ImgFrame.Type.GRAY8)
25        self.imgFrame = imgFrame
26    def run(self):
27        while self.mainLoop():
28            self.output.send(self.imgFrame)
29            time.sleep(0.03)
30
31with dai.Pipeline() as pipeline:
32    imageReplay = pipeline.create(ImageReplay)
33    aprilTagNode = pipeline.create(dai.node.AprilTag)
34    imageReplay.output.link(aprilTagNode.inputImage)
35    aprilTagNode.initialConfig.setFamily(dai.AprilTagConfig.Family.TAG_16H5)
36
37    passthroughOutputQueue = aprilTagNode.passthroughInputImage.createOutputQueue()
38    outQueue = aprilTagNode.out.createOutputQueue()
39
40    color = (0, 255, 0)
41    startTime = time.monotonic()
42    counter = 0
43    fps = 0.0
44
45    pipeline.start()
46    while pipeline.isRunning():
47        aprilTagMessage = outQueue.get()
48        assert(isinstance(aprilTagMessage, dai.AprilTags))
49        aprilTags = aprilTagMessage.aprilTags
50
51        counter += 1
52        currentTime = time.monotonic()
53        if (currentTime - startTime) > 1:
54            fps = counter / (currentTime - startTime)
55            counter = 0
56            startTime = currentTime
57
58        passthroughImage: dai.ImgFrame = passthroughOutputQueue.get()
59        frame = passthroughImage.getCvFrame()
60        frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
61
62        def to_int(tag):
63            return (int(tag.x), int(tag.y))
64
65        for tag in aprilTags:
66            topLeft = to_int(tag.topLeft)
67            topRight = to_int(tag.topRight)
68            bottomRight = to_int(tag.bottomRight)
69            bottomLeft = to_int(tag.bottomLeft)
70
71            center = (int((topLeft[0] + bottomRight[0]) / 2), int((topLeft[1] + bottomRight[1]) / 2))
72
73            cv2.line(frame, topLeft, topRight, color, 2, cv2.LINE_AA, 0)
74            cv2.line(frame, topRight,bottomRight, color, 2, cv2.LINE_AA, 0)
75            cv2.line(frame, bottomRight,bottomLeft, color, 2, cv2.LINE_AA, 0)
76            cv2.line(frame, bottomLeft,topLeft, color, 2, cv2.LINE_AA, 0)
77
78            idStr = "ID: " + str(tag.id)
79            cv2.putText(frame, idStr, center, cv2.FONT_HERSHEY_TRIPLEX, 0.5, color)
80
81            cv2.putText(frame, f"fps: {fps:.1f}", (200, 20), cv2.FONT_HERSHEY_TRIPLEX, 0.5, color)
82
83        cv2.imshow("detections", frame)
84        if cv2.waitKey(1) == ord("q"):
85            break

1#include <chrono>
2#include <iostream>
3#include <opencv2/opencv.hpp>
4#include <thread>
5
6#include "depthai/depthai.hpp"
7
8class ImageReplay : public dai::NodeCRTP<dai::node::ThreadedHostNode, ImageReplay> {
9   public:
10    constexpr static const char* NAME = "ImageReplay";
11
12    Output output{*this, {"out", DEFAULT_GROUP, {{{dai::DatatypeEnum::ImgFrame, true}}}}};
13
14    ImageReplay() {
15        // Load and prepare the image
16        cv::Mat frame = cv::imread(APRIL_TAGS_PATH);
17        cv::cvtColor(frame, frame, cv::COLOR_BGR2GRAY);
18
19        // Create ImgFrame
20        auto imgFrame = std::make_shared<dai::ImgFrame>();
21        std::vector<uint8_t> data(frame.data, frame.data + frame.total() * frame.elemSize());
22        imgFrame->setData(data);
23        imgFrame->setWidth(frame.cols);
24        imgFrame->setHeight(frame.rows);
25        imgFrame->setType(dai::ImgFrame::Type::GRAY8);
26        _imgFrame = imgFrame;
27    }
28
29    void run() override {
30        while(mainLoop()) {
31            output.send(_imgFrame);
32            std::this_thread::sleep_for(std::chrono::milliseconds(30));
33        }
34    }
35
36   private:
37    std::shared_ptr<dai::ImgFrame> _imgFrame;
38};
39
40int main() {
41    // Create pipeline
42    dai::Pipeline pipeline;
43
44    // Create nodes
45    auto imageReplay = pipeline.create<ImageReplay>();
46    auto aprilTagNode = pipeline.create<dai::node::AprilTag>();
47
48    // Link nodes
49    imageReplay->output.link(aprilTagNode->inputImage);
50    aprilTagNode->initialConfig->setFamily(dai::AprilTagConfig::Family::TAG_16H5);
51
52    // Create output queues
53    auto passthroughOutputQueue = aprilTagNode->passthroughInputImage.createOutputQueue();
54    auto outQueue = aprilTagNode->out.createOutputQueue();
55
56    // Start pipeline
57    pipeline.start();
58
59    // FPS calculation variables
60    cv::Scalar color(0, 255, 0);
61    auto startTime = std::chrono::steady_clock::now();
62    int counter = 0;
63    float fps = 0.0f;
64
65    // Main loop
66    while(pipeline.isRunning()) {
67        auto aprilTagMessage = outQueue->get<dai::AprilTags>();
68        auto aprilTags = aprilTagMessage->aprilTags;
69
70        // Calculate FPS
71        counter++;
72        auto currentTime = std::chrono::steady_clock::now();
73        auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(currentTime - startTime).count();
74        if(elapsed >= 1) {
75            fps = counter / static_cast<float>(elapsed);
76            counter = 0;
77            startTime = currentTime;
78        }
79
80        // Get passthrough image
81        auto passthroughImage = passthroughOutputQueue->get<dai::ImgFrame>();
82        cv::Mat frame = passthroughImage->getCvFrame();
83        cv::cvtColor(frame, frame, cv::COLOR_GRAY2BGR);
84
85        // Helper function to convert points to integers
86        auto to_int = [](const dai::Point2f& p) { return cv::Point(static_cast<int>(p.x), static_cast<int>(p.y)); };
87
88        // Draw detections
89        for(const auto& tag : aprilTags) {
90            auto topLeft = to_int(tag.topLeft);
91            auto topRight = to_int(tag.topRight);
92            auto bottomRight = to_int(tag.bottomRight);
93            auto bottomLeft = to_int(tag.bottomLeft);
94
95            auto center = cv::Point((topLeft.x + bottomRight.x) / 2, (topLeft.y + bottomRight.y) / 2);
96
97            // Draw rectangle
98            cv::line(frame, topLeft, topRight, color, 2, cv::LINE_AA, 0);
99            cv::line(frame, topRight, bottomRight, color, 2, cv::LINE_AA, 0);
100            cv::line(frame, bottomRight, bottomLeft, color, 2, cv::LINE_AA, 0);
101            cv::line(frame, bottomLeft, topLeft, color, 2, cv::LINE_AA, 0);
102
103            // Draw ID
104            std::string idStr = "ID: " + std::to_string(tag.id);
105            cv::putText(frame, idStr, center, cv::FONT_HERSHEY_TRIPLEX, 0.5, color);
106
107            // Draw FPS
108            cv::putText(frame, "fps: " + std::to_string(fps).substr(0, 4), cv::Point(200, 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, color);
109        }
110
111        cv::imshow("detections", frame);
112        if(cv::waitKey(1) == 'q') {
113            break;
114        }
115    }
116
117    return 0;
118}