Spatial Location Calculator

1#!/usr/bin/env python3
2
3import cv2
4import depthai as dai
5import numpy as np
6from pathlib import Path
7
8color = (255, 255, 255)
9
10# Create pipeline
11pipeline = dai.Pipeline()
12# Config
13topLeft = dai.Point2f(0.4, 0.4)
14bottomRight = dai.Point2f(0.6, 0.6)
15
16# Define sources and outputs
17monoLeft = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B)
18monoRight = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_C)
19stereo = pipeline.create(dai.node.StereoDepth)
20spatialLocationCalculator = pipeline.create(dai.node.SpatialLocationCalculator)
21
22# Linking
23monoLeftOut = monoLeft.requestOutput((640, 400))
24monoRightOut = monoRight.requestOutput((640, 400))
25monoLeftOut.link(stereo.left)
26monoRightOut.link(stereo.right)
27
28stereo.setRectification(True)
29stereo.setExtendedDisparity(True)
30
31stepSize = 0.05
32
33config = dai.SpatialLocationCalculatorConfigData()
34config.depthThresholds.lowerThreshold = 10
35config.depthThresholds.upperThreshold = 10000
36calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MEDIAN
37config.roi = dai.Rect(topLeft, bottomRight)
38
39spatialLocationCalculator.inputConfig.setWaitForMessage(False)
40spatialLocationCalculator.initialConfig.addROI(config)
41
42
43xoutSpatialQueue = spatialLocationCalculator.out.createOutputQueue()
44outputDepthQueue = spatialLocationCalculator.passthroughDepth.createOutputQueue()
45
46stereo.depth.link(spatialLocationCalculator.inputDepth)
47
48
49inputConfigQueue = spatialLocationCalculator.inputConfig.createInputQueue()
50
51with pipeline:
52    pipeline.start()
53    while pipeline.isRunning():
54        spatialData = xoutSpatialQueue.get().getSpatialLocations()
55
56        print("Use WASD keys to move ROI!")
57        outputDepthIMage : dai.ImgFrame = outputDepthQueue.get()
58
59        frameDepth = outputDepthIMage.getCvFrame()
60        frameDepth = outputDepthIMage.getFrame()
61        print("Median depth value: ", np.median(frameDepth))
62
63        depthFrameColor = cv2.normalize(frameDepth, None, 255, 0, cv2.NORM_INF, cv2.CV_8UC1)
64        depthFrameColor = cv2.equalizeHist(depthFrameColor)
65        depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_HOT)
66        for depthData in spatialData:
67            roi = depthData.config.roi
68            roi = roi.denormalize(width=depthFrameColor.shape[1], height=depthFrameColor.shape[0])
69            xmin = int(roi.topLeft().x)
70            ymin = int(roi.topLeft().y)
71            xmax = int(roi.bottomRight().x)
72            ymax = int(roi.bottomRight().y)
73
74            depthMin = depthData.depthMin
75            depthMax = depthData.depthMax
76
77            fontType = cv2.FONT_HERSHEY_TRIPLEX
78            cv2.rectangle(depthFrameColor, (xmin, ymin), (xmax, ymax), color, cv2.FONT_HERSHEY_SCRIPT_SIMPLEX)
79            cv2.putText(depthFrameColor, f"X: {int(depthData.spatialCoordinates.x)} mm", (xmin + 10, ymin + 20), fontType, 0.5, color)
80            cv2.putText(depthFrameColor, f"Y: {int(depthData.spatialCoordinates.y)} mm", (xmin + 10, ymin + 35), fontType, 0.5, color)
81            cv2.putText(depthFrameColor, f"Z: {int(depthData.spatialCoordinates.z)} mm", (xmin + 10, ymin + 50), fontType, 0.5, color)
82        # Show the frame
83        cv2.imshow("depth", depthFrameColor)
84
85        key = cv2.waitKey(1)
86        if key == ord('q'):
87            pipeline.stop()
88            break
89
90        stepSize = 0.05
91
92        newConfig = False
93
94        if key == ord('q'):
95            break
96        elif key == ord('w'):
97            if topLeft.y - stepSize >= 0:
98                topLeft.y -= stepSize
99                bottomRight.y -= stepSize
100                newConfig = True
101        elif key == ord('a'):
102            if topLeft.x - stepSize >= 0:
103                topLeft.x -= stepSize
104                bottomRight.x -= stepSize
105                newConfig = True
106        elif key == ord('s'):
107            if bottomRight.y + stepSize <= 1:
108                topLeft.y += stepSize
109                bottomRight.y += stepSize
110                newConfig = True
111        elif key == ord('d'):
112            if bottomRight.x + stepSize <= 1:
113                topLeft.x += stepSize
114                bottomRight.x += stepSize
115                newConfig = True
116        elif key == ord('1'):
117            calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MEAN
118            print('Switching calculation algorithm to MEAN!')
119            newConfig = True
120        elif key == ord('2'):
121            calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MIN
122            print('Switching calculation algorithm to MIN!')
123            newConfig = True
124        elif key == ord('3'):
125            calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MAX
126            print('Switching calculation algorithm to MAX!')
127            newConfig = True
128        elif key == ord('4'):
129            calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MODE
130            print('Switching calculation algorithm to MODE!')
131            newConfig = True
132        elif key == ord('5'):
133            calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MEDIAN
134            print('Switching calculation algorithm to MEDIAN!')
135            newConfig = True
136
137        if newConfig:
138            config.roi = dai.Rect(topLeft, bottomRight)
139            config.calculationAlgorithm = calculationAlgorithm
140            cfg = dai.SpatialLocationCalculatorConfig()
141            cfg.addROI(config)
142            inputConfigQueue.send(cfg)
143            newConfig = False

1#include <atomic>
2#include <csignal>
3#include <iostream>
4#include <memory>
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
15int main() {
16    signal(SIGTERM, signalHandler);
17    signal(SIGINT, signalHandler);
18
19    try {
20        // Create pipeline
21        dai::Pipeline pipeline;
22
23        // Define sources and outputs
24        auto monoLeft = pipeline.create<dai::node::Camera>();
25        monoLeft->build(dai::CameraBoardSocket::CAM_B);
26
27        auto monoRight = pipeline.create<dai::node::Camera>();
28        monoRight->build(dai::CameraBoardSocket::CAM_C);
29
30        auto stereo = pipeline.create<dai::node::StereoDepth>();
31        auto spatialLocationCalculator = pipeline.create<dai::node::SpatialLocationCalculator>();
32
33        // Configure stereo
34        stereo->setRectification(true);
35        stereo->setExtendedDisparity(true);
36
37        // Initial ROI configuration
38        dai::Point2f topLeft(0.4f, 0.4f);
39        dai::Point2f bottomRight(0.6f, 0.6f);
40        float stepSize = 0.05f;
41
42        // Configure spatial location calculator
43        dai::SpatialLocationCalculatorConfigData config;
44        config.depthThresholds.lowerThreshold = 10;
45        config.depthThresholds.upperThreshold = 10000;
46        auto calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEDIAN;
47        config.roi = dai::Rect(topLeft, bottomRight);
48
49        spatialLocationCalculator->inputConfig.setWaitForMessage(false);
50        spatialLocationCalculator->initialConfig->addROI(config);
51
52        // Create output queues
53        auto xoutSpatialQueue = spatialLocationCalculator->out.createOutputQueue();
54        auto outputDepthQueue = spatialLocationCalculator->passthroughDepth.createOutputQueue();
55        auto inputConfigQueue = spatialLocationCalculator->inputConfig.createInputQueue();
56
57        // Linking
58        monoLeft->requestOutput(std::make_pair(640, 400))->link(stereo->left);
59        monoRight->requestOutput(std::make_pair(640, 400))->link(stereo->right);
60        stereo->depth.link(spatialLocationCalculator->inputDepth);
61
62        // Start pipeline
63        pipeline.start();
64
65        cv::Scalar color(255, 255, 255);
66
67        while(pipeline.isRunning() && !quitEvent) {
68            auto spatialData = xoutSpatialQueue->get<dai::SpatialLocationCalculatorData>();
69            std::cout << "Use WASD keys to move ROI!" << std::endl;
70
71            auto outputDepthImage = outputDepthQueue->get<dai::ImgFrame>();
72            cv::Mat frameDepth = outputDepthImage->getCvFrame();
73
74            // Calculate median depth
75            std::vector<float> depthValues;
76            for(int i = 0; i < frameDepth.rows; i++) {
77                for(int j = 0; j < frameDepth.cols; j++) {
78                    uint16_t val = frameDepth.at<uint16_t>(i, j);
79                    if(val > 0) depthValues.push_back(val);
80                }
81            }
82            if(depthValues.empty()) {
83                std::cout << "Median depth value: N/A (no valid depth pixels)" << std::endl;
84            } else {
85                std::sort(depthValues.begin(), depthValues.end());
86                float medianDepth = depthValues[depthValues.size() / 2];
87                std::cout << "Median depth value: " << medianDepth << std::endl;
88            }
89
90            // Process depth frame for visualization
91            cv::Mat depthFrameColor;
92            cv::normalize(frameDepth, depthFrameColor, 255, 0, cv::NORM_INF, CV_8UC1);
93            cv::equalizeHist(depthFrameColor, depthFrameColor);
94            cv::applyColorMap(depthFrameColor, depthFrameColor, cv::COLORMAP_HOT);
95
96            // Draw spatial data
97            for(const auto& depthData : spatialData->spatialLocations) {
98                auto roi = depthData.config.roi;
99                roi = roi.denormalize(depthFrameColor.cols, depthFrameColor.rows);
100                int xmin = static_cast<int>(roi.topLeft().x);
101                int ymin = static_cast<int>(roi.topLeft().y);
102                int xmax = static_cast<int>(roi.bottomRight().x);
103                int ymax = static_cast<int>(roi.bottomRight().y);
104
105                float depthMin = depthData.depthMin;
106                float depthMax = depthData.depthMax;
107
108                cv::rectangle(depthFrameColor, cv::Point(xmin, ymin), cv::Point(xmax, ymax), color, 1);
109                cv::putText(depthFrameColor,
110                            "X: " + std::to_string(static_cast<int>(depthData.spatialCoordinates.x)) + " mm",
111                            cv::Point(xmin + 10, ymin + 20),
112                            cv::FONT_HERSHEY_TRIPLEX,
113                            0.5,
114                            color);
115                cv::putText(depthFrameColor,
116                            "Y: " + std::to_string(static_cast<int>(depthData.spatialCoordinates.y)) + " mm",
117                            cv::Point(xmin + 10, ymin + 35),
118                            cv::FONT_HERSHEY_TRIPLEX,
119                            0.5,
120                            color);
121                cv::putText(depthFrameColor,
122                            "Z: " + std::to_string(static_cast<int>(depthData.spatialCoordinates.z)) + " mm",
123                            cv::Point(xmin + 10, ymin + 50),
124                            cv::FONT_HERSHEY_TRIPLEX,
125                            0.5,
126                            color);
127            }
128
129            cv::imshow("depth", depthFrameColor);
130
131            int key = cv::waitKey(1);
132            bool newConfig = false;
133
134            if(key == 'q') {
135                break;
136            } else if(key == 'w') {
137                if(topLeft.y - stepSize >= 0) {
138                    topLeft.y -= stepSize;
139                    bottomRight.y -= stepSize;
140                    newConfig = true;
141                }
142            } else if(key == 'a') {
143                if(topLeft.x - stepSize >= 0) {
144                    topLeft.x -= stepSize;
145                    bottomRight.x -= stepSize;
146                    newConfig = true;
147                }
148            } else if(key == 's') {
149                if(bottomRight.y + stepSize <= 1) {
150                    topLeft.y += stepSize;
151                    bottomRight.y += stepSize;
152                    newConfig = true;
153                }
154            } else if(key == 'd') {
155                if(bottomRight.x + stepSize <= 1) {
156                    topLeft.x += stepSize;
157                    bottomRight.x += stepSize;
158                    newConfig = true;
159                }
160            } else if(key == '1') {
161                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEAN;
162                std::cout << "Switching calculation algorithm to MEAN!" << std::endl;
163                newConfig = true;
164            } else if(key == '2') {
165                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MIN;
166                std::cout << "Switching calculation algorithm to MIN!" << std::endl;
167                newConfig = true;
168            } else if(key == '3') {
169                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MAX;
170                std::cout << "Switching calculation algorithm to MAX!" << std::endl;
171                newConfig = true;
172            } else if(key == '4') {
173                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MODE;
174                std::cout << "Switching calculation algorithm to MODE!" << std::endl;
175                newConfig = true;
176            } else if(key == '5') {
177                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEDIAN;
178                std::cout << "Switching calculation algorithm to MEDIAN!" << std::endl;
179                newConfig = true;
180            }
181
182            if(newConfig) {
183                config.roi = dai::Rect(topLeft, bottomRight);
184                config.calculationAlgorithm = calculationAlgorithm;
185                std::shared_ptr<dai::SpatialLocationCalculatorConfig> cfg = std::make_shared<dai::SpatialLocationCalculatorConfig>();
186                cfg->addROI(config);
187                inputConfigQueue->send(cfg);
188            }
189        }
190
191        // Cleanup
192        pipeline.stop();
193        pipeline.wait();
194
195    } catch(const std::exception& e) {
196        std::cerr << "Error: " << e.what() << std::endl;
197        return 1;
198    }
199
200    return 0;
201}