Stereo Depth Remap

View as Markdown

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

1#include <atomic>
2#include <csignal>
3#include <iostream>
4#include <opencv2/opencv.hpp>
5
6#include "depthai/depthai.hpp"
7
8std::atomic<bool> quitEvent(false);
9
10void signalHandler(int) {
11    quitEvent = true;
12}
13
14// Helper function to draw rotated rectangle
15void drawRotatedRectangle(cv::Mat& frame, const cv::Point2f& center, const cv::Size2f& size, float angle, const cv::Scalar& color, int thickness = 2) {
16    // Create a rotated rectangle
17    cv::RotatedRect rect(center, size, angle);
18
19    // Get the four vertices of the rotated rectangle
20    cv::Point2f vertices[4];
21    rect.points(vertices);
22
23    // Convert vertices to integer points
24    std::vector<cv::Point> points;
25    for(int i = 0; i < 4; i++) {
26        points.push_back(cv::Point(static_cast<int>(vertices[i].x), static_cast<int>(vertices[i].y)));
27    }
28
29    // Draw the rectangle
30    cv::polylines(frame, points, true, color, thickness);
31}
32
33// Helper function to process depth frame
34cv::Mat processDepthFrame(const cv::Mat& depthFrame) {
35    cv::Mat depth_downscaled;
36    cv::resize(depthFrame, depth_downscaled, cv::Size(), 0.25, 0.25);
37
38    double min_depth = 0;
39    if(!cv::countNonZero(depth_downscaled == 0)) {
40        std::vector<uint16_t> nonZeroDepth;
41        nonZeroDepth.reserve(depth_downscaled.rows * depth_downscaled.cols);
42
43        for(int i = 0; i < depth_downscaled.rows; i++) {
44            for(int j = 0; j < depth_downscaled.cols; j++) {
45                uint16_t depth = depth_downscaled.at<uint16_t>(i, j);
46                if(depth > 0) nonZeroDepth.push_back(depth);
47            }
48        }
49
50        if(!nonZeroDepth.empty()) {
51            std::sort(nonZeroDepth.begin(), nonZeroDepth.end());
52            min_depth = nonZeroDepth[static_cast<int>(nonZeroDepth.size() * 0.01)];  // 1st percentile
53        }
54    }
55
56    std::vector<uint16_t> allDepth;
57    allDepth.reserve(depth_downscaled.rows * depth_downscaled.cols);
58    for(int i = 0; i < depth_downscaled.rows; i++) {
59        for(int j = 0; j < depth_downscaled.cols; j++) {
60            allDepth.push_back(depth_downscaled.at<uint16_t>(i, j));
61        }
62    }
63    std::sort(allDepth.begin(), allDepth.end());
64    double max_depth = allDepth[static_cast<int>(allDepth.size() * 0.99)];  // 99th percentile
65
66    // Normalize and colorize
67    cv::Mat normalized;
68    cv::normalize(depthFrame, normalized, 0, 255, cv::NORM_MINMAX, CV_8UC1, depthFrame > min_depth);
69    cv::Mat colorized;
70    cv::applyColorMap(normalized, colorized, cv::COLORMAP_HOT);
71    return colorized;
72}
73
74int main() {
75    signal(SIGTERM, signalHandler);
76    signal(SIGINT, signalHandler);
77
78    // Create pipeline
79    dai::Pipeline pipeline;
80
81    // Create and configure nodes
82    auto color = pipeline.create<dai::node::Camera>();
83    color->build(dai::CameraBoardSocket::CAM_A);
84
85    auto monoLeft = pipeline.create<dai::node::Camera>();
86    monoLeft->build(dai::CameraBoardSocket::CAM_B);
87
88    auto monoRight = pipeline.create<dai::node::Camera>();
89    monoRight->build(dai::CameraBoardSocket::CAM_C);
90
91    auto stereo = pipeline.create<dai::node::StereoDepth>();
92
93    // Configure stereo node
94    stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::DEFAULT);
95    // Uncomment to align depth to RGB
96    // stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
97    // stereo->setOutputSize(640, 400);
98
99    // Configure outputs
100    auto colorCamOut = color->requestOutput(std::make_pair(640, 480));
101    auto monoLeftOut = monoLeft->requestOutput(std::make_pair(640, 480));
102    auto monoRightOut = monoRight->requestOutput(std::make_pair(640, 480));
103
104    // Link mono cameras to stereo
105    monoLeftOut->link(stereo->left);
106    monoRightOut->link(stereo->right);
107
108    // Create output queues
109    auto colorOut = colorCamOut->createOutputQueue();
110    auto rightOut = monoRightOut->createOutputQueue();
111    auto stereoOut = stereo->depth.createOutputQueue();
112
113    pipeline.start();
114
115    while(pipeline.isRunning() && !quitEvent) {
116        auto colorFrame = colorOut->get<dai::ImgFrame>();
117        auto stereoFrame = stereoOut->get<dai::ImgFrame>();
118
119        if(colorFrame == nullptr || stereoFrame == nullptr) continue;
120
121        // Validate transformations
122        if(!colorFrame->validateTransformations() || !stereoFrame->validateTransformations()) {
123            std::cerr << "Invalid transformations!" << std::endl;
124            continue;
125        }
126
127        // Get frames
128        cv::Mat clr = colorFrame->getCvFrame();
129        cv::Mat depth = processDepthFrame(stereoFrame->getCvFrame());
130
131        // Create and remap rectangle
132        dai::RotatedRect rect(dai::Point2f(300, 200), dai::Size2f(200, 100), 10);
133        auto remappedRect = colorFrame->transformation.remapRectTo(stereoFrame->transformation, rect);
134
135        // Print rectangle information
136        std::cout << "Original rect x: " << rect.center.x << " y: " << rect.center.y << " width: " << rect.size.width << " height: " << rect.size.height
137                  << " angle: " << rect.angle << std::endl;
138        std::cout << "Remapped rect x: " << remappedRect.center.x << " y: " << remappedRect.center.y << " width: " << remappedRect.size.width
139                  << " height: " << remappedRect.size.height << " angle: " << remappedRect.angle << std::endl;
140
141        // Draw rectangles
142        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));
143
144        drawRotatedRectangle(depth,
145                             cv::Point2f(remappedRect.center.x, remappedRect.center.y),
146                             cv::Size2f(remappedRect.size.width, remappedRect.size.height),
147                             remappedRect.angle,
148                             cv::Scalar(255, 0, 0));
149
150        // Show frames
151        cv::imshow("color", clr);
152        cv::imshow("depth", depth);
153
154        if(cv::waitKey(1) == 'q') {
155            break;
156        }
157    }
158
159    pipeline.stop();
160    pipeline.wait();
161
162    return 0;
163}