立体深度重映射

1import depthai as dai
2import cv2
3import numpy as np
4import time
5
6def draw_rotated_rectangle(frame, center, size, angle, color, thickness=2):
7    """
8    Draws a rotated rectangle on the given frame.
9
10    Args:
11        frame (numpy.ndarray): The image/frame to draw on.
12        center (tuple): The (x, y) coordinates of the rectangle's center.
13        size (tuple): The (width, height) of the rectangle.
14        angle (float): The rotation angle of the rectangle in degrees (counter-clockwise).
15        color (tuple): The color of the rectangle in BGR format (e.g., (0, 255, 0) for green).
16        thickness (int): The thickness of the rectangle edges. Default is 2.
17    """
18    # Create a rotated rectangle
19    rect = ((center[0], center[1]), (size[0], size[1]), angle)
20
21    # Get the four vertices of the rotated rectangle
22    box = cv2.boxPoints(rect)
23    box = np.intp(box)  # Convert to integer coordinates
24
25    # Draw the rectangle on the frame
26    cv2.polylines(frame, [box], isClosed=True, color=color, thickness=thickness)
27
28def processDepthFrame(depthFrame: dai.ImgFrame):
29    return dai.utility.colorizeDepthFrame(depthFrame, colormap=cv2.COLORMAP_HOT).getCvFrame()
30
31with dai.Pipeline() as pipeline:
32    color = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_A)
33    monoLeft = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B)
34    monoRight = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_C)
35    stereo = pipeline.create(dai.node.StereoDepth)
36
37    stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.DEFAULT)
38    # stereo.setDepthAlign(dai.CameraBoardSocket.CAM_A)
39    # stereo.setOutputSize(640, 400)
40
41    colorCamOut = color.requestOutput((640, 480))
42
43    monoLeftOut = monoLeft.requestOutput((640, 480))
44    monoRightOut = monoRight.requestOutput((640, 480))
45
46    monoLeftOut.link(stereo.left)
47    monoRightOut.link(stereo.right)
48
49    colorOut = colorCamOut.createOutputQueue()
50    rightOut = monoRightOut.createOutputQueue()
51    stereoOut = stereo.depth.createOutputQueue()
52
53    pipeline.start()
54    lastPrintTime = 0.0
55    while pipeline.isRunning():
56        colorFrame = colorOut.get()
57        stereoFrame = stereoOut.get()
58
59        assert colorFrame.validateTransformations()
60        assert stereoFrame.validateTransformations()
61
62        clr = colorFrame.getCvFrame()
63        depth = processDepthFrame(stereoFrame)
64
65        rect = dai.RotatedRect(dai.Point2f(300, 200), dai.Size2f(200, 100), 10)
66        remappedRect = colorFrame.getTransformation().remapRectTo(stereoFrame.getTransformation(), rect)
67
68        now = time.monotonic()
69        if now - lastPrintTime >= 1.0:
70            print(f"Original rect x: {rect.center.x} y: {rect.center.y} width: {rect.size.width} height: {rect.size.height} angle: {rect.angle}")
71            print(f"Remapped rect x: {remappedRect.center.x} y: {remappedRect.center.y} width: {remappedRect.size.width} height: {remappedRect.size.height} angle: {remappedRect.angle}")
72            lastPrintTime = now
73
74        draw_rotated_rectangle(clr, (rect.center.x, rect.center.y), (rect.size.width, rect.size.height), rect.angle, (255, 0, 0))
75        draw_rotated_rectangle(depth, (remappedRect.center.x, remappedRect.center.y), (remappedRect.size.width, remappedRect.size.height), remappedRect.angle, (255, 0, 0))
76
77        cv2.imshow("color", clr)
78        cv2.imshow("depth", depth)
79
80        if cv2.waitKey(1) == ord('q'):
81            break
82    pipeline.stop()

1#include <atomic>
2#include <chrono>
3#include <csignal>
4#include <iostream>
5#include <opencv2/opencv.hpp>
6
7#include "depthai/depthai.hpp"
8
9std::atomic<bool> quitEvent(false);
10
11void signalHandler(int) {
12    quitEvent = true;
13}
14
15// Helper function to draw rotated rectangle
16void drawRotatedRectangle(cv::Mat& frame, const cv::Point2f& center, const cv::Size2f& size, float angle, const cv::Scalar& color, int thickness = 2) {
17    // Create a rotated rectangle
18    cv::RotatedRect rect(center, size, angle);
19
20    // Get the four vertices of the rotated rectangle
21    cv::Point2f vertices[4];
22    rect.points(vertices);
23
24    // Convert vertices to integer points
25    std::vector<cv::Point> points;
26    for(int i = 0; i < 4; i++) {
27        points.push_back(cv::Point(static_cast<int>(vertices[i].x), static_cast<int>(vertices[i].y)));
28    }
29
30    // Draw the rectangle
31    cv::polylines(frame, points, true, color, thickness);
32}
33
34// Helper function to process depth frame
35cv::Mat processDepthFrame(const dai::ImgFrame& depthFrame) {
36    return dai::utility::colorizeDepthFrame(depthFrame, 500.0f, 12000.0f, cv::COLORMAP_HOT, true).getCvFrame();
37}
38
39int main() {
40    signal(SIGTERM, signalHandler);
41    signal(SIGINT, signalHandler);
42
43    // Create pipeline
44    dai::Pipeline pipeline;
45
46    // Create and configure nodes
47    auto color = pipeline.create<dai::node::Camera>();
48    color->build(dai::CameraBoardSocket::CAM_A);
49
50    auto monoLeft = pipeline.create<dai::node::Camera>();
51    monoLeft->build(dai::CameraBoardSocket::CAM_B);
52
53    auto monoRight = pipeline.create<dai::node::Camera>();
54    monoRight->build(dai::CameraBoardSocket::CAM_C);
55
56    auto stereo = pipeline.create<dai::node::StereoDepth>();
57
58    // Configure stereo node
59    stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::DEFAULT);
60    // Uncomment to align depth to RGB
61    // stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
62    // stereo->setOutputSize(640, 400);
63
64    // Configure outputs
65    auto colorCamOut = color->requestOutput(std::make_pair(640, 480));
66    auto monoLeftOut = monoLeft->requestOutput(std::make_pair(640, 480));
67    auto monoRightOut = monoRight->requestOutput(std::make_pair(640, 480));
68
69    // Link mono cameras to stereo
70    monoLeftOut->link(stereo->left);
71    monoRightOut->link(stereo->right);
72
73    // Create output queues
74    auto colorOut = colorCamOut->createOutputQueue();
75    auto rightOut = monoRightOut->createOutputQueue();
76    auto stereoOut = stereo->depth.createOutputQueue();
77
78    pipeline.start();
79
80    auto lastPrintTime = std::chrono::steady_clock::now() - std::chrono::seconds(1);
81    while(pipeline.isRunning() && !quitEvent) {
82        auto colorFrame = colorOut->get<dai::ImgFrame>();
83        auto stereoFrame = stereoOut->get<dai::ImgFrame>();
84
85        if(colorFrame == nullptr || stereoFrame == nullptr) continue;
86
87        // Validate transformations
88        if(!colorFrame->validateTransformations() || !stereoFrame->validateTransformations()) {
89            std::cerr << "Invalid transformations!" << std::endl;
90            throw std::runtime_error("Invalid transformations!");
91            continue;
92        }
93
94        // Get frames
95        cv::Mat clr = colorFrame->getCvFrame();
96        cv::Mat depth = processDepthFrame(*stereoFrame);
97
98        // Create and remap rectangle
99        dai::RotatedRect rect(dai::Point2f(300, 200), dai::Size2f(200, 100), 10);
100        auto remappedRect = colorFrame->transformation.remapRectTo(stereoFrame->transformation, rect);
101
102        const auto now = std::chrono::steady_clock::now();
103        if(now - lastPrintTime >= std::chrono::seconds(1)) {
104            // Print rectangle information at most once per second.
105            std::cout << "Original rect x: " << rect.center.x << " y: " << rect.center.y << " width: " << rect.size.width << " height: " << rect.size.height
106                      << " angle: " << rect.angle << std::endl;
107            std::cout << "Remapped rect x: " << remappedRect.center.x << " y: " << remappedRect.center.y << " width: " << remappedRect.size.width
108                      << " height: " << remappedRect.size.height << " angle: " << remappedRect.angle << std::endl;
109            lastPrintTime = now;
110        }
111
112        // Draw rectangles
113        drawRotatedRectangle(clr, cv::Point2f(rect.center.x, rect.center.y), cv::Size2f(rect.size.width, rect.size.height), rect.angle, cv::Scalar(255, 0, 0));
114
115        drawRotatedRectangle(depth,
116                             cv::Point2f(remappedRect.center.x, remappedRect.center.y),
117                             cv::Size2f(remappedRect.size.width, remappedRect.size.height),
118                             remappedRect.angle,
119                             cv::Scalar(255, 0, 0));
120
121        // Show frames
122        cv::imshow("color", clr);
123        cv::imshow("depth", depth);
124
125        if(cv::waitKey(1) == 'q') {
126            break;
127        }
128    }
129
130    pipeline.stop();
131    pipeline.wait();
132
133    return 0;
134}