Spatial location calculator¶
This example shows how to retrieve spatial location data (X,Y,Z) on a runtime configurable ROI. You can move the ROI using WASD keys. X,Y,Z coordinates are relative to the center of depth map.
You can also calculate spatial coordiantes on host side, demo here.
Similar samples:
Demo¶

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