Stereo Depth from host
This example runs stereo disparity matching on-device using stereo pair images from the host computer. Stereo pair images are pre-rectified, so device doesn't need to perform rectified.There are 3 depth modes which you can select inside the code:lr_check: used for better occlusion handling. For more information click hereextended_disparity: suitable for short range objects. For more information click heresubpixel: suitable for long range. For more information click here
Demo
Setup
This example requires the DepthAI v3 API, see installation instructions.This example also requires dataset folder - you can download it from hereSource code
Python
Python
PythonGitHub
1#!/usr/bin/env python3
2
3import cv2
4import numpy as np
5import depthai as dai
6from time import sleep
7import datetime
8import argparse
9from pathlib import Path
10import math
11import os, re
12import csv
13import datetime
14
15datasetDefault = str((Path(__file__).parent / Path("../models/dataset")).resolve().absolute())
16parser = argparse.ArgumentParser()
17parser.add_argument("-p", "--dataset", nargs="?", help="Path to recorded frames", default=None)
18parser.add_argument("-d", "--debug", action="store_true", help="Enable debug outputs.")
19parser.add_argument("-e", "--evaluate", help="Evaluate the disparity calculation.", default=None)
20parser.add_argument("-dumpdispcost", "--dumpdisparitycostvalues", action="store_true", help="Dumps the disparity cost values for each disparity range. 96 byte for each pixel.")
21parser.add_argument("--download", action="store_true", help="Downloads the 2014 Middlebury dataset.")
22parser.add_argument("--calibration", help="Path to calibration file", default=None)
23parser.add_argument("--rectify", action="store_true", help="Enable rectified streams")
24parser.add_argument("--swapLR", action="store_true", help="Swap left and right cameras.")
25args = parser.parse_args()
26
27if args.evaluate is not None and args.dataset is not None:
28 import sys
29 raise ValueError("Cannot use both --dataset and --evaluate arguments at the same time.")
30
31evaluation_mode = args.evaluate is not None
32args.dataset = args.dataset or datasetDefault
33
34if args.download and args.evaluate is None:
35 import sys
36 raise ValueError("Cannot use --download without --evaluate argument.")
37
38if args.evaluate is None and not Path(args.dataset).exists():
39 import sys
40 raise FileNotFoundError(f"Required file/s not found, please run '{sys.executable} install_requirements.py'")
41
42if args.evaluate is not None and not args.download and not Path(args.evaluate).exists():
43 import sys
44 raise FileNotFoundError(f"Evaluation dataset path does not exist, use the --evaluate argument to specify the path.")
45
46if args.evaluate is not None and args.download and not Path(args.evaluate).exists():
47 os.makedirs(args.evaluate)
48
49def download_2014_middlebury(data_path):
50 import requests, zipfile, io
51 url = "https://vision.middlebury.edu/stereo/data/scenes2014/zip/"
52 r = requests.get(url)
53 c = r.content
54 reg = re.compile(r"href=('|\")(.+\.zip)('|\")")
55 matches = reg.findall(c.decode("utf-8"))
56 files = [m[1] for m in matches]
57
58 for f in files:
59 if os.path.isdir(os.path.join(data_path, f[:-4])):
60 print(f"Skipping {f}")
61 else:
62 print(f"Downloading {f} from {url + f}")
63 r = requests.get(url + f)
64 print(f"Extracting {f} to {data_path}")
65 z = zipfile.ZipFile(io.BytesIO(r.content))
66 z.extractall(data_path)
67
68if args.download:
69 download_2014_middlebury(args.evaluate)
70 if not evaluation_mode:
71 sys.exit(0)
72
73class StereoConfigHandlerRVC2:
74
75 class Trackbar:
76 def __init__(self, trackbarName, windowName, minValue, maxValue, defaultValue, handler):
77 self.min = minValue
78 self.max = maxValue
79 self.windowName = windowName
80 self.trackbarName = trackbarName
81 cv2.createTrackbar(trackbarName, windowName, minValue, maxValue, handler)
82 cv2.setTrackbarPos(trackbarName, windowName, defaultValue)
83
84 def set(self, value):
85 if value < self.min:
86 value = self.min
87 print(f"{self.trackbarName} min value is {self.min}")
88 if value > self.max:
89 value = self.max
90 print(f"{self.trackbarName} max value is {self.max}")
91 cv2.setTrackbarPos(self.trackbarName, self.windowName, value)
92
93 class CensusMaskHandler:
94
95 stateColor = [(0, 0, 0), (255, 255, 255)]
96 gridHeight = 50
97 gridWidth = 50
98
99 def fillRectangle(self, row, col):
100 src = self.gridList[row][col]["topLeft"]
101 dst = self.gridList[row][col]["bottomRight"]
102
103 stateColor = self.stateColor[1] if self.gridList[row][col]["state"] else self.stateColor[0]
104 self.changed = True
105
106 cv2.rectangle(self.gridImage, src, dst, stateColor, -1)
107 cv2.imshow(self.windowName, self.gridImage)
108
109
110 def clickCallback(self, event, x, y, flags, param):
111 if event == cv2.EVENT_LBUTTONDOWN:
112 col = x * (self.gridSize[1]) // self.width
113 row = y * (self.gridSize[0]) // self.height
114 self.gridList[row][col]["state"] = not self.gridList[row][col]["state"]
115 self.fillRectangle(row, col)
116
117
118 def __init__(self, windowName, gridSize):
119 self.gridSize = gridSize
120 self.windowName = windowName
121 self.changed = False
122
123 cv2.namedWindow(self.windowName)
124
125 self.width = StereoConfigHandlerRVC2.CensusMaskHandler.gridWidth * self.gridSize[1]
126 self.height = StereoConfigHandlerRVC2.CensusMaskHandler.gridHeight * self.gridSize[0]
127
128 self.gridImage = np.zeros((self.height + 50, self.width, 3), np.uint8)
129
130 cv2.putText(self.gridImage, "Click on grid to change mask!", (0, self.height+20), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255))
131 cv2.putText(self.gridImage, "White: ON | Black: OFF", (0, self.height+40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255))
132
133 self.gridList = [[dict() for _ in range(self.gridSize[1])] for _ in range(self.gridSize[0])]
134
135 for row in range(self.gridSize[0]):
136 rowFactor = self.height // self.gridSize[0]
137 srcY = row*rowFactor + 1
138 dstY = (row+1)*rowFactor - 1
139 for col in range(self.gridSize[1]):
140 colFactor = self.width // self.gridSize[1]
141 srcX = col*colFactor + 1
142 dstX = (col+1)*colFactor - 1
143 src = (srcX, srcY)
144 dst = (dstX, dstY)
145 self.gridList[row][col]["topLeft"] = src
146 self.gridList[row][col]["bottomRight"] = dst
147 self.gridList[row][col]["state"] = False
148 self.fillRectangle(row, col)
149
150 cv2.setMouseCallback(self.windowName, self.clickCallback)
151 cv2.imshow(self.windowName, self.gridImage)
152
153 def getMask(self) -> np.uint64:
154 mask = np.uint64(0)
155 for row in range(self.gridSize[0]):
156 for col in range(self.gridSize[1]):
157 if self.gridList[row][col]["state"]:
158 pos = row*self.gridSize[1] + col
159 mask = np.bitwise_or(mask, np.uint64(1) << np.uint64(pos))
160
161 return mask
162
163 def setMask(self, _mask: np.uint64):
164 mask = np.uint64(_mask)
165 for row in range(self.gridSize[0]):
166 for col in range(self.gridSize[1]):
167 pos = row*self.gridSize[1] + col
168 if np.bitwise_and(mask, np.uint64(1) << np.uint64(pos)):
169 self.gridList[row][col]["state"] = True
170 else:
171 self.gridList[row][col]["state"] = False
172
173 self.fillRectangle(row, col)
174
175 def isChanged(self):
176 changed = self.changed
177 self.changed = False
178 return changed
179
180 def destroyWindow(self):
181 cv2.destroyWindow(self.windowName)
182
183
184 censusMaskHandler = None
185 newConfig = False
186 config = None
187 trSigma = list()
188 trConfidence = list()
189 trLrCheck = list()
190 trFractionalBits = list()
191 trLineqAlpha = list()
192 trLineqBeta = list()
193 trLineqThreshold = list()
194 trCostAggregationP1 = list()
195 trCostAggregationP2 = list()
196 trTemporalAlpha = list()
197 trTemporalDelta = list()
198 trThresholdMinRange = list()
199 trThresholdMaxRange = list()
200 trSpeckleRange = list()
201 trSpatialAlpha = list()
202 trSpatialDelta = list()
203 trSpatialHoleFilling = list()
204 trSpatialNumIterations = list()
205 trDecimationFactor = list()
206 trDisparityShift = list()
207 trCenterAlignmentShift = list()
208 trInvalidateEdgePixels = list()
209
210 def trackbarSigma(value):
211 StereoConfigHandlerRVC2.config.postProcessing.bilateralSigmaValue = value
212 StereoConfigHandlerRVC2.newConfig = True
213 for tr in StereoConfigHandlerRVC2.trSigma:
214 tr.set(value)
215
216 def trackbarConfidence(value):
217 StereoConfigHandlerRVC2.config.costMatching.confidenceThreshold = value
218 StereoConfigHandlerRVC2.newConfig = True
219 for tr in StereoConfigHandlerRVC2.trConfidence:
220 tr.set(value)
221
222 def trackbarLrCheckThreshold(value):
223 StereoConfigHandlerRVC2.config.algorithmControl.leftRightCheckThreshold = value
224 StereoConfigHandlerRVC2.newConfig = True
225 for tr in StereoConfigHandlerRVC2.trLrCheck:
226 tr.set(value)
227
228 def trackbarFractionalBits(value):
229 StereoConfigHandlerRVC2.config.algorithmControl.subpixelFractionalBits = value
230 StereoConfigHandlerRVC2.newConfig = True
231 for tr in StereoConfigHandlerRVC2.trFractionalBits:
232 tr.set(value)
233
234 def trackbarLineqAlpha(value):
235 StereoConfigHandlerRVC2.config.costMatching.linearEquationParameters.alpha = value
236 StereoConfigHandlerRVC2.newConfig = True
237 for tr in StereoConfigHandlerRVC2.trLineqAlpha:
238 tr.set(value)
239
240 def trackbarLineqBeta(value):
241 StereoConfigHandlerRVC2.config.costMatching.linearEquationParameters.beta = value
242 StereoConfigHandlerRVC2.newConfig = True
243 for tr in StereoConfigHandlerRVC2.trLineqBeta:
244 tr.set(value)
245
246 def trackbarLineqThreshold(value):
247 StereoConfigHandlerRVC2.config.costMatching.linearEquationParameters.threshold = value
248 StereoConfigHandlerRVC2.newConfig = True
249 for tr in StereoConfigHandlerRVC2.trLineqThreshold:
250 tr.set(value)
251
252 def trackbarCostAggregationP1(value):
253 StereoConfigHandlerRVC2.config.costAggregation.horizontalPenaltyCostP1 = value
254 StereoConfigHandlerRVC2.config.costAggregation.verticalPenaltyCostP1 = value
255 StereoConfigHandlerRVC2.newConfig = True
256 for tr in StereoConfigHandlerRVC2.trCostAggregationP1:
257 tr.set(value)
258
259 def trackbarCostAggregationP2(value):
260 StereoConfigHandlerRVC2.config.costAggregation.horizontalPenaltyCostP2 = value
261 StereoConfigHandlerRVC2.config.costAggregation.verticalPenaltyCostP2 = value
262 StereoConfigHandlerRVC2.newConfig = True
263 for tr in StereoConfigHandlerRVC2.trCostAggregationP2:
264 tr.set(value)
265
266 def trackbarTemporalFilterAlpha(value):
267 StereoConfigHandlerRVC2.config.postProcessing.temporalFilter.alpha = value / 100.
268 StereoConfigHandlerRVC2.newConfig = True
269 for tr in StereoConfigHandlerRVC2.trTemporalAlpha:
270 tr.set(value)
271
272 def trackbarTemporalFilterDelta(value):
273 StereoConfigHandlerRVC2.config.postProcessing.temporalFilter.delta = value
274 StereoConfigHandlerRVC2.newConfig = True
275 for tr in StereoConfigHandlerRVC2.trTemporalDelta:
276 tr.set(value)
277
278 def trackbarSpatialFilterAlpha(value):
279 StereoConfigHandlerRVC2.config.postProcessing.spatialFilter.alpha = value / 100.
280 StereoConfigHandlerRVC2.newConfig = True
281 for tr in StereoConfigHandlerRVC2.trSpatialAlpha:
282 tr.set(value)
283
284 def trackbarSpatialFilterDelta(value):
285 StereoConfigHandlerRVC2.config.postProcessing.spatialFilter.delta = value
286 StereoConfigHandlerRVC2.newConfig = True
287 for tr in StereoConfigHandlerRVC2.trSpatialDelta:
288 tr.set(value)
289
290 def trackbarSpatialFilterHoleFillingRadius(value):
291 StereoConfigHandlerRVC2.config.postProcessing.spatialFilter.holeFillingRadius = value
292 StereoConfigHandlerRVC2.newConfig = True
293 for tr in StereoConfigHandlerRVC2.trSpatialHoleFilling:
294 tr.set(value)
295
296 def trackbarSpatialFilterNumIterations(value):
297 StereoConfigHandlerRVC2.config.postProcessing.spatialFilter.numIterations = value
298 StereoConfigHandlerRVC2.newConfig = True
299 for tr in StereoConfigHandlerRVC2.trSpatialNumIterations:
300 tr.set(value)
301
302 def trackbarThresholdMinRange(value):
303 StereoConfigHandlerRVC2.config.postProcessing.thresholdFilter.minRange = value * 1000
304 StereoConfigHandlerRVC2.newConfig = True
305 for tr in StereoConfigHandlerRVC2.trThresholdMinRange:
306 tr.set(value)
307
308 def trackbarThresholdMaxRange(value):
309 StereoConfigHandlerRVC2.config.postProcessing.thresholdFilter.maxRange = value * 1000
310 StereoConfigHandlerRVC2.newConfig = True
311 for tr in StereoConfigHandlerRVC2.trThresholdMaxRange:
312 tr.set(value)
313
314 def trackbarSpeckleRange(value):
315 StereoConfigHandlerRVC2.config.postProcessing.speckleFilter.speckleRange = value
316 StereoConfigHandlerRVC2.newConfig = True
317 for tr in StereoConfigHandlerRVC2.trSpeckleRange:
318 tr.set(value)
319
320 def trackbarDecimationFactor(value):
321 StereoConfigHandlerRVC2.config.postProcessing.decimationFilter.decimationFactor = value
322 StereoConfigHandlerRVC2.newConfig = True
323 for tr in StereoConfigHandlerRVC2.trDecimationFactor:
324 tr.set(value)
325
326 def trackbarDisparityShift(value):
327 StereoConfigHandlerRVC2.config.algorithmControl.disparityShift = value
328 StereoConfigHandlerRVC2.newConfig = True
329 for tr in StereoConfigHandlerRVC2.trDisparityShift:
330 tr.set(value)
331
332 def trackbarCenterAlignmentShift(value):
333 if StereoConfigHandlerRVC2.config.algorithmControl.depthAlign != dai.StereoDepthConfig.AlgorithmControl.DepthAlign.CENTER:
334 print("Center alignment shift factor requires CENTER alignment enabled!")
335 return
336 StereoConfigHandlerRVC2.config.algorithmControl.centerAlignmentShiftFactor = value / 100.
337 print(f"centerAlignmentShiftFactor: {StereoConfigHandlerRVC2.config.algorithmControl.centerAlignmentShiftFactor:.2f}")
338 StereoConfigHandlerRVC2.newConfig = True
339 for tr in StereoConfigHandlerRVC2.trCenterAlignmentShift:
340 tr.set(value)
341
342 def trackbarInvalidateEdgePixels(value):
343 StereoConfigHandlerRVC2.config.algorithmControl.numInvalidateEdgePixels = value
344 print(f"numInvalidateEdgePixels: {StereoConfigHandlerRVC2.config.algorithmControl.numInvalidateEdgePixels:.2f}")
345 StereoConfigHandlerRVC2.newConfig = True
346 for tr in StereoConfigHandlerRVC2.trInvalidateEdgePixels:
347 tr.set(value)
348
349 def handleKeypress(key, stereoDepthConfigInQueue):
350 if key == ord("m"):
351 StereoConfigHandlerRVC2.newConfig = True
352 medianSettings = [dai.MedianFilter.MEDIAN_OFF, dai.MedianFilter.KERNEL_3x3, dai.MedianFilter.KERNEL_5x5, dai.MedianFilter.KERNEL_7x7]
353 currentMedian = StereoConfigHandlerRVC2.config.postProcessing.median
354 nextMedian = medianSettings[(medianSettings.index(currentMedian)+1) % len(medianSettings)]
355 print(f"Changing median to {nextMedian.name} from {currentMedian.name}")
356 StereoConfigHandlerRVC2.config.postProcessing.median = nextMedian
357 if key == ord("w"):
358 StereoConfigHandlerRVC2.newConfig = True
359 StereoConfigHandlerRVC2.config.postProcessing.spatialFilter.enable = not StereoConfigHandlerRVC2.config.postProcessing.spatialFilter.enable
360 state = "on" if StereoConfigHandlerRVC2.config.postProcessing.spatialFilter.enable else "off"
361 print(f"Spatial filter {state}")
362 if key == ord("t"):
363 StereoConfigHandlerRVC2.newConfig = True
364 StereoConfigHandlerRVC2.config.postProcessing.temporalFilter.enable = not StereoConfigHandlerRVC2.config.postProcessing.temporalFilter.enable
365 state = "on" if StereoConfigHandlerRVC2.config.postProcessing.temporalFilter.enable else "off"
366 print(f"Temporal filter {state}")
367 if key == ord("s"):
368 StereoConfigHandlerRVC2.newConfig = True
369 StereoConfigHandlerRVC2.config.postProcessing.speckleFilter.enable = not StereoConfigHandlerRVC2.config.postProcessing.speckleFilter.enable
370 state = "on" if StereoConfigHandlerRVC2.config.postProcessing.speckleFilter.enable else "off"
371 print(f"Speckle filter {state}")
372 if key == ord("r"):
373 StereoConfigHandlerRVC2.newConfig = True
374 temporalSettings = [dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.PERSISTENCY_OFF,
375 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_8_OUT_OF_8,
376 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_IN_LAST_3,
377 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_IN_LAST_4,
378 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_OUT_OF_8,
379 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_2,
380 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_5,
381 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_8,
382 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.PERSISTENCY_INDEFINITELY,
383 ]
384 currentTemporal = StereoConfigHandlerRVC2.config.postProcessing.temporalFilter.persistencyMode
385 nextTemporal = temporalSettings[(temporalSettings.index(currentTemporal)+1) % len(temporalSettings)]
386 print(f"Changing temporal persistency to {nextTemporal.name} from {currentTemporal.name}")
387 StereoConfigHandlerRVC2.config.postProcessing.temporalFilter.persistencyMode = nextTemporal
388 if key == ord("n"):
389 StereoConfigHandlerRVC2.newConfig = True
390 decimationSettings = [dai.StereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.PIXEL_SKIPPING,
391 dai.StereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.NON_ZERO_MEDIAN,
392 dai.StereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.NON_ZERO_MEAN,
393 ]
394 currentDecimation = StereoConfigHandlerRVC2.config.postProcessing.decimationFilter.decimationMode
395 nextDecimation = decimationSettings[(decimationSettings.index(currentDecimation)+1) % len(decimationSettings)]
396 print(f"Changing decimation mode to {nextDecimation.name} from {currentDecimation.name}")
397 StereoConfigHandlerRVC2.config.postProcessing.decimationFilter.decimationMode = nextDecimation
398 if key == ord("a"):
399 StereoConfigHandlerRVC2.newConfig = True
400 alignmentSettings = [dai.StereoDepthConfig.AlgorithmControl.DepthAlign.RECTIFIED_RIGHT,
401 dai.StereoDepthConfig.AlgorithmControl.DepthAlign.RECTIFIED_LEFT,
402 dai.StereoDepthConfig.AlgorithmControl.DepthAlign.CENTER,
403 ]
404 currentAlignment = StereoConfigHandlerRVC2.config.algorithmControl.depthAlign
405 nextAlignment = alignmentSettings[(alignmentSettings.index(currentAlignment)+1) % len(alignmentSettings)]
406 print(f"Changing alignment mode to {nextAlignment.name} from {currentAlignment.name}")
407 StereoConfigHandlerRVC2.config.algorithmControl.depthAlign = nextAlignment
408 elif key == ord("c"):
409 StereoConfigHandlerRVC2.newConfig = True
410 censusSettings = [dai.StereoDepthConfig.CensusTransform.KernelSize.AUTO, dai.StereoDepthConfig.CensusTransform.KernelSize.KERNEL_5x5, dai.StereoDepthConfig.CensusTransform.KernelSize.KERNEL_7x7, dai.StereoDepthConfig.CensusTransform.KernelSize.KERNEL_7x9]
411 currentCensus = StereoConfigHandlerRVC2.config.censusTransform.kernelSize
412 nextCensus = censusSettings[(censusSettings.index(currentCensus)+1) % len(censusSettings)]
413 if nextCensus != dai.StereoDepthConfig.CensusTransform.KernelSize.AUTO:
414 censusGridSize = [(5,5), (7,7), (7,9)]
415 censusDefaultMask = [np.uint64(0XA82415), np.uint64(0XAA02A8154055), np.uint64(0X2AA00AA805540155)]
416 censusGrid = censusGridSize[nextCensus]
417 censusMask = censusDefaultMask[nextCensus]
418 StereoConfigHandlerRVC2.censusMaskHandler = StereoConfigHandlerRVC2.CensusMaskHandler("Census mask", censusGrid)
419 StereoConfigHandlerRVC2.censusMaskHandler.setMask(censusMask)
420 else:
421 print("Census mask config is not available in AUTO census kernel mode. Change using the 'c' key")
422 StereoConfigHandlerRVC2.config.censusTransform.kernelMask = 0
423 StereoConfigHandlerRVC2.censusMaskHandler.destroyWindow()
424 print(f"Changing census transform to {nextCensus.name} from {currentCensus.name}")
425 StereoConfigHandlerRVC2.config.censusTransform.kernelSize = nextCensus
426 elif key == ord("d"):
427 StereoConfigHandlerRVC2.newConfig = True
428 dispRangeSettings = [dai.StereoDepthConfig.CostMatching.DisparityWidth.DISPARITY_64, dai.StereoDepthConfig.CostMatching.DisparityWidth.DISPARITY_96]
429 currentDispRange = StereoConfigHandlerRVC2.config.costMatching.disparityWidth
430 nextDispRange = dispRangeSettings[(dispRangeSettings.index(currentDispRange)+1) % len(dispRangeSettings)]
431 print(f"Changing disparity range to {nextDispRange.name} from {currentDispRange.name}")
432 StereoConfigHandlerRVC2.config.costMatching.disparityWidth = nextDispRange
433 elif key == ord("f"):
434 StereoConfigHandlerRVC2.newConfig = True
435 StereoConfigHandlerRVC2.config.costMatching.enableCompanding = not StereoConfigHandlerRVC2.config.costMatching.enableCompanding
436 state = "on" if StereoConfigHandlerRVC2.config.costMatching.enableCompanding else "off"
437 print(f"Companding {state}")
438 elif key == ord("v"):
439 StereoConfigHandlerRVC2.newConfig = True
440 StereoConfigHandlerRVC2.config.censusTransform.enableMeanMode = not StereoConfigHandlerRVC2.config.censusTransform.enableMeanMode
441 state = "on" if StereoConfigHandlerRVC2.config.censusTransform.enableMeanMode else "off"
442 print(f"Census transform mean mode {state}")
443 elif key == ord("1"):
444 StereoConfigHandlerRVC2.newConfig = True
445 StereoConfigHandlerRVC2.config.algorithmControl.enableLeftRightCheck = not StereoConfigHandlerRVC2.config.algorithmControl.enableLeftRightCheck
446 state = "on" if StereoConfigHandlerRVC2.config.algorithmControl.enableLeftRightCheck else "off"
447 print(f"LR-check {state}")
448 elif key == ord("2"):
449 StereoConfigHandlerRVC2.newConfig = True
450 StereoConfigHandlerRVC2.config.algorithmControl.enableSubpixel = not StereoConfigHandlerRVC2.config.algorithmControl.enableSubpixel
451 state = "on" if StereoConfigHandlerRVC2.config.algorithmControl.enableSubpixel else "off"
452 print(f"Subpixel {state}")
453 elif key == ord("3"):
454 StereoConfigHandlerRVC2.newConfig = True
455 StereoConfigHandlerRVC2.config.algorithmControl.enableExtended = not StereoConfigHandlerRVC2.config.algorithmControl.enableExtended
456 state = "on" if StereoConfigHandlerRVC2.config.algorithmControl.enableExtended else "off"
457 print(f"Extended {state}")
458
459 censusMaskChanged = False
460 if StereoConfigHandlerRVC2.censusMaskHandler is not None:
461 censusMaskChanged = StereoConfigHandlerRVC2.censusMaskHandler.isChanged()
462 if censusMaskChanged:
463 StereoConfigHandlerRVC2.config.censusTransform.kernelMask = StereoConfigHandlerRVC2.censusMaskHandler.getMask()
464 StereoConfigHandlerRVC2.newConfig = True
465
466 StereoConfigHandlerRVC2.sendConfig(stereoDepthConfigInQueue)
467
468 def sendConfig(stereoDepthConfigInQueue):
469 if StereoConfigHandlerRVC2.newConfig:
470 StereoConfigHandlerRVC2.newConfig = False
471 # configMessage = dai.StereoDepthConfig()
472 configMessage = StereoConfigHandlerRVC2.config
473 stereoDepthConfigInQueue.send(configMessage)
474
475 def updateDefaultConfig(config):
476 StereoConfigHandlerRVC2.config = config
477
478 def registerWindow(stream):
479 cv2.namedWindow(stream, cv2.WINDOW_NORMAL)
480
481 StereoConfigHandlerRVC2.trConfidence.append(StereoConfigHandlerRVC2.Trackbar("Disparity confidence", stream, 0, 255, StereoConfigHandlerRVC2.config.costMatching.confidenceThreshold, StereoConfigHandlerRVC2.trackbarConfidence))
482 StereoConfigHandlerRVC2.trSigma.append(StereoConfigHandlerRVC2.Trackbar("Bilateral sigma", stream, 0, 100, StereoConfigHandlerRVC2.config.postProcessing.bilateralSigmaValue, StereoConfigHandlerRVC2.trackbarSigma))
483 StereoConfigHandlerRVC2.trLrCheck.append(StereoConfigHandlerRVC2.Trackbar("LR-check threshold", stream, 0, 16, StereoConfigHandlerRVC2.config.algorithmControl.leftRightCheckThreshold, StereoConfigHandlerRVC2.trackbarLrCheckThreshold))
484 StereoConfigHandlerRVC2.trFractionalBits.append(StereoConfigHandlerRVC2.Trackbar("Subpixel fractional bits", stream, 3, 5, StereoConfigHandlerRVC2.config.algorithmControl.subpixelFractionalBits, StereoConfigHandlerRVC2.trackbarFractionalBits))
485 StereoConfigHandlerRVC2.trDisparityShift.append(StereoConfigHandlerRVC2.Trackbar("Disparity shift", stream, 0, 100, StereoConfigHandlerRVC2.config.algorithmControl.disparityShift, StereoConfigHandlerRVC2.trackbarDisparityShift))
486 StereoConfigHandlerRVC2.trCenterAlignmentShift.append(StereoConfigHandlerRVC2.Trackbar("Center alignment shift factor", stream, 0, 100, StereoConfigHandlerRVC2.config.algorithmControl.centerAlignmentShiftFactor, StereoConfigHandlerRVC2.trackbarCenterAlignmentShift))
487 StereoConfigHandlerRVC2.trInvalidateEdgePixels.append(StereoConfigHandlerRVC2.Trackbar("Invalidate edge pixels", stream, 0, 100, StereoConfigHandlerRVC2.config.algorithmControl.numInvalidateEdgePixels, StereoConfigHandlerRVC2.trackbarInvalidateEdgePixels))
488 StereoConfigHandlerRVC2.trLineqAlpha.append(StereoConfigHandlerRVC2.Trackbar("Linear equation alpha", stream, 0, 15, StereoConfigHandlerRVC2.config.costMatching.linearEquationParameters.alpha, StereoConfigHandlerRVC2.trackbarLineqAlpha))
489 StereoConfigHandlerRVC2.trLineqBeta.append(StereoConfigHandlerRVC2.Trackbar("Linear equation beta", stream, 0, 15, StereoConfigHandlerRVC2.config.costMatching.linearEquationParameters.beta, StereoConfigHandlerRVC2.trackbarLineqBeta))
490 StereoConfigHandlerRVC2.trLineqThreshold.append(StereoConfigHandlerRVC2.Trackbar("Linear equation threshold", stream, 0, 255, StereoConfigHandlerRVC2.config.costMatching.linearEquationParameters.threshold, StereoConfigHandlerRVC2.trackbarLineqThreshold))
491 StereoConfigHandlerRVC2.trCostAggregationP1.append(StereoConfigHandlerRVC2.Trackbar("Cost aggregation P1", stream, 0, 500, StereoConfigHandlerRVC2.config.costAggregation.horizontalPenaltyCostP1, StereoConfigHandlerRVC2.trackbarCostAggregationP1))
492 StereoConfigHandlerRVC2.trCostAggregationP2.append(StereoConfigHandlerRVC2.Trackbar("Cost aggregation P2", stream, 0, 500, StereoConfigHandlerRVC2.config.costAggregation.horizontalPenaltyCostP2, StereoConfigHandlerRVC2.trackbarCostAggregationP2))
493 StereoConfigHandlerRVC2.trTemporalAlpha.append(StereoConfigHandlerRVC2.Trackbar("Temporal filter alpha", stream, 0, 100, int(StereoConfigHandlerRVC2.config.postProcessing.temporalFilter.alpha*100), StereoConfigHandlerRVC2.trackbarTemporalFilterAlpha))
494 StereoConfigHandlerRVC2.trTemporalDelta.append(StereoConfigHandlerRVC2.Trackbar("Temporal filter delta", stream, 0, 100, StereoConfigHandlerRVC2.config.postProcessing.temporalFilter.delta, StereoConfigHandlerRVC2.trackbarTemporalFilterDelta))
495 StereoConfigHandlerRVC2.trSpatialAlpha.append(StereoConfigHandlerRVC2.Trackbar("Spatial filter alpha", stream, 0, 100, int(StereoConfigHandlerRVC2.config.postProcessing.spatialFilter.alpha*100), StereoConfigHandlerRVC2.trackbarSpatialFilterAlpha))
496 StereoConfigHandlerRVC2.trSpatialDelta.append(StereoConfigHandlerRVC2.Trackbar("Spatial filter delta", stream, 0, 100, StereoConfigHandlerRVC2.config.postProcessing.spatialFilter.delta, StereoConfigHandlerRVC2.trackbarSpatialFilterDelta))
497 StereoConfigHandlerRVC2.trSpatialHoleFilling.append(StereoConfigHandlerRVC2.Trackbar("Spatial filter hole filling radius", stream, 0, 16, StereoConfigHandlerRVC2.config.postProcessing.spatialFilter.holeFillingRadius, StereoConfigHandlerRVC2.trackbarSpatialFilterHoleFillingRadius))
498 StereoConfigHandlerRVC2.trSpatialNumIterations.append(StereoConfigHandlerRVC2.Trackbar("Spatial filter number of iterations", stream, 0, 4, StereoConfigHandlerRVC2.config.postProcessing.spatialFilter.numIterations, StereoConfigHandlerRVC2.trackbarSpatialFilterNumIterations))
499 StereoConfigHandlerRVC2.trThresholdMinRange.append(StereoConfigHandlerRVC2.Trackbar("Threshold filter min range", stream, 0, 65, StereoConfigHandlerRVC2.config.postProcessing.thresholdFilter.minRange, StereoConfigHandlerRVC2.trackbarThresholdMinRange))
500 StereoConfigHandlerRVC2.trThresholdMaxRange.append(StereoConfigHandlerRVC2.Trackbar("Threshold filter max range", stream, 0, 65, StereoConfigHandlerRVC2.config.postProcessing.thresholdFilter.maxRange, StereoConfigHandlerRVC2.trackbarThresholdMaxRange))
501 StereoConfigHandlerRVC2.trSpeckleRange.append(StereoConfigHandlerRVC2.Trackbar("Speckle filter range", stream, 0, 240, StereoConfigHandlerRVC2.config.postProcessing.speckleFilter.speckleRange, StereoConfigHandlerRVC2.trackbarSpeckleRange))
502 StereoConfigHandlerRVC2.trDecimationFactor.append(StereoConfigHandlerRVC2.Trackbar("Decimation factor", stream, 1, 4, StereoConfigHandlerRVC2.config.postProcessing.decimationFilter.decimationFactor, StereoConfigHandlerRVC2.trackbarDecimationFactor))
503
504 def __init__(self, config):
505 print("Control median filter using the 'm' key.")
506 print("Control census transform kernel size using the 'c' key.")
507 print("Control disparity search range using the 'd' key.")
508 print("Control disparity companding using the 'f' key.")
509 print("Control census transform mean mode using the 'v' key.")
510 print("Control depth alignment using the 'a' key.")
511 print("Control decimation algorithm using the 'a' key.")
512 print("Control temporal persistency mode using the 'r' key.")
513 print("Control spatial filter using the 'w' key.")
514 print("Control temporal filter using the 't' key.")
515 print("Control speckle filter using the 's' key.")
516 print("Control left-right check mode using the '1' key.")
517 print("Control subpixel mode using the '2' key.")
518 print("Control extended mode using the '3' key.")
519 if evaluation_mode:
520 print("Switch between images using '[' and ']' keys.")
521
522 StereoConfigHandlerRVC2.config = config
523
524 if StereoConfigHandlerRVC2.config.censusTransform.kernelSize != dai.StereoDepthConfig.CensusTransform.KernelSize.AUTO:
525 censusMask = StereoConfigHandlerRVC2.config.censusTransform.kernelMask
526 censusGridSize = [(5,5), (7,7), (7,9)]
527 censusGrid = censusGridSize[StereoConfigHandlerRVC2.config.censusTransform.kernelSize]
528 if StereoConfigHandlerRVC2.config.censusTransform.kernelMask == 0:
529 censusDefaultMask = [np.uint64(0xA82415), np.uint64(0xAA02A8154055), np.uint64(0x2AA00AA805540155)]
530 censusMask = censusDefaultMask[StereoConfigHandlerRVC2.config.censusTransform.kernelSize]
531 StereoConfigHandlerRVC2.censusMaskHandler = StereoConfigHandlerRVC2.CensusMaskHandler("Census mask", censusGrid)
532 StereoConfigHandlerRVC2.censusMaskHandler.setMask(censusMask)
533 else:
534 print("Census mask config is not available in AUTO Census kernel mode. Change using the 'c' key")
535class StereoConfigHandlerRVC4:
536
537 class Trackbar:
538 def __init__(self, trackbarName, windowName, minValue, maxValue, defaultValue, handler):
539 self.min = minValue
540 self.max = maxValue
541 self.windowName = windowName
542 self.trackbarName = trackbarName
543 cv2.createTrackbar(trackbarName, windowName, minValue, maxValue, handler)
544 cv2.setTrackbarPos(trackbarName, windowName, defaultValue)
545
546 def set(self, value):
547 if value < self.min:
548 value = self.min
549 print(f"{self.trackbarName} min value is {self.min}")
550 if value > self.max:
551 value = self.max
552 print(f"{self.trackbarName} max value is {self.max}")
553 cv2.setTrackbarPos(self.trackbarName, self.windowName, value)
554
555 newConfig = False
556 config = None
557 trConfidence = list()
558 trLrCheck = list()
559 trTemporalAlpha = list()
560 trTemporalDelta = list()
561 trThresholdMinRange = list()
562 trThresholdMaxRange = list()
563 trSpeckleRange = list()
564 trSpatialAlpha = list()
565 trSpatialDelta = list()
566 trSpatialHoleFilling = list()
567 trSpatialNumIterations = list()
568 trDecimationFactor = list()
569 trDisparityShift = list()
570 trCenterAlignmentShift = list()
571 trInvalidateEdgePixels = list()
572
573 def trackbarConfidence(value):
574 StereoConfigHandlerRVC4.config.costMatching.confidenceThreshold = value
575 StereoConfigHandlerRVC4.newConfig = True
576 for tr in StereoConfigHandlerRVC4.trConfidence:
577 tr.set(value)
578
579 def trackbarLrCheckThreshold(value):
580 StereoConfigHandlerRVC4.config.algorithmControl.leftRightCheckThreshold = value
581 StereoConfigHandlerRVC4.newConfig = True
582 for tr in StereoConfigHandlerRVC4.trLrCheck:
583 tr.set(value)
584
585 def trackbarTemporalFilterAlpha(value):
586 StereoConfigHandlerRVC4.config.postProcessing.temporalFilter.alpha = value / 100.
587 StereoConfigHandlerRVC4.newConfig = True
588 for tr in StereoConfigHandlerRVC4.trTemporalAlpha:
589 tr.set(value)
590
591 def trackbarTemporalFilterDelta(value):
592 StereoConfigHandlerRVC4.config.postProcessing.temporalFilter.delta = value
593 StereoConfigHandlerRVC4.newConfig = True
594 for tr in StereoConfigHandlerRVC4.trTemporalDelta:
595 tr.set(value)
596
597 def trackbarSpatialFilterAlpha(value):
598 StereoConfigHandlerRVC4.config.postProcessing.spatialFilter.alpha = value / 100.
599 StereoConfigHandlerRVC4.newConfig = True
600 for tr in StereoConfigHandlerRVC4.trSpatialAlpha:
601 tr.set(value)
602
603 def trackbarSpatialFilterDelta(value):
604 StereoConfigHandlerRVC4.config.postProcessing.spatialFilter.delta = value
605 StereoConfigHandlerRVC4.newConfig = True
606 for tr in StereoConfigHandlerRVC4.trSpatialDelta:
607 tr.set(value)
608
609 def trackbarSpatialFilterHoleFillingRadius(value):
610 StereoConfigHandlerRVC4.config.postProcessing.spatialFilter.holeFillingRadius = value
611 StereoConfigHandlerRVC4.newConfig = True
612 for tr in StereoConfigHandlerRVC4.trSpatialHoleFilling:
613 tr.set(value)
614
615 def trackbarSpatialFilterNumIterations(value):
616 StereoConfigHandlerRVC4.config.postProcessing.spatialFilter.numIterations = value
617 StereoConfigHandlerRVC4.newConfig = True
618 for tr in StereoConfigHandlerRVC4.trSpatialNumIterations:
619 tr.set(value)
620
621 def trackbarThresholdMinRange(value):
622 StereoConfigHandlerRVC4.config.postProcessing.thresholdFilter.minRange = value * 1000
623 StereoConfigHandlerRVC4.newConfig = True
624 for tr in StereoConfigHandlerRVC4.trThresholdMinRange:
625 tr.set(value)
626
627 def trackbarThresholdMaxRange(value):
628 StereoConfigHandlerRVC4.config.postProcessing.thresholdFilter.maxRange = value * 1000
629 StereoConfigHandlerRVC4.newConfig = True
630 for tr in StereoConfigHandlerRVC4.trThresholdMaxRange:
631 tr.set(value)
632
633 def trackbarSpeckleRange(value):
634 StereoConfigHandlerRVC4.config.postProcessing.speckleFilter.speckleRange = value
635 StereoConfigHandlerRVC4.newConfig = True
636 for tr in StereoConfigHandlerRVC4.trSpeckleRange:
637 tr.set(value)
638
639 def trackbarDecimationFactor(value):
640 StereoConfigHandlerRVC4.config.postProcessing.decimationFilter.decimationFactor = value
641 StereoConfigHandlerRVC4.newConfig = True
642 for tr in StereoConfigHandlerRVC4.trDecimationFactor:
643 tr.set(value)
644
645 def trackbarDisparityShift(value):
646 StereoConfigHandlerRVC4.config.algorithmControl.disparityShift = value
647 StereoConfigHandlerRVC4.newConfig = True
648 for tr in StereoConfigHandlerRVC4.trDisparityShift:
649 tr.set(value)
650
651 def trackbarCenterAlignmentShift(value):
652 if StereoConfigHandlerRVC4.config.algorithmControl.depthAlign != dai.StereoDepthConfig.AlgorithmControl.DepthAlign.CENTER:
653 print("Center alignment shift factor requires CENTER alignment enabled!")
654 return
655 StereoConfigHandlerRVC4.config.algorithmControl.centerAlignmentShiftFactor = value / 100.
656 print(f"centerAlignmentShiftFactor: {StereoConfigHandlerRVC4.config.algorithmControl.centerAlignmentShiftFactor:.2f}")
657 StereoConfigHandlerRVC4.newConfig = True
658 for tr in StereoConfigHandlerRVC4.trCenterAlignmentShift:
659 tr.set(value)
660
661 def trackbarInvalidateEdgePixels(value):
662 StereoConfigHandlerRVC4.config.algorithmControl.numInvalidateEdgePixels = value
663 print(f"numInvalidateEdgePixels: {StereoConfigHandlerRVC4.config.algorithmControl.numInvalidateEdgePixels:.2f}")
664 StereoConfigHandlerRVC4.newConfig = True
665 for tr in StereoConfigHandlerRVC4.trInvalidateEdgePixels:
666 tr.set(value)
667
668 tr_occlusionConfidenceWeight = list()
669 def trackbar_occlusionConfidenceWeight(value):
670 StereoConfigHandlerRVC4.config.confidenceMetrics.occlusionConfidenceWeight = value
671 print(f"occlusionConfidenceWeight: {StereoConfigHandlerRVC4.config.confidenceMetrics.occlusionConfidenceWeight:.2f}")
672 StereoConfigHandlerRVC4.newConfig = True
673 for tr in StereoConfigHandlerRVC4.tr_occlusionConfidenceWeight:
674 tr.set(value)
675
676 tr_motionVectorConfidenceWeight = list()
677 def trackbar_motionVectorConfidenceWeight(value):
678 StereoConfigHandlerRVC4.config.confidenceMetrics.motionVectorConfidenceWeight = value
679 print(f"motionVectorConfidenceWeight: {StereoConfigHandlerRVC4.config.confidenceMetrics.motionVectorConfidenceWeight:.2f}")
680 StereoConfigHandlerRVC4.newConfig = True
681 for tr in StereoConfigHandlerRVC4.tr_motionVectorConfidenceWeight:
682 tr.set(value)
683
684 tr_motionVectorConfidenceThreshold = list()
685 def trackbar_motionVectorConfidenceThreshold(value):
686 StereoConfigHandlerRVC4.config.confidenceMetrics.motionVectorConfidenceThreshold = value
687 print(f"motionVectorConfidenceThreshold: {StereoConfigHandlerRVC4.config.confidenceMetrics.motionVectorConfidenceThreshold:.2f}")
688 StereoConfigHandlerRVC4.newConfig = True
689 for tr in StereoConfigHandlerRVC4.tr_motionVectorConfidenceThreshold:
690 tr.set(value)
691
692 tr_flatnessConfidenceWeight = list()
693 def trackbar_flatnessConfidenceWeight(value):
694 StereoConfigHandlerRVC4.config.confidenceMetrics.flatnessConfidenceWeight = value
695 print(f"flatnessConfidenceWeight: {StereoConfigHandlerRVC4.config.confidenceMetrics.flatnessConfidenceWeight:.2f}")
696 StereoConfigHandlerRVC4.newConfig = True
697 for tr in StereoConfigHandlerRVC4.tr_flatnessConfidenceWeight:
698 tr.set(value)
699
700 tr_flatnessConfidenceThreshold = list()
701 def trackbar_flatnessConfidenceThreshold(value):
702 StereoConfigHandlerRVC4.config.confidenceMetrics.flatnessConfidenceThreshold = value
703 print(f"flatnessConfidenceThreshold: {StereoConfigHandlerRVC4.config.confidenceMetrics.flatnessConfidenceThreshold:.2f}")
704 StereoConfigHandlerRVC4.newConfig = True
705 for tr in StereoConfigHandlerRVC4.tr_flatnessConfidenceThreshold:
706 tr.set(value)
707
708 tr_flatnessOverride = list()
709 def trackbar_flatnessOverride(value):
710 StereoConfigHandlerRVC4.config.confidenceMetrics.flatnessOverride = bool(value)
711 print(f"flatnessOverride: {StereoConfigHandlerRVC4.config.confidenceMetrics.flatnessOverride:.2f}")
712 StereoConfigHandlerRVC4.newConfig = True
713 for tr in StereoConfigHandlerRVC4.tr_flatnessOverride:
714 tr.set(value)
715
716 tr_adaptiveMedianFilter_enable = list()
717 def trackbar_adaptiveMedianFilter_enable(value):
718 StereoConfigHandlerRVC4.config.postProcessing.adaptiveMedianFilter.enable = bool(value)
719 print(f"adaptiveMedianFilter.enable: {StereoConfigHandlerRVC4.config.postProcessing.adaptiveMedianFilter.enable:.2f}")
720 StereoConfigHandlerRVC4.newConfig = True
721 for tr in StereoConfigHandlerRVC4.tr_adaptiveMedianFilter_enable:
722 tr.set(value)
723
724 tr_adaptiveMedianFilter_threshold = list()
725 def trackbar_adaptiveMedianFilter_threshold(value):
726 StereoConfigHandlerRVC4.config.postProcessing.adaptiveMedianFilter.confidenceThreshold = value
727 print(f"adaptiveMedianFilter_threshold: {StereoConfigHandlerRVC4.config.postProcessing.adaptiveMedianFilter.confidenceThreshold:.2f}")
728 StereoConfigHandlerRVC4.newConfig = True
729 for tr in StereoConfigHandlerRVC4.tr_adaptiveMedianFilter_threshold:
730 tr.set(value)
731
732 tr_noiseThresholdOffset = list()
733 def trackbar_noiseThresholdOffset(value):
734 StereoConfigHandlerRVC4.config.censusTransform.noiseThresholdOffset = value
735 print(f"noiseThresholdOffset: {StereoConfigHandlerRVC4.config.censusTransform.noiseThresholdOffset:.2f}")
736 StereoConfigHandlerRVC4.newConfig = True
737 for tr in StereoConfigHandlerRVC4.tr_noiseThresholdOffset:
738 tr.set(value)
739
740 tr_noiseThresholdScale = list()
741 def trackbar_noiseThresholdScale(value):
742 StereoConfigHandlerRVC4.config.censusTransform.noiseThresholdScale = value - 128
743 print(f"noiseThresholdScale: {StereoConfigHandlerRVC4.config.censusTransform.noiseThresholdScale:.2f}")
744 StereoConfigHandlerRVC4.newConfig = True
745 for tr in StereoConfigHandlerRVC4.tr_noiseThresholdScale:
746 tr.set(value)
747
748 tr_p1_enableAdaptive = list()
749 def trackbar_p1_enableAdaptive(value):
750 StereoConfigHandlerRVC4.config.costAggregation.p1Config.enableAdaptive = bool(value)
751 print(f"p1Config.enableAdaptive: {StereoConfigHandlerRVC4.config.costAggregation.p1Config.enableAdaptive:.2f}")
752 StereoConfigHandlerRVC4.newConfig = True
753 for tr in StereoConfigHandlerRVC4.tr_p1_enableAdaptive:
754 tr.set(value)
755
756 tr_p1_defaultValue = list()
757 def trackbar_p1_defaultValue(value):
758 StereoConfigHandlerRVC4.config.costAggregation.p1Config.defaultValue = value
759 print(f"p1Config.defaultValue: {StereoConfigHandlerRVC4.config.costAggregation.p1Config.defaultValue:.2f}")
760 StereoConfigHandlerRVC4.newConfig = True
761 for tr in StereoConfigHandlerRVC4.tr_p1_defaultValue:
762 tr.set(value)
763
764 tr_p1_edgeValue = list()
765 def trackbar_p1_edgeValue(value):
766 StereoConfigHandlerRVC4.config.costAggregation.p1Config.edgeValue = value
767 print(f"p1Config.edgeValue: {StereoConfigHandlerRVC4.config.costAggregation.p1Config.edgeValue:.2f}")
768 StereoConfigHandlerRVC4.newConfig = True
769 for tr in StereoConfigHandlerRVC4.tr_p1_edgeValue:
770 tr.set(value)
771
772 tr_p1_smoothValue = list()
773 def trackbar_p1_smoothValue(value):
774 StereoConfigHandlerRVC4.config.costAggregation.p1Config.smoothValue = value
775 print(f"p1Config.smoothValue: {StereoConfigHandlerRVC4.config.costAggregation.p1Config.smoothValue:.2f}")
776 StereoConfigHandlerRVC4.newConfig = True
777 for tr in StereoConfigHandlerRVC4.tr_p1_smoothValue:
778 tr.set(value)
779
780 tr_p1_edgeThreshold = list()
781 def trackbar_p1_edgeThreshold(value):
782 StereoConfigHandlerRVC4.config.costAggregation.p1Config.edgeThreshold = value
783 print(f"p1Config.edgeThreshold: {StereoConfigHandlerRVC4.config.costAggregation.p1Config.edgeThreshold:.2f}")
784 StereoConfigHandlerRVC4.newConfig = True
785 for tr in StereoConfigHandlerRVC4.tr_p1_edgeThreshold:
786 tr.set(value)
787
788 tr_p1_smoothThreshold = list()
789 def trackbar_p1_smoothThreshold(value):
790 StereoConfigHandlerRVC4.config.costAggregation.p1Config.smoothThreshold = value
791 print(f"p1Config.smoothThreshold: {StereoConfigHandlerRVC4.config.costAggregation.p1Config.smoothThreshold:.2f}")
792 StereoConfigHandlerRVC4.newConfig = True
793 for tr in StereoConfigHandlerRVC4.tr_p1_smoothThreshold:
794 tr.set(value)
795
796 tr_p2_enableAdaptive = list()
797 def trackbar_p2_enableAdaptive(value):
798 StereoConfigHandlerRVC4.config.costAggregation.p2Config.enableAdaptive = bool(value)
799 print(f"p2Config.enableAdaptive: {StereoConfigHandlerRVC4.config.costAggregation.p2Config.enableAdaptive:.2f}")
800 StereoConfigHandlerRVC4.newConfig = True
801 for tr in StereoConfigHandlerRVC4.tr_p2_enableAdaptive:
802 tr.set(value)
803
804 tr_p2_defaultValue = list()
805 def trackbar_p2_defaultValue(value):
806 StereoConfigHandlerRVC4.config.costAggregation.p2Config.defaultValue = value
807 print(f"p2Config.defaultValue: {StereoConfigHandlerRVC4.config.costAggregation.p2Config.defaultValue:.2f}")
808 StereoConfigHandlerRVC4.newConfig = True
809 for tr in StereoConfigHandlerRVC4.tr_p2_defaultValue:
810 tr.set(value)
811
812 tr_p2_edgeValue = list()
813 def trackbar_p2_edgeValue(value):
814 StereoConfigHandlerRVC4.config.costAggregation.p2Config.edgeValue = value
815 print(f"p2Config.edgeValue: {StereoConfigHandlerRVC4.config.costAggregation.p2Config.edgeValue:.2f}")
816 StereoConfigHandlerRVC4.newConfig = True
817 for tr in StereoConfigHandlerRVC4.tr_p2_edgeValue:
818 tr.set(value)
819
820 tr_p2_smoothValue = list()
821 def trackbar_p2_smoothValue(value):
822 StereoConfigHandlerRVC4.config.costAggregation.p2Config.smoothValue = value
823 print(f"p2Config.smoothValue: {StereoConfigHandlerRVC4.config.costAggregation.p2Config.smoothValue:.2f}")
824 StereoConfigHandlerRVC4.newConfig = True
825 for tr in StereoConfigHandlerRVC4.tr_p2_smoothValue:
826 tr.set(value)
827
828 tr_holefilling_enable = list()
829 def trackbar_holefilling_enable(value):
830 StereoConfigHandlerRVC4.config.postProcessing.holeFilling.enable = bool(value)
831 print(f"holeFilling.enable: {StereoConfigHandlerRVC4.config.postProcessing.holeFilling.enable:.2f}")
832 StereoConfigHandlerRVC4.newConfig = True
833 for tr in StereoConfigHandlerRVC4.tr_holefilling_enable:
834 tr.set(value)
835
836 tr_holefilling_highConfidenceThreshold = list()
837 def trackbar_holefilling_highConfidenceThreshold(value):
838 StereoConfigHandlerRVC4.config.postProcessing.holeFilling.highConfidenceThreshold = value
839 print(f"holeFilling.highConfidenceThreshold: {StereoConfigHandlerRVC4.config.postProcessing.holeFilling.highConfidenceThreshold:.2f}")
840 StereoConfigHandlerRVC4.newConfig = True
841 for tr in StereoConfigHandlerRVC4.tr_holefilling_highConfidenceThreshold:
842 tr.set(value)
843
844 tr_holefilling_fillConfidenceThreshold = list()
845 def trackbar_holefilling_fillConfidenceThreshold(value):
846 StereoConfigHandlerRVC4.config.postProcessing.holeFilling.fillConfidenceThreshold = value
847 print(f"holeFilling.fillConfidenceThreshold: {StereoConfigHandlerRVC4.config.postProcessing.holeFilling.fillConfidenceThreshold:.2f}")
848 StereoConfigHandlerRVC4.newConfig = True
849 for tr in StereoConfigHandlerRVC4.tr_holefilling_fillConfidenceThreshold:
850 tr.set(value)
851
852 tr_holefilling_minValidDisparity = list()
853 def trackbar_holefilling_minValidDisparity(value):
854 StereoConfigHandlerRVC4.config.postProcessing.holeFilling.minValidDisparity = value
855 print(f"holeFilling.minValidDisparity: {StereoConfigHandlerRVC4.config.postProcessing.holeFilling.minValidDisparity:.2f}")
856 StereoConfigHandlerRVC4.newConfig = True
857 for tr in StereoConfigHandlerRVC4.tr_holefilling_minValidDisparity:
858 tr.set(value)
859
860 tr_holefilling_invalidateDisparities = list()
861 def trackbar_holefilling_invalidateDisparities(value):
862 StereoConfigHandlerRVC4.config.postProcessing.holeFilling.invalidateDisparities = bool(value)
863 print(f"holeFilling.invalidateDisparities: {StereoConfigHandlerRVC4.config.postProcessing.holeFilling.invalidateDisparities:.2f}")
864 StereoConfigHandlerRVC4.newConfig = True
865 for tr in StereoConfigHandlerRVC4.tr_holefilling_invalidateDisparities:
866 tr.set(value)
867
868 def handleKeypress(key, stereoDepthConfigInQueue):
869 if key == ord("m"):
870 StereoConfigHandlerRVC4.newConfig = True
871 medianSettings = [dai.MedianFilter.MEDIAN_OFF, dai.MedianFilter.KERNEL_3x3, dai.MedianFilter.KERNEL_5x5]
872 currentMedian = StereoConfigHandlerRVC4.config.postProcessing.median
873 nextMedian = medianSettings[(medianSettings.index(currentMedian)+1) % len(medianSettings)]
874 print(f"Changing median to {nextMedian.name} from {currentMedian.name}")
875 StereoConfigHandlerRVC4.config.postProcessing.median = nextMedian
876 if key == ord("w"):
877 StereoConfigHandlerRVC4.newConfig = True
878 StereoConfigHandlerRVC4.config.postProcessing.spatialFilter.enable = not StereoConfigHandlerRVC4.config.postProcessing.spatialFilter.enable
879 state = "on" if StereoConfigHandlerRVC4.config.postProcessing.spatialFilter.enable else "off"
880 print(f"Spatial filter {state}")
881 if key == ord("t"):
882 StereoConfigHandlerRVC4.newConfig = True
883 StereoConfigHandlerRVC4.config.postProcessing.temporalFilter.enable = not StereoConfigHandlerRVC4.config.postProcessing.temporalFilter.enable
884 state = "on" if StereoConfigHandlerRVC4.config.postProcessing.temporalFilter.enable else "off"
885 print(f"Temporal filter {state}")
886 if key == ord("s"):
887 StereoConfigHandlerRVC4.newConfig = True
888 StereoConfigHandlerRVC4.config.postProcessing.speckleFilter.enable = not StereoConfigHandlerRVC4.config.postProcessing.speckleFilter.enable
889 state = "on" if StereoConfigHandlerRVC4.config.postProcessing.speckleFilter.enable else "off"
890 print(f"Speckle filter {state}")
891 if key == ord("r"):
892 StereoConfigHandlerRVC4.newConfig = True
893 temporalSettings = [dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.PERSISTENCY_OFF,
894 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_8_OUT_OF_8,
895 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_IN_LAST_3,
896 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_IN_LAST_4,
897 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_OUT_OF_8,
898 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_2,
899 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_5,
900 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_8,
901 dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.PERSISTENCY_INDEFINITELY,
902 ]
903 currentTemporal = StereoConfigHandlerRVC4.config.postProcessing.temporalFilter.persistencyMode
904 nextTemporal = temporalSettings[(temporalSettings.index(currentTemporal)+1) % len(temporalSettings)]
905 print(f"Changing temporal persistency to {nextTemporal.name} from {currentTemporal.name}")
906 StereoConfigHandlerRVC4.config.postProcessing.temporalFilter.persistencyMode = nextTemporal
907 if key == ord("n"):
908 StereoConfigHandlerRVC4.newConfig = True
909 decimationSettings = [dai.StereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.PIXEL_SKIPPING,
910 dai.StereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.NON_ZERO_MEDIAN,
911 dai.StereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.NON_ZERO_MEAN,
912 ]
913 currentDecimation = StereoConfigHandlerRVC4.config.postProcessing.decimationFilter.decimationMode
914 nextDecimation = decimationSettings[(decimationSettings.index(currentDecimation)+1) % len(decimationSettings)]
915 print(f"Changing decimation mode to {nextDecimation.name} from {currentDecimation.name}")
916 StereoConfigHandlerRVC4.config.postProcessing.decimationFilter.decimationMode = nextDecimation
917 if key == ord("a"):
918 StereoConfigHandlerRVC4.newConfig = True
919 alignmentSettings = [dai.StereoDepthConfig.AlgorithmControl.DepthAlign.RECTIFIED_RIGHT,
920 dai.StereoDepthConfig.AlgorithmControl.DepthAlign.RECTIFIED_LEFT,
921 dai.StereoDepthConfig.AlgorithmControl.DepthAlign.CENTER,
922 ]
923 currentAlignment = StereoConfigHandlerRVC4.config.algorithmControl.depthAlign
924 nextAlignment = alignmentSettings[(alignmentSettings.index(currentAlignment)+1) % len(alignmentSettings)]
925 print(f"Changing alignment mode to {nextAlignment.name} from {currentAlignment.name}")
926 StereoConfigHandlerRVC4.config.algorithmControl.depthAlign = nextAlignment
927 elif key == ord("1"):
928 StereoConfigHandlerRVC4.newConfig = True
929 StereoConfigHandlerRVC4.config.algorithmControl.enableSwLeftRightCheck = not StereoConfigHandlerRVC4.config.algorithmControl.enableSwLeftRightCheck
930 state = "on" if StereoConfigHandlerRVC4.config.algorithmControl.enableSwLeftRightCheck else "off"
931 print(f"LR-check {state}")
932 elif key == ord("4"):
933 StereoConfigHandlerRVC4.newConfig = True
934 StereoConfigHandlerRVC4.config.costMatching.enableSwConfidenceThresholding = not StereoConfigHandlerRVC4.config.costMatching.enableSwConfidenceThresholding
935 state = "on" if StereoConfigHandlerRVC4.config.costMatching.enableSwConfidenceThresholding else "off"
936 print(f"SW confidence thresholding {state}")
937 elif key == ord("3"):
938 StereoConfigHandlerRVC4.newConfig = True
939 StereoConfigHandlerRVC4.config.algorithmControl.enableExtended = not StereoConfigHandlerRVC4.config.algorithmControl.enableExtended
940 state = "on" if StereoConfigHandlerRVC4.config.algorithmControl.enableExtended else "off"
941 print(f"Extended {state}")
942
943 StereoConfigHandlerRVC4.sendConfig(stereoDepthConfigInQueue)
944
945 def sendConfig(stereoDepthConfigInQueue):
946 if StereoConfigHandlerRVC4.newConfig:
947 StereoConfigHandlerRVC4.newConfig = False
948
949 # configMessage = dai.StereoDepthConfig()
950 configMessage = StereoConfigHandlerRVC4.config
951 if configMessage.confidenceMetrics.occlusionConfidenceWeight + StereoConfigHandlerRVC4.config.confidenceMetrics.motionVectorConfidenceWeight + StereoConfigHandlerRVC4.config.confidenceMetrics.flatnessConfidenceWeight != 32:
952 print("Sum of occlusion, motion vector and flatness confidence weights must be 32")
953
954 stereoDepthConfigInQueue.send(configMessage)
955
956 def updateDefaultConfig(config):
957 StereoConfigHandlerRVC4.config = config
958
959 def registerWindow(stream):
960 cv2.namedWindow(stream, cv2.WINDOW_NORMAL)
961
962 StereoConfigHandlerRVC4.trConfidence.append(StereoConfigHandlerRVC4.Trackbar("Disparity confidence", stream, 0, 255, StereoConfigHandlerRVC4.config.costMatching.confidenceThreshold, StereoConfigHandlerRVC4.trackbarConfidence))
963 StereoConfigHandlerRVC4.trLrCheck.append(StereoConfigHandlerRVC4.Trackbar("LR-check threshold", stream, 0, 16, StereoConfigHandlerRVC4.config.algorithmControl.leftRightCheckThreshold, StereoConfigHandlerRVC4.trackbarLrCheckThreshold))
964 StereoConfigHandlerRVC4.trDisparityShift.append(StereoConfigHandlerRVC4.Trackbar("Disparity shift", stream, 0, 100, StereoConfigHandlerRVC4.config.algorithmControl.disparityShift, StereoConfigHandlerRVC4.trackbarDisparityShift))
965 StereoConfigHandlerRVC4.trCenterAlignmentShift.append(StereoConfigHandlerRVC4.Trackbar("Center alignment shift factor", stream, 0, 100, StereoConfigHandlerRVC4.config.algorithmControl.centerAlignmentShiftFactor, StereoConfigHandlerRVC4.trackbarCenterAlignmentShift))
966 StereoConfigHandlerRVC4.trInvalidateEdgePixels.append(StereoConfigHandlerRVC4.Trackbar("Invalidate edge pixels", stream, 0, 100, StereoConfigHandlerRVC4.config.algorithmControl.numInvalidateEdgePixels, StereoConfigHandlerRVC4.trackbarInvalidateEdgePixels))
967 StereoConfigHandlerRVC4.trTemporalAlpha.append(StereoConfigHandlerRVC4.Trackbar("Temporal filter alpha", stream, 0, 100, int(StereoConfigHandlerRVC4.config.postProcessing.temporalFilter.alpha*100), StereoConfigHandlerRVC4.trackbarTemporalFilterAlpha))
968 StereoConfigHandlerRVC4.trTemporalDelta.append(StereoConfigHandlerRVC4.Trackbar("Temporal filter delta", stream, 0, 100, StereoConfigHandlerRVC4.config.postProcessing.temporalFilter.delta, StereoConfigHandlerRVC4.trackbarTemporalFilterDelta))
969 StereoConfigHandlerRVC4.trSpatialAlpha.append(StereoConfigHandlerRVC4.Trackbar("Spatial filter alpha", stream, 0, 100, int(StereoConfigHandlerRVC4.config.postProcessing.spatialFilter.alpha*100), StereoConfigHandlerRVC4.trackbarSpatialFilterAlpha))
970 StereoConfigHandlerRVC4.trSpatialDelta.append(StereoConfigHandlerRVC4.Trackbar("Spatial filter delta", stream, 0, 100, StereoConfigHandlerRVC4.config.postProcessing.spatialFilter.delta, StereoConfigHandlerRVC4.trackbarSpatialFilterDelta))
971 StereoConfigHandlerRVC4.trSpatialHoleFilling.append(StereoConfigHandlerRVC4.Trackbar("Spatial filter hole filling radius", stream, 0, 16, StereoConfigHandlerRVC4.config.postProcessing.spatialFilter.holeFillingRadius, StereoConfigHandlerRVC4.trackbarSpatialFilterHoleFillingRadius))
972 StereoConfigHandlerRVC4.trSpatialNumIterations.append(StereoConfigHandlerRVC4.Trackbar("Spatial filter number of iterations", stream, 0, 4, StereoConfigHandlerRVC4.config.postProcessing.spatialFilter.numIterations, StereoConfigHandlerRVC4.trackbarSpatialFilterNumIterations))
973 StereoConfigHandlerRVC4.trThresholdMinRange.append(StereoConfigHandlerRVC4.Trackbar("Threshold filter min range", stream, 0, 65, StereoConfigHandlerRVC4.config.postProcessing.thresholdFilter.minRange, StereoConfigHandlerRVC4.trackbarThresholdMinRange))
974 StereoConfigHandlerRVC4.trThresholdMaxRange.append(StereoConfigHandlerRVC4.Trackbar("Threshold filter max range", stream, 0, 65, StereoConfigHandlerRVC4.config.postProcessing.thresholdFilter.maxRange, StereoConfigHandlerRVC4.trackbarThresholdMaxRange))
975 StereoConfigHandlerRVC4.trSpeckleRange.append(StereoConfigHandlerRVC4.Trackbar("Speckle filter range", stream, 0, 240, StereoConfigHandlerRVC4.config.postProcessing.speckleFilter.speckleRange, StereoConfigHandlerRVC4.trackbarSpeckleRange))
976 StereoConfigHandlerRVC4.trDecimationFactor.append(StereoConfigHandlerRVC4.Trackbar("Decimation factor", stream, 1, 4, StereoConfigHandlerRVC4.config.postProcessing.decimationFilter.decimationFactor, StereoConfigHandlerRVC4.trackbarDecimationFactor))
977 StereoConfigHandlerRVC4.tr_occlusionConfidenceWeight.append(StereoConfigHandlerRVC4.Trackbar("Occlusion confidence weight", stream, 0, 32, StereoConfigHandlerRVC4.config.confidenceMetrics.occlusionConfidenceWeight, StereoConfigHandlerRVC4.trackbar_occlusionConfidenceWeight))
978 StereoConfigHandlerRVC4.tr_motionVectorConfidenceWeight.append(StereoConfigHandlerRVC4.Trackbar("Motion vector confidence weight", stream, 0, 32, StereoConfigHandlerRVC4.config.confidenceMetrics.motionVectorConfidenceWeight, StereoConfigHandlerRVC4.trackbar_motionVectorConfidenceWeight))
979 StereoConfigHandlerRVC4.tr_motionVectorConfidenceThreshold.append(StereoConfigHandlerRVC4.Trackbar("Motion vector confidence threshold", stream, 0, 3, StereoConfigHandlerRVC4.config.confidenceMetrics.motionVectorConfidenceThreshold, StereoConfigHandlerRVC4.trackbar_motionVectorConfidenceThreshold))
980 StereoConfigHandlerRVC4.tr_flatnessConfidenceWeight.append(StereoConfigHandlerRVC4.Trackbar("Flatness confidence weight", stream, 0, 32, StereoConfigHandlerRVC4.config.confidenceMetrics.flatnessConfidenceWeight, StereoConfigHandlerRVC4.trackbar_flatnessConfidenceWeight))
981 StereoConfigHandlerRVC4.tr_flatnessConfidenceThreshold.append(StereoConfigHandlerRVC4.Trackbar("Flatness confidence threshold", stream, 1, 7, StereoConfigHandlerRVC4.config.confidenceMetrics.flatnessConfidenceThreshold, StereoConfigHandlerRVC4.trackbar_flatnessConfidenceThreshold))
982 StereoConfigHandlerRVC4.tr_flatnessOverride.append(StereoConfigHandlerRVC4.Trackbar("Flatness override enable", stream, 0, 1, int(StereoConfigHandlerRVC4.config.confidenceMetrics.flatnessOverride), StereoConfigHandlerRVC4.trackbar_flatnessOverride))
983 StereoConfigHandlerRVC4.tr_adaptiveMedianFilter_enable.append(StereoConfigHandlerRVC4.Trackbar("Adaptive median filter enable", stream, 0, 1, int(StereoConfigHandlerRVC4.config.postProcessing.adaptiveMedianFilter.enable), StereoConfigHandlerRVC4.trackbar_adaptiveMedianFilter_enable))
984 StereoConfigHandlerRVC4.tr_adaptiveMedianFilter_threshold.append(StereoConfigHandlerRVC4.Trackbar("Adaptive median filter threshold", stream, 0, 255, StereoConfigHandlerRVC4.config.postProcessing.adaptiveMedianFilter.confidenceThreshold, StereoConfigHandlerRVC4.trackbar_adaptiveMedianFilter_threshold))
985 StereoConfigHandlerRVC4.tr_noiseThresholdOffset.append(StereoConfigHandlerRVC4.Trackbar("Noise threshold offset", stream, 0, 127, StereoConfigHandlerRVC4.config.censusTransform.noiseThresholdOffset, StereoConfigHandlerRVC4.trackbar_noiseThresholdOffset))
986 StereoConfigHandlerRVC4.tr_noiseThresholdScale.append(StereoConfigHandlerRVC4.Trackbar("Noise threshold scale (value - 128)", stream, 0, 255, StereoConfigHandlerRVC4.config.censusTransform.noiseThresholdScale + 128, StereoConfigHandlerRVC4.trackbar_noiseThresholdScale))
987 StereoConfigHandlerRVC4.tr_p1_enableAdaptive.append(StereoConfigHandlerRVC4.Trackbar("P1 enable adaptive", stream, 0, 1, int(StereoConfigHandlerRVC4.config.costAggregation.p1Config.enableAdaptive), StereoConfigHandlerRVC4.trackbar_p1_enableAdaptive))
988 StereoConfigHandlerRVC4.tr_p1_defaultValue.append(StereoConfigHandlerRVC4.Trackbar("P1 default value", stream, 10, 50, StereoConfigHandlerRVC4.config.costAggregation.p1Config.defaultValue, StereoConfigHandlerRVC4.trackbar_p1_defaultValue))
989 StereoConfigHandlerRVC4.tr_p1_edgeValue.append(StereoConfigHandlerRVC4.Trackbar("P1 edge value", stream, 10, 50, StereoConfigHandlerRVC4.config.costAggregation.p1Config.edgeValue, StereoConfigHandlerRVC4.trackbar_p1_edgeValue))
990 StereoConfigHandlerRVC4.tr_p1_smoothValue.append(StereoConfigHandlerRVC4.Trackbar("P1 smooth value", stream, 10, 50, StereoConfigHandlerRVC4.config.costAggregation.p1Config.smoothValue, StereoConfigHandlerRVC4.trackbar_p1_smoothValue))
991 StereoConfigHandlerRVC4.tr_p1_edgeThreshold.append(StereoConfigHandlerRVC4.Trackbar("P1 edge threshold", stream, 8, 16, StereoConfigHandlerRVC4.config.costAggregation.p1Config.edgeThreshold, StereoConfigHandlerRVC4.trackbar_p1_edgeThreshold))
992 StereoConfigHandlerRVC4.tr_p1_smoothThreshold.append(StereoConfigHandlerRVC4.Trackbar("P1 smooth threshold", stream, 2, 12, StereoConfigHandlerRVC4.config.costAggregation.p1Config.smoothThreshold, StereoConfigHandlerRVC4.trackbar_p1_smoothThreshold))
993 StereoConfigHandlerRVC4.tr_p2_enableAdaptive.append(StereoConfigHandlerRVC4.Trackbar("P2 enable adaptive", stream, 0, 1, int(StereoConfigHandlerRVC4.config.costAggregation.p2Config.enableAdaptive), StereoConfigHandlerRVC4.trackbar_p2_enableAdaptive))
994 StereoConfigHandlerRVC4.tr_p2_defaultValue.append(StereoConfigHandlerRVC4.Trackbar("P2 default value", stream, 20, 100, StereoConfigHandlerRVC4.config.costAggregation.p2Config.defaultValue, StereoConfigHandlerRVC4.trackbar_p2_defaultValue))
995 StereoConfigHandlerRVC4.tr_p2_edgeValue.append(StereoConfigHandlerRVC4.Trackbar("P2 edge value", stream, 20, 100, StereoConfigHandlerRVC4.config.costAggregation.p2Config.edgeValue, StereoConfigHandlerRVC4.trackbar_p2_edgeValue))
996 StereoConfigHandlerRVC4.tr_p2_smoothValue.append(StereoConfigHandlerRVC4.Trackbar("P2 smooth value", stream, 20, 100, StereoConfigHandlerRVC4.config.costAggregation.p2Config.smoothValue, StereoConfigHandlerRVC4.trackbar_p2_smoothValue))
997 StereoConfigHandlerRVC4.tr_holefilling_enable.append(StereoConfigHandlerRVC4.Trackbar("Hole filling enable", stream, 0, 1, int(StereoConfigHandlerRVC4.config.postProcessing.holeFilling.enable), StereoConfigHandlerRVC4.trackbar_holefilling_enable))
998 StereoConfigHandlerRVC4.tr_holefilling_highConfidenceThreshold.append(StereoConfigHandlerRVC4.Trackbar("Hole filling high confidence threshold", stream, 0, 255, StereoConfigHandlerRVC4.config.postProcessing.holeFilling.highConfidenceThreshold, StereoConfigHandlerRVC4.trackbar_holefilling_highConfidenceThreshold))
999 StereoConfigHandlerRVC4.tr_holefilling_fillConfidenceThreshold.append(StereoConfigHandlerRVC4.Trackbar("Hole filling fill confidence threshold", stream, 0, 255, StereoConfigHandlerRVC4.config.postProcessing.holeFilling.fillConfidenceThreshold, StereoConfigHandlerRVC4.trackbar_holefilling_fillConfidenceThreshold))
1000 StereoConfigHandlerRVC4.tr_holefilling_minValidDisparity.append(StereoConfigHandlerRVC4.Trackbar("Hole filling min valid disparity", stream, 1, 3, StereoConfigHandlerRVC4.config.postProcessing.holeFilling.minValidDisparity, StereoConfigHandlerRVC4.trackbar_holefilling_minValidDisparity))
1001 StereoConfigHandlerRVC4.tr_holefilling_invalidateDisparities.append(StereoConfigHandlerRVC4.Trackbar("Hole filling invalidate disparities", stream, 0, 1, int(StereoConfigHandlerRVC4.config.postProcessing.holeFilling.invalidateDisparities), StereoConfigHandlerRVC4.trackbar_holefilling_invalidateDisparities))
1002
1003 def __init__(self, config):
1004 print("Control median filter using the 'm' key.")
1005 print("Control depth alignment using the 'a' key.")
1006 print("Control decimation algorithm using the 'a' key.")
1007 print("Control temporal persistency mode using the 'r' key.")
1008 print("Control spatial filter using the 'w' key.")
1009 print("Control temporal filter using the 't' key.")
1010 print("Control speckle filter using the 's' key.")
1011 print("Control left-right check mode using the '1' key.")
1012 print("Control extended mode using the '3' key.")
1013 print("Control SW confidence thresholding using the '4' key.")
1014 if evaluation_mode:
1015 print("Switch between images using '[' and ']' keys.")
1016
1017 StereoConfigHandlerRVC4.config = config
1018
1019
1020# StereoDepth initial config options.
1021outDepth = True # Disparity by default
1022outConfidenceMap = False # Output disparity confidence map
1023outRectified = True # Output and display rectified streams
1024lrcheck = True # Better handling for occlusions
1025extended = True # Closer-in minimum depth, disparity range is doubled. Unsupported for now.
1026subpixel = True # Better accuracy for longer distance, fractional disparity 32-levels
1027
1028width = 1280
1029height = 800
1030
1031xoutStereoCfg = None
1032
1033# Create pipeline
1034pipeline = dai.Pipeline()
1035
1036# Define sources and outputs
1037stereo = pipeline.create(dai.node.StereoDepth)
1038
1039monoLeft = stereo.left.createInputQueue()
1040monoRight = stereo.right.createInputQueue()
1041xinStereoDepthConfig = stereo.inputConfig.createInputQueue()
1042
1043xoutDisparity = stereo.disparity.createOutputQueue()
1044xoutDisparity.setName("disparity")
1045xoutStereoCfg = stereo.outConfig.createOutputQueue()
1046xoutStereoCfg.setName("stereoCfg")
1047xoutLeft = stereo.syncedLeft.createOutputQueue()
1048xoutLeft.setName("left")
1049xoutRight = stereo.syncedRight.createOutputQueue()
1050xoutRight.setName("right")
1051if outDepth:
1052 xoutDepth = stereo.depth.createOutputQueue()
1053 xoutDepth.setName("depth")
1054if outConfidenceMap:
1055 xoutConfMap = stereo.confidenceMap.createOutputQueue()
1056 xoutConfMap.setName("confidenceMap")
1057if outRectified:
1058 xoutRectifLeft = stereo.rectifiedLeft.createOutputQueue()
1059 xoutRectifRight = stereo.rectifiedRight.createOutputQueue()
1060 xoutRectifLeft.setName("rectifiedLeft")
1061 xoutRectifRight.setName("rectifiedRight")
1062
1063if args.debug:
1064 xoutDebugLrCheckIt1 = stereo.debugDispLrCheckIt1.createOutputQueue()
1065 xoutDebugLrCheckIt2 = stereo.debugDispLrCheckIt1.createOutputQueue()
1066 xoutDebugExtLrCheckIt1 = stereo.debugDispLrCheckIt1.createOutputQueue()
1067 xoutDebugExtLrCheckIt2 = stereo.debugDispLrCheckIt1.createOutputQueue()
1068
1069 xoutDebugLrCheckIt1.setName("debugLrCheckIt1")
1070 xoutDebugLrCheckIt2.setName("debugLrCheckIt2")
1071 xoutDebugExtLrCheckIt1.setName("debugExtLrCheckIt1")
1072 xoutDebugExtLrCheckIt2.setName("debugExtLrCheckIt2")
1073
1074if args.dumpdisparitycostvalues:
1075 xoutDebugCostDump = stereo.debugDispCostDump.createOutputQueue()
1076 xoutDebugCostDump.setName("debugCostDump")
1077
1078# Properties
1079stereo.initialConfig.setMedianFilter(dai.MedianFilter.MEDIAN_OFF) # KERNEL_7x7 default
1080stereo.setLeftRightCheck(lrcheck)
1081stereo.setExtendedDisparity(extended)
1082stereo.setSubpixel(subpixel)
1083stereo.initialConfig.setSubpixelFractionalBits(4)
1084
1085stereo.initialConfig.costMatching.disparityWidth = dai.StereoDepthConfig.CostMatching.DisparityWidth.DISPARITY_64
1086
1087stereo.initialConfig.setConfidenceThreshold(127)
1088stereo.initialConfig.setLeftRightCheckThreshold(4)
1089
1090# Switching depthAlign mode at runtime is not supported while aligning to a specific camera is enabled
1091stereo.setDepthAlign(dai.StereoDepthConfig.AlgorithmControl.DepthAlign.RECTIFIED_LEFT)
1092
1093# allocates resources for worst case scenario
1094# allowing runtime switch of stereo modes
1095stereo.setRuntimeModeSwitch(True)
1096
1097currentConfig = stereo.initialConfig
1098
1099platform = pipeline.getDefaultDevice().getPlatform()
1100
1101if platform == dai.Platform.RVC2:
1102 StereoConfigHandler = StereoConfigHandlerRVC2
1103elif platform == dai.Platform.RVC4:
1104 StereoConfigHandler = StereoConfigHandlerRVC4
1105 currentConfig.confidenceMetrics = stereo.initialConfig.confidenceMetrics
1106else:
1107 StereoConfigHandler = StereoConfigHandlerRVC2
1108
1109StereoConfigHandler(currentConfig)
1110StereoConfigHandler.registerWindow("Stereo control panel")
1111
1112if(args.calibration):
1113 calibrationHandler = dai.CalibrationHandler(args.calibration)
1114 pipeline.setCalibrationData(calibrationHandler)
1115stereo.setInputResolution(width, height)
1116stereo.setRectification(args.rectify)
1117baseline = 75
1118fov = 71.86
1119focal = width / (2 * math.tan(fov / 2 / 180 * math.pi))
1120
1121stereo.setBaseline(baseline/10)
1122stereo.setFocalLength(focal)
1123
1124
1125def convertToCv2Frame(name, image, config):
1126
1127 maxDisp = config.getMaxDisparity()
1128 subpixelLevels = pow(2, config.algorithmControl.subpixelFractionalBits)
1129 subpixel = config.algorithmControl.enableSubpixel
1130 dispIntegerLevels = maxDisp if not subpixel else maxDisp / subpixelLevels
1131
1132 frame = image.getFrame()
1133
1134 # frame.tofile(name+".raw")
1135
1136 if name == "depth":
1137 dispScaleFactor = baseline * focal
1138 with np.errstate(divide="ignore"):
1139 frame = dispScaleFactor / frame
1140 if np.isnan(frame).any() or np.isinf(frame).any():
1141 frame = np.nan_to_num(frame, nan=0, posinf=0, neginf=0)
1142
1143 frame = np.clip(frame * 255. / dispIntegerLevels, 0, 255).astype(np.uint8)
1144 frame = cv2.applyColorMap(frame, cv2.COLORMAP_HOT)
1145 elif "confidence_map" in name:
1146 pass
1147 elif name == "disparity_cost_dump":
1148 # frame.tofile(name+".raw")
1149 pass
1150 elif "disparity" in name:
1151 if 1: # Optionally, extend disparity range to better visualize it
1152 frame = (frame * 255. / maxDisp).astype(np.uint8)
1153 return frame
1154 # if 1: # Optionally, apply a color map
1155 # frame = cv2.applyColorMap(frame, cv2.COLORMAP_HOT)
1156
1157 return frame
1158
1159class DatasetManager:
1160 def __init__(self, path):
1161 self.path = path
1162 self.index = 0
1163 self.names = [d for d in os.listdir(path) if os.path.isdir(os.path.join(path, d))]
1164 if len(self.names) == 0:
1165 raise RuntimeError("No dataset found at {}".format(path))
1166
1167 def get(self):
1168 return os.path.join(self.path, self.names[self.index])
1169
1170 def get_name(self):
1171 return self.names[self.index]
1172
1173 def next(self):
1174 self.index = (self.index + 1) % len(self.names)
1175 return self.get()
1176
1177 def prev(self):
1178 self.index = (self.index - 1) % len(self.names)
1179 return self.get()
1180
1181
1182def read_pfm(file):
1183 file = open(file, "rb")
1184
1185 color = None
1186 width = None
1187 height = None
1188 scale = None
1189 endian = None
1190
1191 header = file.readline().rstrip()
1192 if header.decode("ascii") == "PF":
1193 color = True
1194 elif header.decode("ascii") == "Pf":
1195 color = False
1196 else:
1197 raise Exception("Not a PFM file.")
1198
1199 dim_match = re.search(r"(\d+)\s(\d+)", file.readline().decode("ascii"))
1200 if dim_match:
1201 width, height = map(int, dim_match.groups())
1202 else:
1203 raise Exception("Malformed PFM header.")
1204
1205 scale = float(file.readline().rstrip())
1206 if scale < 0: # little-endian
1207 endian = "<"
1208 scale = -scale
1209 else:
1210 endian = ">" # big-endian
1211
1212 data = np.fromfile(file, endian + "f")
1213 shape = (height, width, 3) if color else (height, width)
1214 return np.flip(np.reshape(data, shape), axis=0), scale
1215
1216def calculate_err_measures(gt_img, oak_img):
1217 assert gt_img.shape == oak_img.shape
1218
1219 gt_mask = gt_img != 0
1220 oak_mask = oak_img != 0
1221 mask = gt_mask & oak_mask
1222
1223 gt_img[~gt_mask] = 0.
1224 oak_img[~mask] = 0.
1225 err = np.abs(gt_img - oak_img)
1226
1227 n = np.sum(gt_mask)
1228 invalid = np.sum(gt_mask & ~oak_mask)
1229
1230 bad05 = np.sum(mask & (err > 0.5))
1231 bad1 = np.sum(mask & (err > 1.))
1232 bad2 = np.sum(mask & (err > 2.))
1233 bad4 = np.sum(mask & (err > 4.))
1234 sum_err = np.sum(err[mask])
1235 sum_sq_err = np.sum(err[mask] ** 2)
1236 errs = err[mask]
1237
1238 bad05_p = 100. * bad05 / n
1239 total_bad05_p = 100. * (bad05 + invalid) / n
1240 bad1_p = 100. * bad1 / n
1241 total_bad1_p = 100. * (bad1 + invalid) / n
1242 bad2_p = 100. * bad2 / n
1243 total_bad2_p = 100. * (bad2 + invalid) / n
1244 bad4_p = 100. * bad4 / n
1245 total_bad4_p = 100. * (bad4 + invalid) / n
1246 invalid_p = 100. * invalid / n
1247 if n == invalid:
1248 avg_err = 0.
1249 mse = 0.
1250 else:
1251 avg_err = sum_err / (n - invalid)
1252 mse = sum_sq_err / (n - invalid)
1253 if len(errs) == 0:
1254 a50 = 0.
1255 a90 = 0.
1256 a95 = 0.
1257 a99 = 0.
1258 else:
1259 a50 = np.percentile(errs, 50)
1260 a90 = np.percentile(errs, 90)
1261 a95 = np.percentile(errs, 95)
1262 a99 = np.percentile(errs, 99)
1263
1264 return {
1265 "bad0.5": bad05_p,
1266 "total_bad0.5": total_bad05_p,
1267 "bad1": bad1_p,
1268 "total_bad1": total_bad1_p,
1269 "bad2": bad2_p,
1270 "total_bad2": total_bad2_p,
1271 "bad4": bad4_p,
1272 "total_bad4": total_bad4_p,
1273 "invalid": invalid_p,
1274 "avg_err": avg_err,
1275 "mse": mse,
1276 "a50": a50,
1277 "a90": a90,
1278 "a95": a95,
1279 "a99": a99
1280 }
1281
1282def show_evaluation(img_name, evals):
1283 cv2.namedWindow("Evaluation", cv2.WINDOW_NORMAL)
1284 font = cv2.FONT_HERSHEY_SIMPLEX
1285 font_scale = 2
1286 thickness = 3
1287 color = (0, 0, 0)
1288 lines = [
1289 f"Name: {img_name}",
1290 f"Bad0.5: {evals['bad0.5']:.2f}%",
1291 f"Total Bad0.5: {evals['total_bad0.5']:.2f}%",
1292 f"Bad1: {evals['bad1']:.2f}%",
1293 f"Total Bad1: {evals['total_bad1']:.2f}%",
1294 f"Bad2: {evals['bad2']:.2f}%",
1295 f"Total Bad2: {evals['total_bad2']:.2f}%",
1296 f"Bad4: {evals['bad4']:.2f}%",
1297 f"Total Bad4: {evals['total_bad4']:.2f}%",
1298 f"Invalid: {evals['invalid']:.2f}%",
1299 f"Avg Err: {evals['avg_err']:.2f}",
1300 f"MSE: {evals['mse']:.2f}",
1301 f"A50: {evals['a50']:.2f}",
1302 f"A90: {evals['a90']:.2f}",
1303 f"A95: {evals['a95']:.2f}",
1304 f"A99: {evals['a99']:.2f}"
1305 ]
1306 sizes = [cv2.getTextSize(line, font, font_scale, thickness) for line in lines]
1307 sizes = [(size[0][0], size[0][1] + size[1], size[1]) for size in sizes]
1308 max_width = max([size[0] for size in sizes])
1309 total_height = sum([size[1] for size in sizes]) + (len(lines) - 1) * thickness
1310 img = np.ones((total_height + thickness, max_width, 3), dtype=np.uint8) * 255
1311 y = 0
1312 for line, size in zip(lines, sizes):
1313 cv2.putText(img, line, (0, y + size[1] - size[2]), font, font_scale, color, thickness)
1314 y += size[1] + thickness
1315 cv2.imshow("Evaluation", img)
1316
1317def show_debug_disparity(gt_img, oak_img):
1318 def rescale_img(img):
1319 img[img == np.inf] = 0.
1320 img = cv2.resize(img, (1280, 800), interpolation=cv2.INTER_AREA)
1321 return img.astype(np.uint16)
1322
1323 gt_img = rescale_img(gt_img)
1324 oak_img = rescale_img(oak_img)
1325 maxv = max(gt_img.max(), oak_img.max())
1326 gt_img = (gt_img * 255. / maxv).astype(np.uint8)
1327 oak_img = (oak_img * 255. / maxv).astype(np.uint8)
1328 cv2.imshow("GT", gt_img)
1329 cv2.imshow("OAK", oak_img)
1330
1331if evaluation_mode:
1332 dataset = DatasetManager(args.evaluate)
1333
1334 # Get the current timestamp
1335 timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
1336 # Create the filename with the timestamp
1337 filename = f"stereo_middleburry_{'RVC2' if platform == dai.Platform.RVC2 else 'RVC4'}_{timestamp}.csv"
1338 # Write the dictionary to a CSV file (append mode)
1339 file_exists = False
1340
1341 try:
1342 with open(filename, 'r'):
1343 file_exists = True
1344 except FileNotFoundError:
1345 pass
1346
1347print("Connecting and starting the pipeline")
1348# Connect to device and start pipeline
1349with pipeline:
1350 pipeline.start()
1351
1352 stereoDepthConfigInQueue = xinStereoDepthConfig
1353
1354 inStreamsCameraID = [dai.CameraBoardSocket.CAM_B, dai.CameraBoardSocket.CAM_C]
1355 in_q_list = []
1356 in_q_list.append(monoLeft)
1357 in_q_list.append(monoRight)
1358
1359 # Create a receive queue for each stream
1360 q_list = []
1361 q_list.append(xoutLeft)
1362 q_list.append(xoutRight)
1363 if outDepth:
1364 q_list.append(xoutDepth)
1365 if outConfidenceMap:
1366 q_list.append(xoutConfMap)
1367 q_list.append(xoutDisparity)
1368 if outRectified:
1369 q_list.append(xoutRectifLeft)
1370 q_list.append(xoutRectifRight)
1371
1372 inCfg = xoutStereoCfg
1373
1374 # Need to set a timestamp for input frames, for the sync stage in Stereo node
1375 timestamp_ms = 0
1376 index = 0
1377 prevQueues = q_list.copy()
1378 while pipeline.isRunning():
1379 # Handle input streams, if any
1380 if in_q_list:
1381 dataset_size = 1 # Number of image pairs
1382 frame_interval_ms = 50
1383 q_names = ["in_left", "in_right"]
1384 for i, q in enumerate(in_q_list):
1385 path = os.path.join(dataset.get(), f"im{i}.png") if evaluation_mode else args.dataset + "/" + str(index) + "/" + q_names[i] + ".png"
1386 data = cv2.imread(path, cv2.IMREAD_GRAYSCALE)
1387 data = cv2.resize(data, (width, height), interpolation = cv2.INTER_AREA)
1388 data = data.reshape(height*width)
1389 tstamp = datetime.timedelta(seconds = timestamp_ms // 1000,
1390 milliseconds = timestamp_ms % 1000)
1391 img = dai.ImgFrame()
1392 img.setData(data)
1393 img.setTimestamp(tstamp)
1394 img.setInstanceNum(inStreamsCameraID[i])
1395 img.setType(dai.ImgFrame.Type.RAW8)
1396 img.setWidth(width)
1397 img.setStride(width)
1398 img.setHeight(height)
1399 q.send(img)
1400 # print("Sent frame: {:25s}".format(path), "timestamp_ms:", timestamp_ms)
1401 timestamp_ms += frame_interval_ms
1402 index = (index + 1) % dataset_size
1403 sleep(frame_interval_ms / 1000)
1404
1405 gt_disparity = None
1406 if evaluation_mode:
1407 # Load GT disparity
1408 if currentConfig.algorithmControl.depthAlign == dai.StereoDepthConfig.AlgorithmControl.DepthAlign.RECTIFIED_LEFT:
1409 gt_disparity = read_pfm(os.path.join(dataset.get(), f"disp0.pfm"))[0]
1410 else:
1411 gt_disparity = read_pfm(os.path.join(dataset.get(), f"disp1.pfm"))[0]
1412
1413 # Handle output streams
1414 currentConfig = inCfg.get()
1415
1416 lrCheckEnabled = currentConfig.algorithmControl.enableLeftRightCheck
1417 extendedEnabled = currentConfig.algorithmControl.enableExtended
1418 queues = q_list.copy()
1419
1420 if args.dumpdisparitycostvalues:
1421 queues.append(xoutDebugCostDump)
1422
1423 if args.debug:
1424 q_list_debug = []
1425
1426 activeDebugStreams = []
1427 if lrCheckEnabled:
1428 q_list_debug.append(xoutDebugLrCheckIt1)
1429 q_list_debug.append(xoutDebugLrCheckIt2)
1430 if extendedEnabled:
1431 q_list_debug.append(xoutDebugExtLrCheckIt1)
1432 if lrCheckEnabled:
1433 q_list_debug.append(xoutDebugExtLrCheckIt2)
1434
1435 queues.extend(q_list_debug)
1436
1437 def ListDiff(li1, li2):
1438 return list(set(li1) - set(li2)) + list(set(li2) - set(li1))
1439
1440 diff = ListDiff(prevQueues, queues)
1441 for s in diff:
1442 name = s.getName()
1443 cv2.destroyWindow(name)
1444 prevQueues = queues.copy()
1445
1446 disparity = None
1447 for q in queues:
1448 if q.getName() in ["left", "right"]: continue
1449 data = q.get()
1450 if q.getName() == "disparity":
1451 disparity = data.getFrame()
1452 frame = convertToCv2Frame(q.getName(), data, currentConfig)
1453 cv2.imshow(q.getName(), frame)
1454
1455 if disparity is not None and gt_disparity is not None:
1456 subpixel_bits = 1 << currentConfig.algorithmControl.subpixelFractionalBits
1457 subpixel_enabled = currentConfig.algorithmControl.enableSubpixel
1458 width_scale = float(gt_disparity.shape[1]) / float(disparity.shape[1])
1459
1460 disparity = disparity.astype(np.float32)
1461 if subpixel_enabled:
1462 disparity = disparity / subpixel_bits
1463 disparity = disparity * width_scale
1464 disparity = cv2.resize(disparity, (gt_disparity.shape[1], gt_disparity.shape[0]), interpolation = cv2.INTER_LINEAR)
1465
1466 gt_disparity[gt_disparity == np.inf] = 0
1467 # disparity[disparity == 0.] = np.inf
1468
1469 show_debug_disparity(gt_disparity, disparity)
1470 err_vals = calculate_err_measures(gt_disparity, disparity)
1471 show_evaluation(dataset.get_name(), err_vals)
1472 err_vals["name"] = dataset.get_name()
1473
1474 saveToCsvFile = False
1475 key = cv2.waitKey(1)
1476 if key == ord("q"):
1477 break
1478 elif evaluation_mode and key == ord("["):
1479 dataset.next()
1480 saveToCsvFile = True
1481 elif evaluation_mode and key == ord("]"):
1482 dataset.prev()
1483 saveToCsvFile = True
1484
1485 if saveToCsvFile:
1486 with open(filename, 'a', newline='') as csvfile:
1487 writer = csv.writer(csvfile)
1488 # Write the header only if the file doesn't exist
1489 if not file_exists:
1490 writer.writerow(err_vals.keys())
1491 file_exists = True
1492 # Write the err_vals
1493 writer.writerow(err_vals.values())
1494
1495 StereoConfigHandler.handleKeypress(key, stereoDepthConfigInQueue)Similar samples
- Stereo Depth - Live stereo depth from stereo camera
Pipeline
Need assistance?
Head over to Discussion Forum for technical support or any other questions you might have.