1+ from depthai_sdk import OakCamera
2+ from depthai_sdk .visualize .configs import StereoColor
3+ from depthai_sdk .classes .packets import DepthPacket
4+ import math
5+ import depthai as dai
6+ import cv2
7+
8+ # User-defined constants
9+ WARNING = 1000 # 1m, orange
10+ CRITICAL = 500 # 50cm, red
11+
12+ slc_data = []
13+
14+ def cb (packet : DepthPacket ):
15+ global slc_data
16+ fontType = cv2 .FONT_HERSHEY_TRIPLEX
17+
18+ depthFrameColor = packet .visualizer .draw (packet .frame )
19+
20+ for depthData in slc_data :
21+ roi = depthData .config .roi
22+ roi = roi .denormalize (width = depthFrameColor .shape [1 ], height = depthFrameColor .shape [0 ])
23+
24+ xmin = int (roi .topLeft ().x )
25+ ymin = int (roi .topLeft ().y )
26+ xmax = int (roi .bottomRight ().x )
27+ ymax = int (roi .bottomRight ().y )
28+
29+ coords = depthData .spatialCoordinates
30+ distance = math .sqrt (coords .x ** 2 + coords .y ** 2 + coords .z ** 2 )
31+
32+ if distance == 0 : # Invalid
33+ continue
34+
35+ if distance < CRITICAL :
36+ color = (0 , 0 , 255 )
37+ cv2 .rectangle (depthFrameColor , (xmin , ymin ), (xmax , ymax ), color , thickness = 4 )
38+ cv2 .putText (depthFrameColor , "{:.1f}m" .format (distance / 1000 ), (xmin + 10 , ymin + 20 ), fontType , 0.5 , color )
39+ elif distance < WARNING :
40+ color = (0 , 140 , 255 )
41+ cv2 .rectangle (depthFrameColor , (xmin , ymin ), (xmax , ymax ), color , thickness = 2 )
42+ cv2 .putText (depthFrameColor , "{:.1f}m" .format (distance / 1000 ), (xmin + 10 , ymin + 20 ), fontType , 0.5 , color )
43+
44+ cv2 .imshow ('0_depth' , depthFrameColor )
45+
46+ with OakCamera () as oak :
47+ stereo = oak .create_stereo ('720p' )
48+ # We don't need high fill rate, just very accurate depth, that's why we enable some filters, and
49+ # set the confidence threshold to 50
50+ config = stereo .node .initialConfig .get ()
51+ config .postProcessing .brightnessFilter .minBrightness = 0
52+ config .postProcessing .brightnessFilter .maxBrightness = 255
53+ stereo .node .initialConfig .set (config )
54+ stereo .config_postprocessing (colorize = StereoColor .RGBD , colormap = cv2 .COLORMAP_BONE )
55+ stereo .config_stereo (confidence = 50 , lr_check = True , extended = True )
56+
57+ oak .visualize ([stereo ], fps = True , callback = cb )
58+
59+ oak .build ()
60+
61+ slc = oak .pipeline .create (dai .node .SpatialLocationCalculator )
62+ for x in range (15 ):
63+ for y in range (9 ):
64+ config = dai .SpatialLocationCalculatorConfigData ()
65+ config .depthThresholds .lowerThreshold = 200
66+ config .depthThresholds .upperThreshold = 10000
67+ 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 ))
68+ # TODO: change from median to 10th percentile once supported
69+ config .calculationAlgorithm = dai .SpatialLocationCalculatorAlgorithm .MEDIAN
70+ slc .initialConfig .addROI (config )
71+
72+ stereo .depth .link (slc .inputDepth )
73+
74+ slc_out = oak .pipeline .create (dai .node .XLinkOut )
75+ slc_out .setStreamName ('slc' )
76+ slc .out .link (slc_out .input )
77+
78+ oak .start () # Start the pipeline (upload it to the OAK)
79+
80+ q = oak .device .getOutputQueue ('slc' ) # Create output queue after calling start()
81+ while oak .running ():
82+ if q .has ():
83+ slc_data = q .get ().getSpatialLocations ()
84+ oak .poll ()
0 commit comments