来自主机的立体深度

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
1129    frame = image.getFrame()
1130
1131    # frame.tofile(name+".raw")
1132
1133    if name == "depth":
1134        dispScaleFactor = baseline * focal
1135        with np.errstate(divide="ignore"):
1136            frame = dispScaleFactor / frame
1137            if np.isnan(frame).any() or np.isinf(frame).any():
1138                frame = np.nan_to_num(frame, nan=0, posinf=0, neginf=0)
1139
1140        depthImg = dai.ImgFrame()
1141        depthImg.setCvFrame(frame.astype(np.float32), dai.ImgFrame.Type.GRAYF16)
1142        frame = dai.utility.colorizeDepthFrame(depthImg,colormap=cv2.COLORMAP_HOT).getCvFrame()
1143    elif "confidence_map" in name:
1144        pass
1145    elif name == "disparity_cost_dump":
1146        # frame.tofile(name+".raw")
1147        pass
1148    elif "disparity" in name:
1149        if 1: # Optionally, extend disparity range to better visualize it
1150            frame = (frame * 255. / maxDisp).astype(np.uint8)
1151        return frame
1152        # if 1: # Optionally, apply a color map
1153        #     frame = cv2.applyColorMap(frame, cv2.COLORMAP_HOT)
1154
1155    return frame
1156
1157class DatasetManager:
1158    def __init__(self, path):
1159        self.path = path
1160        self.index = 0
1161        self.names = [d for d in os.listdir(path) if os.path.isdir(os.path.join(path, d))]
1162        if len(self.names) == 0:
1163            raise RuntimeError("No dataset found at {}".format(path))
1164
1165    def get(self):
1166        return os.path.join(self.path, self.names[self.index])
1167
1168    def get_name(self):
1169        return self.names[self.index]
1170
1171    def next(self):
1172        self.index = (self.index + 1) % len(self.names)
1173        return self.get()
1174
1175    def prev(self):
1176        self.index = (self.index - 1) % len(self.names)
1177        return self.get()
1178
1179
1180def read_pfm(file):
1181    file = open(file, "rb")
1182
1183    color = None
1184    width = None
1185    height = None
1186    scale = None
1187    endian = None
1188
1189    header = file.readline().rstrip()
1190    if header.decode("ascii") == "PF":
1191        color = True
1192    elif header.decode("ascii") == "Pf":
1193        color = False
1194    else:
1195        raise Exception("Not a PFM file.")
1196
1197    dim_match = re.search(r"(\d+)\s(\d+)", file.readline().decode("ascii"))
1198    if dim_match:
1199        width, height = map(int, dim_match.groups())
1200    else:
1201        raise Exception("Malformed PFM header.")
1202
1203    scale = float(file.readline().rstrip())
1204    if scale < 0: # little-endian
1205        endian = "<"
1206        scale = -scale
1207    else:
1208        endian = ">" # big-endian
1209
1210    data = np.fromfile(file, endian + "f")
1211    shape = (height, width, 3) if color else (height, width)
1212    return np.flip(np.reshape(data, shape), axis=0), scale
1213
1214def calculate_err_measures(gt_img, oak_img):
1215    assert gt_img.shape == oak_img.shape
1216
1217    gt_mask = gt_img != 0
1218    oak_mask = oak_img != 0
1219    mask = gt_mask & oak_mask
1220
1221    gt_img[~gt_mask] = 0.
1222    oak_img[~mask] = 0.
1223    err = np.abs(gt_img - oak_img)
1224
1225    n = np.sum(gt_mask)
1226    invalid = np.sum(gt_mask & ~oak_mask)
1227
1228    bad05 = np.sum(mask & (err > 0.5))
1229    bad1 = np.sum(mask & (err > 1.))
1230    bad2 = np.sum(mask & (err > 2.))
1231    bad4 = np.sum(mask & (err > 4.))
1232    sum_err = np.sum(err[mask])
1233    sum_sq_err = np.sum(err[mask] ** 2)
1234    errs = err[mask]
1235
1236    bad05_p = 100. * bad05 / n
1237    total_bad05_p = 100. * (bad05 + invalid) / n
1238    bad1_p = 100. * bad1 / n
1239    total_bad1_p = 100. * (bad1 + invalid) / n
1240    bad2_p = 100. * bad2 / n
1241    total_bad2_p = 100. * (bad2 + invalid) / n
1242    bad4_p = 100. * bad4 / n
1243    total_bad4_p = 100. * (bad4 + invalid) / n
1244    invalid_p = 100. * invalid / n
1245    if n == invalid:
1246        avg_err = 0.
1247        mse = 0.
1248    else:
1249        avg_err = sum_err / (n - invalid)
1250        mse = sum_sq_err / (n - invalid)
1251    if len(errs) == 0:
1252        a50 = 0.
1253        a90 = 0.
1254        a95 = 0.
1255        a99 = 0.
1256    else:
1257        a50 = np.percentile(errs, 50)
1258        a90 = np.percentile(errs, 90)
1259        a95 = np.percentile(errs, 95)
1260        a99 = np.percentile(errs, 99)
1261
1262    return {
1263        "bad0.5": bad05_p,
1264        "total_bad0.5": total_bad05_p,
1265        "bad1": bad1_p,
1266        "total_bad1": total_bad1_p,
1267        "bad2": bad2_p,
1268        "total_bad2": total_bad2_p,
1269        "bad4": bad4_p,
1270        "total_bad4": total_bad4_p,
1271        "invalid": invalid_p,
1272        "avg_err": avg_err,
1273        "mse": mse,
1274        "a50": a50,
1275        "a90": a90,
1276        "a95": a95,
1277        "a99": a99
1278    }
1279
1280def show_evaluation(img_name, evals):
1281    cv2.namedWindow("Evaluation", cv2.WINDOW_NORMAL)
1282    font = cv2.FONT_HERSHEY_SIMPLEX
1283    font_scale = 2
1284    thickness = 3
1285    color = (0, 0, 0)
1286    lines = [
1287        f"Name: {img_name}",
1288        f"Bad0.5: {evals['bad0.5']:.2f}%",
1289        f"Total Bad0.5: {evals['total_bad0.5']:.2f}%",
1290        f"Bad1: {evals['bad1']:.2f}%",
1291        f"Total Bad1: {evals['total_bad1']:.2f}%",
1292        f"Bad2: {evals['bad2']:.2f}%",
1293        f"Total Bad2: {evals['total_bad2']:.2f}%",
1294        f"Bad4: {evals['bad4']:.2f}%",
1295        f"Total Bad4: {evals['total_bad4']:.2f}%",
1296        f"Invalid: {evals['invalid']:.2f}%",
1297        f"Avg Err: {evals['avg_err']:.2f}",
1298        f"MSE: {evals['mse']:.2f}",
1299        f"A50: {evals['a50']:.2f}",
1300        f"A90: {evals['a90']:.2f}",
1301        f"A95: {evals['a95']:.2f}",
1302        f"A99: {evals['a99']:.2f}"
1303    ]
1304    sizes = [cv2.getTextSize(line, font, font_scale, thickness) for line in lines]
1305    sizes = [(size[0][0], size[0][1] + size[1], size[1]) for size in sizes]
1306    max_width = max([size[0] for size in sizes])
1307    total_height = sum([size[1] for size in sizes]) + (len(lines) - 1) * thickness
1308    img = np.ones((total_height + thickness, max_width, 3), dtype=np.uint8) * 255
1309    y = 0
1310    for line, size in zip(lines, sizes):
1311        cv2.putText(img, line, (0, y + size[1] - size[2]), font, font_scale, color, thickness)
1312        y += size[1] + thickness
1313    cv2.imshow("Evaluation", img)
1314
1315def show_debug_disparity(gt_img, oak_img):
1316    def rescale_img(img):
1317        img[img == np.inf] = 0.
1318        img = cv2.resize(img, (1280, 800), interpolation=cv2.INTER_AREA)
1319        return img.astype(np.uint16)
1320
1321    gt_img = rescale_img(gt_img)
1322    oak_img = rescale_img(oak_img)
1323    maxv = max(gt_img.max(), oak_img.max())
1324    gt_img = (gt_img * 255. / maxv).astype(np.uint8)
1325    oak_img = (oak_img * 255. / maxv).astype(np.uint8)
1326    cv2.imshow("GT", gt_img)
1327    cv2.imshow("OAK", oak_img)
1328
1329if evaluation_mode:
1330    dataset = DatasetManager(args.evaluate)
1331
1332    # Get the current timestamp
1333    timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
1334    # Create the filename with the timestamp
1335    filename = f"stereo_middleburry_{'RVC2' if platform == dai.Platform.RVC2 else 'RVC4'}_{timestamp}.csv"
1336    # Write the dictionary to a CSV file (append mode)
1337    file_exists = False
1338
1339    try:
1340        with open(filename, 'r'):
1341            file_exists = True
1342    except FileNotFoundError:
1343        pass
1344
1345print("Connecting and starting the pipeline")
1346# Connect to device and start pipeline
1347with pipeline:
1348    pipeline.start()
1349
1350    stereoDepthConfigInQueue = xinStereoDepthConfig
1351
1352    inStreamsCameraID = [dai.CameraBoardSocket.CAM_B, dai.CameraBoardSocket.CAM_C]
1353    in_q_list = []
1354    in_q_list.append(monoLeft)
1355    in_q_list.append(monoRight)
1356
1357    # Create a receive queue for each stream
1358    q_list = []
1359    q_list.append(xoutLeft)
1360    q_list.append(xoutRight)
1361    if outDepth:
1362        q_list.append(xoutDepth)
1363    if outConfidenceMap:
1364        q_list.append(xoutConfMap)
1365    q_list.append(xoutDisparity)
1366    if outRectified:
1367        q_list.append(xoutRectifLeft)
1368        q_list.append(xoutRectifRight)
1369
1370    inCfg = xoutStereoCfg
1371
1372    # Need to set a timestamp for input frames, for the sync stage in Stereo node
1373    timestamp_ms = 0
1374    index = 0
1375    prevQueues = q_list.copy()
1376    while pipeline.isRunning():
1377        # Handle input streams, if any
1378        if in_q_list:
1379            dataset_size = 1  # Number of image pairs
1380            frame_interval_ms = 50
1381            q_names = ["in_left", "in_right"]
1382            for i, q in enumerate(in_q_list):
1383                path = os.path.join(dataset.get(), f"im{i}.png") if evaluation_mode else args.dataset + "/" + str(index) + "/" + q_names[i] + ".png"
1384                data = cv2.imread(path, cv2.IMREAD_GRAYSCALE)
1385                data = cv2.resize(data, (width, height), interpolation = cv2.INTER_AREA)
1386                data = data.reshape(height*width)
1387                tstamp = datetime.timedelta(seconds = timestamp_ms // 1000,
1388                                            milliseconds = timestamp_ms % 1000)
1389                img = dai.ImgFrame()
1390                img.setData(data)
1391                img.setTimestamp(tstamp)
1392                img.setInstanceNum(inStreamsCameraID[i])
1393                img.setType(dai.ImgFrame.Type.RAW8)
1394                img.setWidth(width)
1395                img.setStride(width)
1396                img.setHeight(height)
1397                q.send(img)
1398                # print("Sent frame: {:25s}".format(path), "timestamp_ms:", timestamp_ms)
1399            timestamp_ms += frame_interval_ms
1400            index = (index + 1) % dataset_size
1401            sleep(frame_interval_ms / 1000)
1402
1403        gt_disparity = None
1404        if evaluation_mode:
1405            # Load GT disparity
1406            if currentConfig.algorithmControl.depthAlign == dai.StereoDepthConfig.AlgorithmControl.DepthAlign.RECTIFIED_LEFT:
1407                gt_disparity = read_pfm(os.path.join(dataset.get(), f"disp0.pfm"))[0]
1408            else:
1409                gt_disparity = read_pfm(os.path.join(dataset.get(), f"disp1.pfm"))[0]
1410
1411        # Handle output streams
1412        currentConfig = inCfg.get()
1413
1414        lrCheckEnabled = currentConfig.algorithmControl.enableLeftRightCheck
1415        extendedEnabled = currentConfig.algorithmControl.enableExtended
1416        queues = q_list.copy()
1417
1418        if args.dumpdisparitycostvalues:
1419            queues.append(xoutDebugCostDump)
1420
1421        if args.debug:
1422            q_list_debug = []
1423
1424            activeDebugStreams = []
1425            if lrCheckEnabled:
1426                q_list_debug.append(xoutDebugLrCheckIt1)
1427                q_list_debug.append(xoutDebugLrCheckIt2)
1428            if extendedEnabled:
1429                q_list_debug.append(xoutDebugExtLrCheckIt1)
1430                if lrCheckEnabled:
1431                    q_list_debug.append(xoutDebugExtLrCheckIt2)
1432
1433            queues.extend(q_list_debug)
1434
1435        def ListDiff(li1, li2):
1436            return list(set(li1) - set(li2)) + list(set(li2) - set(li1))
1437
1438        diff = ListDiff(prevQueues, queues)
1439        for s in diff:
1440            name = s.getName()
1441            cv2.destroyWindow(name)
1442        prevQueues = queues.copy()
1443
1444        disparity = None
1445        for q in queues:
1446            if q.getName() in ["left", "right"]: continue
1447            data = q.get()
1448            if q.getName() == "disparity":
1449                disparity = data.getFrame()
1450            frame = convertToCv2Frame(q.getName(), data, currentConfig)
1451            cv2.imshow(q.getName(), frame)
1452
1453        if disparity is not None and gt_disparity is not None:
1454            subpixel_bits = 1 << currentConfig.algorithmControl.subpixelFractionalBits
1455            subpixel_enabled = currentConfig.algorithmControl.enableSubpixel
1456            width_scale = float(gt_disparity.shape[1]) / float(disparity.shape[1])
1457
1458            disparity = disparity.astype(np.float32)
1459            if subpixel_enabled:
1460                disparity = disparity / subpixel_bits
1461            disparity = disparity * width_scale
1462            disparity = cv2.resize(disparity, (gt_disparity.shape[1], gt_disparity.shape[0]), interpolation = cv2.INTER_LINEAR)
1463
1464            gt_disparity[gt_disparity == np.inf] = 0
1465            # disparity[disparity == 0.] = np.inf
1466
1467            show_debug_disparity(gt_disparity, disparity)
1468            err_vals = calculate_err_measures(gt_disparity, disparity)
1469            show_evaluation(dataset.get_name(), err_vals)
1470            err_vals["name"] = dataset.get_name()
1471
1472        saveToCsvFile = False
1473        key = cv2.waitKey(1)
1474        if key == ord("q"):
1475            break
1476        elif evaluation_mode and key == ord("["):
1477            dataset.next()
1478            saveToCsvFile = True
1479        elif evaluation_mode and key == ord("]"):
1480            dataset.prev()
1481            saveToCsvFile = True
1482
1483        if saveToCsvFile:
1484            with open(filename, 'a', newline='') as csvfile:
1485                writer = csv.writer(csvfile)
1486                # Write the header only if the file doesn't exist
1487                if not file_exists:
1488                    writer.writerow(err_vals.keys())
1489                    file_exists = True
1490                # Write the err_vals
1491                writer.writerow(err_vals.values())
1492
1493        StereoConfigHandler.handleKeypress(key, stereoDepthConfigInQueue)