Collision avoidance

This example demonstrates how to use DepthAI to implement a collision avoidance system with the OAK-D camera. The script measures objects distance from the camera in real-time, displaying warnings based on predefined distance thresholds.

The script uses stereo cameras to calculate the distance of objects from the camera. The depth map is then aligned to center (color) camera in order to overlay the distance information on the color frame.

User-defined constants `WARNING` and `CRITICAL` are used to define distance thresholds for orange and red alerts respectively.

Demo

Collision Avoidance

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

This example script requires external file(s) to run. If you are using:

  • depthai-python, run python3 examples/install_requirements.py to download required file(s)

  • dephtai-core, required file(s) will get downloaded automatically when building the example

Source code

Python

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
import depthai as dai
import cv2
import numpy as np
import math

# User-defined constants
WARNING = 500  # 50cm, orange
CRITICAL = 300  # 30cm, red

# Create pipeline
pipeline = dai.Pipeline()

# Color camera
camRgb = pipeline.create(dai.node.ColorCamera)
camRgb.setPreviewSize(300, 300)
camRgb.setInterleaved(False)

# Define source - stereo depth cameras
left = pipeline.create(dai.node.MonoCamera)
left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P)
left.setBoardSocket(dai.CameraBoardSocket.LEFT)

right = pipeline.create(dai.node.MonoCamera)
right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_720_P)
right.setBoardSocket(dai.CameraBoardSocket.RIGHT)

# Create stereo depth node
stereo = pipeline.create(dai.node.StereoDepth)
stereo.setConfidenceThreshold(50)
stereo.setLeftRightCheck(True)
stereo.setExtendedDisparity(True)

# Linking
left.out.link(stereo.left)
right.out.link(stereo.right)

# Spatial location calculator configuration
slc = pipeline.create(dai.node.SpatialLocationCalculator)
for x in range(15):
    for y in range(9):
        config = dai.SpatialLocationCalculatorConfigData()
        config.depthThresholds.lowerThreshold = 200
        config.depthThresholds.upperThreshold = 10000
        config.roi = dai.Rect(dai.Point2f((x+0.5)*0.0625, (y+0.5)*0.1), dai.Point2f((x+1.5)*0.0625, (y+1.5)*0.1))
        config.calculationAlgorithm = dai.SpatialLocationCalculatorAlgorithm.MEDIAN
        slc.initialConfig.addROI(config)

stereo.depth.link(slc.inputDepth)
stereo.setDepthAlign(dai.CameraBoardSocket.RGB)

# Create output
slcOut = pipeline.create(dai.node.XLinkOut)
slcOut.setStreamName('slc')
slc.out.link(slcOut.input)

colorOut = pipeline.create(dai.node.XLinkOut)
colorOut.setStreamName('color')
camRgb.video.link(colorOut.input)

# Connect to device and start pipeline
with dai.Device(pipeline) as device:
    # Output queues will be used to get the color mono frames and spatial location data
    qColor = device.getOutputQueue(name="color", maxSize=4, blocking=False)
    qSlc = device.getOutputQueue(name="slc", maxSize=4, blocking=False)

    fontType = cv2.FONT_HERSHEY_TRIPLEX

    while True:
        inColor = qColor.get()  # Try to get a frame from the color camera
        inSlc = qSlc.get()  # Try to get spatial location data

        if inColor is None:
            print("No color camera data")
        if inSlc is None:
            print("No spatial location data")

        colorFrame = None
        if inColor is not None:
            colorFrame = inColor.getCvFrame()  # Fetch the frame from the color mono camera
            

        if inSlc is not None and colorFrame is not None:
            slc_data = inSlc.getSpatialLocations()
            for depthData in slc_data:
                roi = depthData.config.roi
                roi = roi.denormalize(width=colorFrame.shape[1], height=colorFrame.shape[0])

                xmin = int(roi.topLeft().x)
                ymin = int(roi.topLeft().y)
                xmax = int(roi.bottomRight().x)
                ymax = int(roi.bottomRight().y)

                coords = depthData.spatialCoordinates
                distance = math.sqrt(coords.x ** 2 + coords.y ** 2 + coords.z ** 2)

                if distance == 0:  # Invalid
                    continue

                # Determine color based on distance
                if distance < CRITICAL:
                    color = (0, 0, 255)  # Red
                elif distance < WARNING:
                    color = (0, 140, 255)  # Orange
                else:
                    continue  # Skip drawing for non-critical/non-warning distances

                # Draw rectangle and distance text on the color frame
                cv2.rectangle(colorFrame, (xmin, ymin), (xmax, ymax), color, thickness=2)
                cv2.putText(colorFrame, "{:.1f}m".format(distance / 1000), (xmin + 10, ymin + 20), fontType, 0.5, color)

            # Display the color frame
            cv2.imshow('Left Mono Camera', colorFrame)
            if cv2.waitKey(1) == ord('q'):
                break

Got questions?

Head over to Discussion Forum for technical support or any other questions you might have.