# 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.

This example requires the DepthAI v3 API, see [installation instructions](https://docs.luxonis.com/software-v3/depthai.md).

## Pipeline

## Source code

#### Python

```python
#!/usr/bin/env python3

import cv2
import depthai as dai
import numpy as np
from pathlib import Path

color = (255, 255, 255)

# Create pipeline
pipeline = dai.Pipeline()
# Config
topLeft = dai.Point2f(0.4, 0.4)
bottomRight = dai.Point2f(0.6, 0.6)

# Define sources and outputs
monoLeft = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_B)
monoRight = pipeline.create(dai.node.Camera).build(dai.CameraBoardSocket.CAM_C)
stereo = pipeline.create(dai.node.StereoDepth)
spatialLocationCalculator = pipeline.create(dai.node.SpatialLocationCalculator)

# Linking
monoLeftOut = monoLeft.requestOutput((640, 400))
monoRightOut = monoRight.requestOutput((640, 400))
monoLeftOut.link(stereo.left)
monoRightOut.link(stereo.right)

stereo.setRectification(True)
stereo.setExtendedDisparity(True)

stepSize = 0.05

config = dai.SpatialLocationCalculatorConfigData()
config.depthThresholds.lowerThreshold = 10
config.depthThresholds.upperThreshold = 10000
calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MEDIAN
config.roi = dai.Rect(topLeft, bottomRight)

spatialLocationCalculator.inputConfig.setWaitForMessage(False)
spatialLocationCalculator.initialConfig.addROI(config)

xoutSpatialQueue = spatialLocationCalculator.out.createOutputQueue()
outputDepthQueue = spatialLocationCalculator.passthroughDepth.createOutputQueue()

stereo.depth.link(spatialLocationCalculator.inputDepth)

inputConfigQueue = spatialLocationCalculator.inputConfig.createInputQueue()

with pipeline:
    pipeline.start()
    while pipeline.isRunning():
        spatialData = xoutSpatialQueue.get().getSpatialLocations()

        print("Use WASD keys to move ROI!")
        outputDepthIMage : dai.ImgFrame = outputDepthQueue.get()

        frameDepth = outputDepthIMage.getCvFrame()
        frameDepth = outputDepthIMage.getFrame()
        print("Median depth value: ", np.median(frameDepth))

        depthFrameColor = cv2.normalize(frameDepth, None, 255, 0, cv2.NORM_INF, cv2.CV_8UC1)
        depthFrameColor = cv2.equalizeHist(depthFrameColor)
        depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_HOT)
        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, 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'):
            pipeline.stop()
            break

        stepSize = 0.05

        newConfig = False

        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)
            inputConfigQueue.send(cfg)
            newConfig = False
```

#### C++

```cpp
#include <atomic>
#include <csignal>
#include <iostream>
#include <memory>
#include <opencv2/opencv.hpp>

#include "depthai/depthai.hpp"

std::atomic<bool> quitEvent(false);

void signalHandler(int) {
    quitEvent = true;
}

int main() {
    signal(SIGTERM, signalHandler);
    signal(SIGINT, signalHandler);

    try {
        // Create pipeline
        dai::Pipeline pipeline;

        // Define sources and outputs
        auto monoLeft = pipeline.create<dai::node::Camera>();
        monoLeft->build(dai::CameraBoardSocket::CAM_B);

        auto monoRight = pipeline.create<dai::node::Camera>();
        monoRight->build(dai::CameraBoardSocket::CAM_C);

        auto stereo = pipeline.create<dai::node::StereoDepth>();
        auto spatialLocationCalculator = pipeline.create<dai::node::SpatialLocationCalculator>();

        // Configure stereo
        stereo->setRectification(true);
        stereo->setExtendedDisparity(true);

        // Initial ROI configuration
        dai::Point2f topLeft(0.4f, 0.4f);
        dai::Point2f bottomRight(0.6f, 0.6f);
        float stepSize = 0.05f;

        // Configure spatial location calculator
        dai::SpatialLocationCalculatorConfigData config;
        config.depthThresholds.lowerThreshold = 10;
        config.depthThresholds.upperThreshold = 10000;
        auto calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEDIAN;
        config.roi = dai::Rect(topLeft, bottomRight);

        spatialLocationCalculator->inputConfig.setWaitForMessage(false);
        spatialLocationCalculator->initialConfig->addROI(config);

        // Create output queues
        auto xoutSpatialQueue = spatialLocationCalculator->out.createOutputQueue();
        auto outputDepthQueue = spatialLocationCalculator->passthroughDepth.createOutputQueue();
        auto inputConfigQueue = spatialLocationCalculator->inputConfig.createInputQueue();

        // Linking
        monoLeft->requestOutput(std::make_pair(640, 400))->link(stereo->left);
        monoRight->requestOutput(std::make_pair(640, 400))->link(stereo->right);
        stereo->depth.link(spatialLocationCalculator->inputDepth);

        // Start pipeline
        pipeline.start();

        cv::Scalar color(255, 255, 255);

        while(pipeline.isRunning() && !quitEvent) {
            auto spatialData = xoutSpatialQueue->get<dai::SpatialLocationCalculatorData>();
            std::cout << "Use WASD keys to move ROI!" << std::endl;

            auto outputDepthImage = outputDepthQueue->get<dai::ImgFrame>();
            cv::Mat frameDepth = outputDepthImage->getCvFrame();

            // Calculate median depth
            std::vector<float> depthValues;
            for(int i = 0; i < frameDepth.rows; i++) {
                for(int j = 0; j < frameDepth.cols; j++) {
                    uint16_t val = frameDepth.at<uint16_t>(i, j);
                    if(val > 0) depthValues.push_back(val);
                }
            }
            if(depthValues.empty()) {
                std::cout << "Median depth value: N/A (no valid depth pixels)" << std::endl;
            } else {
                std::sort(depthValues.begin(), depthValues.end());
                float medianDepth = depthValues[depthValues.size() / 2];
                std::cout << "Median depth value: " << medianDepth << std::endl;
            }

            // Process depth frame for visualization
            cv::Mat depthFrameColor;
            cv::normalize(frameDepth, depthFrameColor, 255, 0, cv::NORM_INF, CV_8UC1);
            cv::equalizeHist(depthFrameColor, depthFrameColor);
            cv::applyColorMap(depthFrameColor, depthFrameColor, cv::COLORMAP_HOT);

            // Draw spatial data
            for(const auto& depthData : spatialData->spatialLocations) {
                auto roi = depthData.config.roi;
                roi = roi.denormalize(depthFrameColor.cols, depthFrameColor.rows);
                int xmin = static_cast<int>(roi.topLeft().x);
                int ymin = static_cast<int>(roi.topLeft().y);
                int xmax = static_cast<int>(roi.bottomRight().x);
                int ymax = static_cast<int>(roi.bottomRight().y);

                float depthMin = depthData.depthMin;
                float depthMax = depthData.depthMax;

                cv::rectangle(depthFrameColor, cv::Point(xmin, ymin), cv::Point(xmax, ymax), color, 1);
                cv::putText(depthFrameColor,
                            "X: " + std::to_string(static_cast<int>(depthData.spatialCoordinates.x)) + " mm",
                            cv::Point(xmin + 10, ymin + 20),
                            cv::FONT_HERSHEY_TRIPLEX,
                            0.5,
                            color);
                cv::putText(depthFrameColor,
                            "Y: " + std::to_string(static_cast<int>(depthData.spatialCoordinates.y)) + " mm",
                            cv::Point(xmin + 10, ymin + 35),
                            cv::FONT_HERSHEY_TRIPLEX,
                            0.5,
                            color);
                cv::putText(depthFrameColor,
                            "Z: " + std::to_string(static_cast<int>(depthData.spatialCoordinates.z)) + " mm",
                            cv::Point(xmin + 10, ymin + 50),
                            cv::FONT_HERSHEY_TRIPLEX,
                            0.5,
                            color);
            }

            cv::imshow("depth", depthFrameColor);

            int key = cv::waitKey(1);
            bool newConfig = false;

            if(key == 'q') {
                break;
            } else if(key == 'w') {
                if(topLeft.y - stepSize >= 0) {
                    topLeft.y -= stepSize;
                    bottomRight.y -= stepSize;
                    newConfig = true;
                }
            } else if(key == 'a') {
                if(topLeft.x - stepSize >= 0) {
                    topLeft.x -= stepSize;
                    bottomRight.x -= stepSize;
                    newConfig = true;
                }
            } else if(key == 's') {
                if(bottomRight.y + stepSize <= 1) {
                    topLeft.y += stepSize;
                    bottomRight.y += stepSize;
                    newConfig = true;
                }
            } else if(key == 'd') {
                if(bottomRight.x + stepSize <= 1) {
                    topLeft.x += stepSize;
                    bottomRight.x += stepSize;
                    newConfig = true;
                }
            } else if(key == '1') {
                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEAN;
                std::cout << "Switching calculation algorithm to MEAN!" << std::endl;
                newConfig = true;
            } else if(key == '2') {
                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MIN;
                std::cout << "Switching calculation algorithm to MIN!" << std::endl;
                newConfig = true;
            } else if(key == '3') {
                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MAX;
                std::cout << "Switching calculation algorithm to MAX!" << std::endl;
                newConfig = true;
            } else if(key == '4') {
                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MODE;
                std::cout << "Switching calculation algorithm to MODE!" << std::endl;
                newConfig = true;
            } else if(key == '5') {
                calculationAlgorithm = dai::SpatialLocationCalculatorAlgorithm::MEDIAN;
                std::cout << "Switching calculation algorithm to MEDIAN!" << std::endl;
                newConfig = true;
            }

            if(newConfig) {
                config.roi = dai::Rect(topLeft, bottomRight);
                config.calculationAlgorithm = calculationAlgorithm;
                std::shared_ptr<dai::SpatialLocationCalculatorConfig> cfg = std::make_shared<dai::SpatialLocationCalculatorConfig>();
                cfg->addROI(config);
                inputConfigQueue->send(cfg);
            }
        }

        // Cleanup
        pipeline.stop();
        pipeline.wait();

    } catch(const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        return 1;
    }

    return 0;
}
```

### Need assistance?

Head over to [Discussion Forum](https://discuss.luxonis.com/) for technical support or any other questions you might have.