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 | #!/usr/bin/env python3
import cv2
import depthai as dai
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.setBoardSocket(dai.CameraBoardSocket.LEFT)
monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT)
lrcheck = False
subpixel = False
stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
stereo.setLeftRightCheck(lrcheck)
stereo.setSubpixel(subpixel)
# 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
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
depthFrameColor = cv2.normalize(depthFrame, None, 255, 0, cv2.NORM_INF, cv2.CV_8UC1)
depthFrameColor = cv2.equalizeHist(depthFrameColor)
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, cv2.FONT_HERSHEY_SCRIPT_SIMPLEX)
cv2.putText(depthFrameColor, f"X: {int(depthData.spatialCoordinates.x)} mm", (xmin + 10, ymin + 20), fontType, 0.5, 255)
cv2.putText(depthFrameColor, f"Y: {int(depthData.spatialCoordinates.y)} mm", (xmin + 10, ymin + 35), fontType, 0.5, 255)
cv2.putText(depthFrameColor, f"Z: {int(depthData.spatialCoordinates.z)} mm", (xmin + 10, ymin + 50), fontType, 0.5, 255)
# 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
if newConfig:
config.roi = dai.Rect(topLeft, bottomRight)
config.calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.AVERAGE
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 | #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->setBoardSocket(dai::CameraBoardSocket::LEFT);
monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
monoRight->setBoardSocket(dai::CameraBoardSocket::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;
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;
default:
break;
}
if(newConfig) {
config.roi = dai::Rect(topLeft, bottomRight);
dai::SpatialLocationCalculatorConfig cfg;
cfg.addROI(config);
spatialCalcConfigInQueue->send(cfg);
newConfig = false;
}
}
return 0;
}
|
Got questions?
We’re always happy to help with code or other questions you might have.