Spatial location calculator

1#!/usr/bin/env python3
2
3import cv2
4import depthai as dai
5import numpy as np
6stepSize = 0.05
7
8newConfig = False
9
10# Create pipeline
11pipeline = dai.Pipeline()
12
13# Define sources and outputs
14monoLeft = pipeline.create(dai.node.MonoCamera)
15monoRight = pipeline.create(dai.node.MonoCamera)
16stereo = pipeline.create(dai.node.StereoDepth)
17spatialLocationCalculator = pipeline.create(dai.node.SpatialLocationCalculator)
18
19xoutDepth = pipeline.create(dai.node.XLinkOut)
20xoutSpatialData = pipeline.create(dai.node.XLinkOut)
21xinSpatialCalcConfig = pipeline.create(dai.node.XLinkIn)
22
23xoutDepth.setStreamName("depth")
24xoutSpatialData.setStreamName("spatialData")
25xinSpatialCalcConfig.setStreamName("spatialCalcConfig")
26
27# Properties
28monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
29monoLeft.setCamera("left")
30monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
31monoRight.setCamera("right")
32
33stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
34stereo.setLeftRightCheck(True)
35stereo.setSubpixel(True)
36
37# Config
38topLeft = dai.Point2f(0.4, 0.4)
39bottomRight = dai.Point2f(0.6, 0.6)
40
41config = dai.SpatialLocationCalculatorConfigData()
42config.depthThresholds.lowerThreshold = 100
43config.depthThresholds.upperThreshold = 10000
44calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MEDIAN
45config.roi = dai.Rect(topLeft, bottomRight)
46
47spatialLocationCalculator.inputConfig.setWaitForMessage(False)
48spatialLocationCalculator.initialConfig.addROI(config)
49
50# Linking
51monoLeft.out.link(stereo.left)
52monoRight.out.link(stereo.right)
53
54spatialLocationCalculator.passthroughDepth.link(xoutDepth.input)
55stereo.depth.link(spatialLocationCalculator.inputDepth)
56
57spatialLocationCalculator.out.link(xoutSpatialData.input)
58xinSpatialCalcConfig.out.link(spatialLocationCalculator.inputConfig)
59
60# Connect to device and start pipeline
61with dai.Device(pipeline) as device:
62
63    # Output queue will be used to get the depth frames from the outputs defined above
64    depthQueue = device.getOutputQueue(name="depth", maxSize=4, blocking=False)
65    spatialCalcQueue = device.getOutputQueue(name="spatialData", maxSize=4, blocking=False)
66    spatialCalcConfigInQueue = device.getInputQueue("spatialCalcConfig")
67
68    color = (255, 255, 255)
69
70    print("Use WASD keys to move ROI!")
71
72    while True:
73        inDepth = depthQueue.get() # Blocking call, will wait until a new data has arrived
74
75        depthFrame = inDepth.getFrame() # depthFrame values are in millimeters
76
77        depth_downscaled = depthFrame[::4]
78        if np.all(depth_downscaled == 0):
79            min_depth = 0  # Set a default minimum depth value when all elements are zero
80        else:
81            min_depth = np.percentile(depth_downscaled[depth_downscaled != 0], 1)
82        max_depth = np.percentile(depth_downscaled, 99)
83        depthFrameColor = np.interp(depthFrame, (min_depth, max_depth), (0, 255)).astype(np.uint8)
84        depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_HOT)
85
86        spatialData = spatialCalcQueue.get().getSpatialLocations()
87        for depthData in spatialData:
88            roi = depthData.config.roi
89            roi = roi.denormalize(width=depthFrameColor.shape[1], height=depthFrameColor.shape[0])
90            xmin = int(roi.topLeft().x)
91            ymin = int(roi.topLeft().y)
92            xmax = int(roi.bottomRight().x)
93            ymax = int(roi.bottomRight().y)
94
95            depthMin = depthData.depthMin
96            depthMax = depthData.depthMax
97
98            fontType = cv2.FONT_HERSHEY_TRIPLEX
99            cv2.rectangle(depthFrameColor, (xmin, ymin), (xmax, ymax), color, 1)
100            cv2.putText(depthFrameColor, f"X: {int(depthData.spatialCoordinates.x)} mm", (xmin + 10, ymin + 20), fontType, 0.5, color)
101            cv2.putText(depthFrameColor, f"Y: {int(depthData.spatialCoordinates.y)} mm", (xmin + 10, ymin + 35), fontType, 0.5, color)
102            cv2.putText(depthFrameColor, f"Z: {int(depthData.spatialCoordinates.z)} mm", (xmin + 10, ymin + 50), fontType, 0.5, color)
103        # Show the frame
104        cv2.imshow("depth", depthFrameColor)
105
106        key = cv2.waitKey(1)
107        if key == ord('q'):
108            break
109        elif key == ord('w'):
110            if topLeft.y - stepSize >= 0:
111                topLeft.y -= stepSize
112                bottomRight.y -= stepSize
113                newConfig = True
114        elif key == ord('a'):
115            if topLeft.x - stepSize >= 0:
116                topLeft.x -= stepSize
117                bottomRight.x -= stepSize
118                newConfig = True
119        elif key == ord('s'):
120            if bottomRight.y + stepSize <= 1:
121                topLeft.y += stepSize
122                bottomRight.y += stepSize
123                newConfig = True
124        elif key == ord('d'):
125            if bottomRight.x + stepSize <= 1:
126                topLeft.x += stepSize
127                bottomRight.x += stepSize
128                newConfig = True
129        elif key == ord('1'):
130            calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MEAN
131            print('Switching calculation algorithm to MEAN!')
132            newConfig = True
133        elif key == ord('2'):
134            calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MIN
135            print('Switching calculation algorithm to MIN!')
136            newConfig = True
137        elif key == ord('3'):
138            calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MAX
139            print('Switching calculation algorithm to MAX!')
140            newConfig = True
141        elif key == ord('4'):
142            calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MODE
143            print('Switching calculation algorithm to MODE!')
144            newConfig = True
145        elif key == ord('5'):
146            calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MEDIAN
147            print('Switching calculation algorithm to MEDIAN!')
148            newConfig = True
149
150        if newConfig:
151            config.roi = dai.Rect(topLeft, bottomRight)
152            config.calculationAlgorithm = calculationAlgorithm
153            cfg = dai.SpatialLocationCalculatorConfig()
154            cfg.addROI(config)
155            spatialCalcConfigInQueue.send(cfg)
156            newConfig = False

1#include <iostream>
2
3#include "utility.hpp"
4
5// Includes common necessary includes for development using depthai library
6#include "depthai/depthai.hpp"
7
8static constexpr float stepSize = 0.05f;
9
10static std::atomic<bool> newConfig{false};
11
12int main() {
13    using namespace std;
14
15    // Create pipeline
16    dai::Pipeline pipeline;
17
18    // Define sources and outputs
19    auto monoLeft = pipeline.create<dai::node::MonoCamera>();
20    auto monoRight = pipeline.create<dai::node::MonoCamera>();
21    auto stereo = pipeline.create<dai::node::StereoDepth>();
22    auto spatialDataCalculator = pipeline.create<dai::node::SpatialLocationCalculator>();
23
24    auto xoutDepth = pipeline.create<dai::node::XLinkOut>();
25    auto xoutSpatialData = pipeline.create<dai::node::XLinkOut>();
26    auto xinSpatialCalcConfig = pipeline.create<dai::node::XLinkIn>();
27
28    xoutDepth->setStreamName("depth");
29    xoutSpatialData->setStreamName("spatialData");
30    xinSpatialCalcConfig->setStreamName("spatialCalcConfig");
31
32    // Properties
33    monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
34    monoLeft->setCamera("left");
35    monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
36    monoRight->setCamera("right");
37
38    bool lrcheck = false;
39    bool subpixel = false;
40
41    stereo->setDefaultProfilePreset(dai::node::StereoDepth::PresetMode::HIGH_DENSITY);
42    stereo->setLeftRightCheck(lrcheck);
43    stereo->setSubpixel(subpixel);
44
45    // Config
46    dai::Point2f topLeft(0.4f, 0.4f);
47    dai::Point2f bottomRight(0.6f, 0.6f);
48
49    dai::SpatialLocationCalculatorConfigData config;
50    config.depthThresholds.lowerThreshold = 100;
51    config.depthThresholds.upperThreshold = 10000;
52    auto calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEDIAN;
53    config.calculationAlgorithm = calculationAlgorithm;
54    config.roi = dai::Rect(topLeft, bottomRight);
55
56    spatialDataCalculator->inputConfig.setWaitForMessage(false);
57    spatialDataCalculator->initialConfig.addROI(config);
58
59    // Linking
60    monoLeft->out.link(stereo->left);
61    monoRight->out.link(stereo->right);
62
63    spatialDataCalculator->passthroughDepth.link(xoutDepth->input);
64    stereo->depth.link(spatialDataCalculator->inputDepth);
65
66    spatialDataCalculator->out.link(xoutSpatialData->input);
67    xinSpatialCalcConfig->out.link(spatialDataCalculator->inputConfig);
68
69    // Connect to device and start pipeline
70    dai::Device device(pipeline);
71
72    // Output queue will be used to get the depth frames from the outputs defined above
73    auto depthQueue = device.getOutputQueue("depth", 8, false);
74    auto spatialCalcQueue = device.getOutputQueue("spatialData", 8, false);
75    auto spatialCalcConfigInQueue = device.getInputQueue("spatialCalcConfig");
76
77    auto color = cv::Scalar(255, 255, 255);
78
79    std::cout << "Use WASD keys to move ROI!" << std::endl;
80
81    while(true) {
82        auto inDepth = depthQueue->get<dai::ImgFrame>();
83
84        cv::Mat depthFrame = inDepth->getFrame();  // depthFrame values are in millimeters
85        cv::Mat depthFrameColor;
86
87        cv::normalize(depthFrame, depthFrameColor, 255, 0, cv::NORM_INF, CV_8UC1);
88        cv::equalizeHist(depthFrameColor, depthFrameColor);
89        cv::applyColorMap(depthFrameColor, depthFrameColor, cv::COLORMAP_HOT);
90
91        auto spatialData = spatialCalcQueue->get<dai::SpatialLocationCalculatorData>()->getSpatialLocations();
92        for(auto depthData : spatialData) {
93            auto roi = depthData.config.roi;
94            roi = roi.denormalize(depthFrameColor.cols, depthFrameColor.rows);
95            auto xmin = (int)roi.topLeft().x;
96            auto ymin = (int)roi.topLeft().y;
97            auto xmax = (int)roi.bottomRight().x;
98            auto ymax = (int)roi.bottomRight().y;
99
100            auto depthMin = depthData.depthMin;
101            auto depthMax = depthData.depthMax;
102
103            cv::rectangle(depthFrameColor, cv::Rect(cv::Point(xmin, ymin), cv::Point(xmax, ymax)), color, cv::FONT_HERSHEY_SIMPLEX);
104            std::stringstream depthX;
105            depthX << "X: " << (int)depthData.spatialCoordinates.x << " mm";
106            cv::putText(depthFrameColor, depthX.str(), cv::Point(xmin + 10, ymin + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, color);
107            std::stringstream depthY;
108            depthY << "Y: " << (int)depthData.spatialCoordinates.y << " mm";
109            cv::putText(depthFrameColor, depthY.str(), cv::Point(xmin + 10, ymin + 35), cv::FONT_HERSHEY_TRIPLEX, 0.5, color);
110            std::stringstream depthZ;
111            depthZ << "Z: " << (int)depthData.spatialCoordinates.z << " mm";
112            cv::putText(depthFrameColor, depthZ.str(), cv::Point(xmin + 10, ymin + 50), cv::FONT_HERSHEY_TRIPLEX, 0.5, color);
113        }
114        // Show the frame
115        cv::imshow("depth", depthFrameColor);
116
117        int key = cv::waitKey(1);
118        switch(key) {
119            case 'q':
120                return 0;
121            case 'w':
122                if(topLeft.y - stepSize >= 0) {
123                    topLeft.y -= stepSize;
124                    bottomRight.y -= stepSize;
125                    newConfig = true;
126                }
127                break;
128            case 'a':
129                if(topLeft.x - stepSize >= 0) {
130                    topLeft.x -= stepSize;
131                    bottomRight.x -= stepSize;
132                    newConfig = true;
133                }
134                break;
135            case 's':
136                if(bottomRight.y + stepSize <= 1) {
137                    topLeft.y += stepSize;
138                    bottomRight.y += stepSize;
139                    newConfig = true;
140                }
141                break;
142            case 'd':
143                if(bottomRight.x + stepSize <= 1) {
144                    topLeft.x += stepSize;
145                    bottomRight.x += stepSize;
146                    newConfig = true;
147                }
148                break;
149            case '1':
150                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEAN;
151                newConfig = true;
152                std::cout << "Switching calculation algorithm to MEAN!" << std::endl;
153                break;
154            case '2':
155                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MIN;
156                newConfig = true;
157                std::cout << "Switching calculation algorithm to MIN!" << std::endl;
158                break;
159            case '3':
160                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MAX;
161                newConfig = true;
162                std::cout << "Switching calculation algorithm to MAX!" << std::endl;
163                break;
164            case '4':
165                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MODE;
166                newConfig = true;
167                std::cout << "Switching calculation algorithm to MODE!" << std::endl;
168                break;
169            case '5':
170                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEDIAN;
171                newConfig = true;
172                std::cout << "Switching calculation algorithm to MEDIAN!" << std::endl;
173                break;
174            default:
175                break;
176        }
177
178        if(newConfig) {
179            config.roi = dai::Rect(topLeft, bottomRight);
180            config.calculationAlgorithm = calculationAlgorithm;
181            dai::SpatialLocationCalculatorConfig cfg;
182            cfg.addROI(config);
183            spatialCalcConfigInQueue->send(cfg);
184            newConfig = false;
185        }
186    }
187    return 0;
188}