Depth Crop Control

1#!/usr/bin/env python3
2
3"""
4This example shows usage of depth camera in crop mode with the possibility to move the crop.
5Use 'WASD' in order to do it.
6"""
7
8import cv2
9import depthai as dai
10import numpy as np
11
12# Step size ('W','A','S','D' controls)
13stepSize = 0.02
14
15# Create pipeline
16pipeline = dai.Pipeline()
17
18# Define sources and outputs
19monoRight = pipeline.create(dai.node.MonoCamera)
20monoLeft = pipeline.create(dai.node.MonoCamera)
21manip = pipeline.create(dai.node.ImageManip)
22stereo = pipeline.create(dai.node.StereoDepth)
23
24configIn = pipeline.create(dai.node.XLinkIn)
25xout = pipeline.create(dai.node.XLinkOut)
26
27configIn.setStreamName('config')
28xout.setStreamName("depth")
29
30# Crop range
31topLeft = dai.Point2f(0.2, 0.2)
32bottomRight = dai.Point2f(0.8, 0.8)
33
34# Properties
35monoRight.setCamera("right")
36monoLeft.setCamera("left")
37monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
38monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
39
40manip.initialConfig.setCropRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y)
41manip.setMaxOutputFrameSize(monoRight.getResolutionHeight()*monoRight.getResolutionWidth()*3)
42stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
43stereo.setSubpixel(True)
44
45# Linking
46configIn.out.link(manip.inputConfig)
47stereo.depth.link(manip.inputImage)
48manip.out.link(xout.input)
49monoRight.out.link(stereo.right)
50monoLeft.out.link(stereo.left)
51
52# Connect to device and start pipeline
53with dai.Device(pipeline) as device:
54
55    # Queues
56    q = device.getOutputQueue(xout.getStreamName(), maxSize=4, blocking=False)
57    configQueue = device.getInputQueue(configIn.getStreamName())
58
59    sendCamConfig = False
60
61    while True:
62        inDepth = q.get()
63        depthFrame = inDepth.getFrame() # depthFrame values are in millimeters
64
65        # Frame is transformed, the color map will be applied to highlight the depth info
66        depth_downscaled = depthFrame[::4]
67        if np.all(depth_downscaled == 0):
68            min_depth = 0  # Set a default minimum depth value when all elements are zero
69        else:
70            min_depth = np.percentile(depth_downscaled[depth_downscaled != 0], 1)
71        max_depth = np.percentile(depth_downscaled, 99)
72        depthFrameColor = np.interp(depthFrame, (min_depth, max_depth), (0, 255)).astype(np.uint8)
73        depthFrameColor = cv2.applyColorMap(depthFrameColor, cv2.COLORMAP_HOT)
74
75        # Frame is ready to be shown
76        cv2.imshow("depth", depthFrameColor)
77
78        # Update screen
79        key = cv2.waitKey(10)
80        if key == ord('q'):
81            break
82        elif key == ord('w'):
83            if topLeft.y - stepSize >= 0:
84                topLeft.y -= stepSize
85                bottomRight.y -= stepSize
86                sendCamConfig = True
87        elif key == ord('a'):
88            if topLeft.x - stepSize >= 0:
89                topLeft.x -= stepSize
90                bottomRight.x -= stepSize
91                sendCamConfig = True
92        elif key == ord('s'):
93            if bottomRight.y + stepSize <= 1:
94                topLeft.y += stepSize
95                bottomRight.y += stepSize
96                sendCamConfig = True
97        elif key == ord('d'):
98            if bottomRight.x + stepSize <= 1:
99                topLeft.x += stepSize
100                bottomRight.x += stepSize
101                sendCamConfig = True
102
103        # Send new config to camera
104        if sendCamConfig:
105            cfg = dai.ImageManipConfig()
106            cfg.setCropRect(topLeft.x, topLeft.y, bottomRight.x, bottomRight.y)
107            configQueue.send(cfg)
108            sendCamConfig = False