Stereo Depth from host

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
12
13datasetDefault = str((Path(__file__).parent / Path("../models/dataset")).resolve().absolute())
14parser = argparse.ArgumentParser()
15parser.add_argument("-p", "--dataset", nargs="?", help="Path to recorded frames", default=None)
16parser.add_argument("-d", "--debug", action="store_true", help="Enable debug outputs.")
17parser.add_argument("-e", "--evaluate", help="Evaluate the disparity calculation.", default=None)
18parser.add_argument("-dumpdispcost", "--dumpdisparitycostvalues", action="store_true", help="Dumps the disparity cost values for each disparity range. 96 byte for each pixel.")
19parser.add_argument("--download", action="store_true", help="Downloads the 2014 Middlebury dataset.")
20parser.add_argument("--calibration", help="Path to calibration file", default=None)
21parser.add_argument("--rectify", action="store_true", help="Enable rectified streams")
22parser.add_argument("--swapLR", action="store_true", help="Swap left and right cameras.")
23args = parser.parse_args()
24
25if args.evaluate is not None and args.dataset is not None:
26    import sys
27    raise ValueError("Cannot use both --dataset and --evaluate arguments at the same time.")
28
29evaluation_mode = args.evaluate is not None
30args.dataset = args.dataset or datasetDefault
31
32if args.download and args.evaluate is None:
33    import sys
34    raise ValueError("Cannot use --download without --evaluate argument.")
35
36if args.evaluate is None and not Path(args.dataset).exists():
37    import sys
38    raise FileNotFoundError(f"Required file/s not found, please run '{sys.executable} install_requirements.py'")
39
40if args.evaluate is not None and not args.download and not Path(args.evaluate).exists():
41    import sys
42    raise FileNotFoundError(f"Evaluation dataset path does not exist, use the --evaluate argument to specify the path.")
43
44if args.evaluate is not None and args.download and not Path(args.evaluate).exists():
45    os.makedirs(args.evaluate)
46
47def download_2014_middlebury(data_path):
48    import requests, zipfile, io
49    url = "https://vision.middlebury.edu/stereo/data/scenes2014/zip/"
50    r = requests.get(url)
51    c = r.content
52    reg = re.compile(r"href=('|\")(.+\.zip)('|\")")
53    matches = reg.findall(c.decode("utf-8"))
54    files = [m[1] for m in matches]
55
56    for f in files:
57        if os.path.isdir(os.path.join(data_path, f[:-4])):
58            print(f"Skipping {f}")
59        else:
60            print(f"Downloading {f} from {url + f}")
61            r = requests.get(url + f)
62            print(f"Extracting {f} to {data_path}")
63            z = zipfile.ZipFile(io.BytesIO(r.content))
64            z.extractall(data_path)
65
66if args.download:
67    download_2014_middlebury(args.evaluate)
68    if not evaluation_mode:
69        sys.exit(0)
70
71class StereoConfigHandler:
72
73    class Trackbar:
74        def __init__(self, trackbarName, windowName, minValue, maxValue, defaultValue, handler):
75            self.min = minValue
76            self.max = maxValue
77            self.windowName = windowName
78            self.trackbarName = trackbarName
79            cv2.createTrackbar(trackbarName, windowName, minValue, maxValue, handler)
80            cv2.setTrackbarPos(trackbarName, windowName, defaultValue)
81
82        def set(self, value):
83            if value < self.min:
84                value = self.min
85                print(f"{self.trackbarName} min value is {self.min}")
86            if value > self.max:
87                value = self.max
88                print(f"{self.trackbarName} max value is {self.max}")
89            cv2.setTrackbarPos(self.trackbarName, self.windowName, value)
90
91    class CensusMaskHandler:
92
93        stateColor = [(0, 0, 0), (255, 255, 255)]
94        gridHeight = 50
95        gridWidth = 50
96
97        def fillRectangle(self, row, col):
98            src = self.gridList[row][col]["topLeft"]
99            dst = self.gridList[row][col]["bottomRight"]
100
101            stateColor = self.stateColor[1] if self.gridList[row][col]["state"] else self.stateColor[0]
102            self.changed = True
103
104            cv2.rectangle(self.gridImage, src, dst, stateColor, -1)
105            cv2.imshow(self.windowName, self.gridImage)
106
107
108        def clickCallback(self, event, x, y, flags, param):
109            if event == cv2.EVENT_LBUTTONDOWN:
110                col = x * (self.gridSize[1]) // self.width
111                row = y * (self.gridSize[0]) // self.height
112                self.gridList[row][col]["state"] = not self.gridList[row][col]["state"]
113                self.fillRectangle(row, col)
114
115
116        def __init__(self, windowName, gridSize):
117            self.gridSize = gridSize
118            self.windowName = windowName
119            self.changed = False
120
121            cv2.namedWindow(self.windowName)
122
123            self.width = StereoConfigHandler.CensusMaskHandler.gridWidth * self.gridSize[1]
124            self.height = StereoConfigHandler.CensusMaskHandler.gridHeight * self.gridSize[0]
125
126            self.gridImage = np.zeros((self.height + 50, self.width, 3), np.uint8)
127
128            cv2.putText(self.gridImage, "Click on grid to change mask!", (0, self.height+20), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255))
129            cv2.putText(self.gridImage, "White: ON   |   Black: OFF", (0, self.height+40), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 255))
130
131            self.gridList = [[dict() for _ in range(self.gridSize[1])] for _ in range(self.gridSize[0])]
132
133            for row in range(self.gridSize[0]):
134                rowFactor = self.height // self.gridSize[0]
135                srcY = row*rowFactor + 1
136                dstY = (row+1)*rowFactor - 1
137                for col in range(self.gridSize[1]):
138                    colFactor = self.width // self.gridSize[1]
139                    srcX = col*colFactor + 1
140                    dstX = (col+1)*colFactor - 1
141                    src = (srcX, srcY)
142                    dst = (dstX, dstY)
143                    self.gridList[row][col]["topLeft"] = src
144                    self.gridList[row][col]["bottomRight"] = dst
145                    self.gridList[row][col]["state"] = False
146                    self.fillRectangle(row, col)
147
148            cv2.setMouseCallback(self.windowName, self.clickCallback)
149            cv2.imshow(self.windowName, self.gridImage)
150
151        def getMask(self) -> np.uint64:
152            mask = np.uint64(0)
153            for row in range(self.gridSize[0]):
154                for col in range(self.gridSize[1]):
155                    if self.gridList[row][col]["state"]:
156                        pos = row*self.gridSize[1] + col
157                        mask = np.bitwise_or(mask, np.uint64(1) << np.uint64(pos))
158
159            return mask
160
161        def setMask(self, _mask: np.uint64):
162            mask = np.uint64(_mask)
163            for row in range(self.gridSize[0]):
164                for col in range(self.gridSize[1]):
165                    pos = row*self.gridSize[1] + col
166                    if np.bitwise_and(mask, np.uint64(1) << np.uint64(pos)):
167                        self.gridList[row][col]["state"] = True
168                    else:
169                        self.gridList[row][col]["state"] = False
170
171                    self.fillRectangle(row, col)
172
173        def isChanged(self):
174            changed = self.changed
175            self.changed = False
176            return changed
177
178        def destroyWindow(self):
179            cv2.destroyWindow(self.windowName)
180
181
182    censusMaskHandler = None
183    newConfig = False
184    config = None
185    trSigma = list()
186    trConfidence = list()
187    trLrCheck = list()
188    trFractionalBits = list()
189    trLineqAlpha = list()
190    trLineqBeta = list()
191    trLineqThreshold = list()
192    trCostAggregationP1 = list()
193    trCostAggregationP2 = list()
194    trTemporalAlpha = list()
195    trTemporalDelta = list()
196    trThresholdMinRange = list()
197    trThresholdMaxRange = list()
198    trSpeckleRange = list()
199    trSpatialAlpha = list()
200    trSpatialDelta = list()
201    trSpatialHoleFilling = list()
202    trSpatialNumIterations = list()
203    trDecimationFactor = list()
204    trDisparityShift = list()
205    trCenterAlignmentShift = list()
206    trInvalidateEdgePixels = list()
207
208    def trackbarSigma(value):
209        StereoConfigHandler.config.postProcessing.bilateralSigmaValue = value
210        StereoConfigHandler.newConfig = True
211        for tr in StereoConfigHandler.trSigma:
212            tr.set(value)
213
214    def trackbarConfidence(value):
215        StereoConfigHandler.config.costMatching.confidenceThreshold = value
216        StereoConfigHandler.newConfig = True
217        for tr in StereoConfigHandler.trConfidence:
218            tr.set(value)
219
220    def trackbarLrCheckThreshold(value):
221        StereoConfigHandler.config.algorithmControl.leftRightCheckThreshold = value
222        StereoConfigHandler.newConfig = True
223        for tr in StereoConfigHandler.trLrCheck:
224            tr.set(value)
225
226    def trackbarFractionalBits(value):
227        StereoConfigHandler.config.algorithmControl.subpixelFractionalBits = value
228        StereoConfigHandler.newConfig = True
229        for tr in StereoConfigHandler.trFractionalBits:
230            tr.set(value)
231
232    def trackbarLineqAlpha(value):
233        StereoConfigHandler.config.costMatching.linearEquationParameters.alpha = value
234        StereoConfigHandler.newConfig = True
235        for tr in StereoConfigHandler.trLineqAlpha:
236            tr.set(value)
237
238    def trackbarLineqBeta(value):
239        StereoConfigHandler.config.costMatching.linearEquationParameters.beta = value
240        StereoConfigHandler.newConfig = True
241        for tr in StereoConfigHandler.trLineqBeta:
242            tr.set(value)
243
244    def trackbarLineqThreshold(value):
245        StereoConfigHandler.config.costMatching.linearEquationParameters.threshold = value
246        StereoConfigHandler.newConfig = True
247        for tr in StereoConfigHandler.trLineqThreshold:
248            tr.set(value)
249
250    def trackbarCostAggregationP1(value):
251        StereoConfigHandler.config.costAggregation.horizontalPenaltyCostP1 = value
252        StereoConfigHandler.config.costAggregation.verticalPenaltyCostP1 = value
253        StereoConfigHandler.newConfig = True
254        for tr in StereoConfigHandler.trCostAggregationP1:
255            tr.set(value)
256
257    def trackbarCostAggregationP2(value):
258        StereoConfigHandler.config.costAggregation.horizontalPenaltyCostP2 = value
259        StereoConfigHandler.config.costAggregation.verticalPenaltyCostP2 = value
260        StereoConfigHandler.newConfig = True
261        for tr in StereoConfigHandler.trCostAggregationP2:
262            tr.set(value)
263
264    def trackbarTemporalFilterAlpha(value):
265        StereoConfigHandler.config.postProcessing.temporalFilter.alpha = value / 100.
266        StereoConfigHandler.newConfig = True
267        for tr in StereoConfigHandler.trTemporalAlpha:
268            tr.set(value)
269
270    def trackbarTemporalFilterDelta(value):
271        StereoConfigHandler.config.postProcessing.temporalFilter.delta = value
272        StereoConfigHandler.newConfig = True
273        for tr in StereoConfigHandler.trTemporalDelta:
274            tr.set(value)
275
276    def trackbarSpatialFilterAlpha(value):
277        StereoConfigHandler.config.postProcessing.spatialFilter.alpha = value / 100.
278        StereoConfigHandler.newConfig = True
279        for tr in StereoConfigHandler.trSpatialAlpha:
280            tr.set(value)
281
282    def trackbarSpatialFilterDelta(value):
283        StereoConfigHandler.config.postProcessing.spatialFilter.delta = value
284        StereoConfigHandler.newConfig = True
285        for tr in StereoConfigHandler.trSpatialDelta:
286            tr.set(value)
287
288    def trackbarSpatialFilterHoleFillingRadius(value):
289        StereoConfigHandler.config.postProcessing.spatialFilter.holeFillingRadius = value
290        StereoConfigHandler.newConfig = True
291        for tr in StereoConfigHandler.trSpatialHoleFilling:
292            tr.set(value)
293
294    def trackbarSpatialFilterNumIterations(value):
295        StereoConfigHandler.config.postProcessing.spatialFilter.numIterations = value
296        StereoConfigHandler.newConfig = True
297        for tr in StereoConfigHandler.trSpatialNumIterations:
298            tr.set(value)
299
300    def trackbarThresholdMinRange(value):
301        StereoConfigHandler.config.postProcessing.thresholdFilter.minRange = value * 1000
302        StereoConfigHandler.newConfig = True
303        for tr in StereoConfigHandler.trThresholdMinRange:
304            tr.set(value)
305
306    def trackbarThresholdMaxRange(value):
307        StereoConfigHandler.config.postProcessing.thresholdFilter.maxRange = value * 1000
308        StereoConfigHandler.newConfig = True
309        for tr in StereoConfigHandler.trThresholdMaxRange:
310            tr.set(value)
311
312    def trackbarSpeckleRange(value):
313        StereoConfigHandler.config.postProcessing.speckleFilter.speckleRange = value
314        StereoConfigHandler.newConfig = True
315        for tr in StereoConfigHandler.trSpeckleRange:
316            tr.set(value)
317
318    def trackbarDecimationFactor(value):
319        StereoConfigHandler.config.postProcessing.decimationFilter.decimationFactor = value
320        StereoConfigHandler.newConfig = True
321        for tr in StereoConfigHandler.trDecimationFactor:
322            tr.set(value)
323
324    def trackbarDisparityShift(value):
325        StereoConfigHandler.config.algorithmControl.disparityShift = value
326        StereoConfigHandler.newConfig = True
327        for tr in StereoConfigHandler.trDisparityShift:
328            tr.set(value)
329
330    def trackbarCenterAlignmentShift(value):
331        if StereoConfigHandler.config.algorithmControl.depthAlign != dai.StereoDepthConfig.AlgorithmControl.DepthAlign.CENTER:
332            print("Center alignment shift factor requires CENTER alignment enabled!")
333            return
334        StereoConfigHandler.config.algorithmControl.centerAlignmentShiftFactor = value / 100.
335        print(f"centerAlignmentShiftFactor: {StereoConfigHandler.config.algorithmControl.centerAlignmentShiftFactor:.2f}")
336        StereoConfigHandler.newConfig = True
337        for tr in StereoConfigHandler.trCenterAlignmentShift:
338            tr.set(value)
339
340    def trackbarInvalidateEdgePixels(value):
341        StereoConfigHandler.config.algorithmControl.numInvalidateEdgePixels = value
342        print(f"numInvalidateEdgePixels: {StereoConfigHandler.config.algorithmControl.numInvalidateEdgePixels:.2f}")
343        StereoConfigHandler.newConfig = True
344        for tr in StereoConfigHandler.trInvalidateEdgePixels:
345            tr.set(value)
346
347    def handleKeypress(key, stereoDepthConfigInQueue):
348        if key == ord("m"):
349            StereoConfigHandler.newConfig = True
350            medianSettings = [dai.MedianFilter.MEDIAN_OFF, dai.MedianFilter.KERNEL_3x3, dai.MedianFilter.KERNEL_5x5, dai.MedianFilter.KERNEL_7x7]
351            currentMedian = StereoConfigHandler.config.postProcessing.median
352            nextMedian = medianSettings[(medianSettings.index(currentMedian)+1) % len(medianSettings)]
353            print(f"Changing median to {nextMedian.name} from {currentMedian.name}")
354            StereoConfigHandler.config.postProcessing.median = nextMedian
355        if key == ord("w"):
356            StereoConfigHandler.newConfig = True
357            StereoConfigHandler.config.postProcessing.spatialFilter.enable = not StereoConfigHandler.config.postProcessing.spatialFilter.enable
358            state = "on" if StereoConfigHandler.config.postProcessing.spatialFilter.enable else "off"
359            print(f"Spatial filter {state}")
360        if key == ord("t"):
361            StereoConfigHandler.newConfig = True
362            StereoConfigHandler.config.postProcessing.temporalFilter.enable = not StereoConfigHandler.config.postProcessing.temporalFilter.enable
363            state = "on" if StereoConfigHandler.config.postProcessing.temporalFilter.enable else "off"
364            print(f"Temporal filter {state}")
365        if key == ord("s"):
366            StereoConfigHandler.newConfig = True
367            StereoConfigHandler.config.postProcessing.speckleFilter.enable = not StereoConfigHandler.config.postProcessing.speckleFilter.enable
368            state = "on" if StereoConfigHandler.config.postProcessing.speckleFilter.enable else "off"
369            print(f"Speckle filter {state}")
370        if key == ord("r"):
371            StereoConfigHandler.newConfig = True
372            temporalSettings = [dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.PERSISTENCY_OFF,
373            dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_8_OUT_OF_8,
374            dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_IN_LAST_3,
375            dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_IN_LAST_4,
376            dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_OUT_OF_8,
377            dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_2,
378            dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_5,
379            dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_8,
380            dai.StereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.PERSISTENCY_INDEFINITELY,
381            ]
382            currentTemporal = StereoConfigHandler.config.postProcessing.temporalFilter.persistencyMode
383            nextTemporal = temporalSettings[(temporalSettings.index(currentTemporal)+1) % len(temporalSettings)]
384            print(f"Changing temporal persistency to {nextTemporal.name} from {currentTemporal.name}")
385            StereoConfigHandler.config.postProcessing.temporalFilter.persistencyMode = nextTemporal
386        if key == ord("n"):
387            StereoConfigHandler.newConfig = True
388            decimationSettings = [dai.StereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.PIXEL_SKIPPING,
389            dai.StereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.NON_ZERO_MEDIAN,
390            dai.StereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.NON_ZERO_MEAN,
391            ]
392            currentDecimation = StereoConfigHandler.config.postProcessing.decimationFilter.decimationMode
393            nextDecimation = decimationSettings[(decimationSettings.index(currentDecimation)+1) % len(decimationSettings)]
394            print(f"Changing decimation mode to {nextDecimation.name} from {currentDecimation.name}")
395            StereoConfigHandler.config.postProcessing.decimationFilter.decimationMode = nextDecimation
396        if key == ord("a"):
397            StereoConfigHandler.newConfig = True
398            alignmentSettings = [dai.StereoDepthConfig.AlgorithmControl.DepthAlign.RECTIFIED_RIGHT,
399            dai.StereoDepthConfig.AlgorithmControl.DepthAlign.RECTIFIED_LEFT,
400            dai.StereoDepthConfig.AlgorithmControl.DepthAlign.CENTER,
401            ]
402            currentAlignment = StereoConfigHandler.config.algorithmControl.depthAlign
403            nextAlignment = alignmentSettings[(alignmentSettings.index(currentAlignment)+1) % len(alignmentSettings)]
404            print(f"Changing alignment mode to {nextAlignment.name} from {currentAlignment.name}")
405            StereoConfigHandler.config.algorithmControl.depthAlign = nextAlignment
406        elif key == ord("c"):
407            StereoConfigHandler.newConfig = True
408            censusSettings = [dai.StereoDepthConfig.CensusTransform.KernelSize.AUTO, dai.StereoDepthConfig.CensusTransform.KernelSize.KERNEL_5x5, dai.StereoDepthConfig.CensusTransform.KernelSize.KERNEL_7x7, dai.StereoDepthConfig.CensusTransform.KernelSize.KERNEL_7x9]
409            currentCensus = StereoConfigHandler.config.censusTransform.kernelSize
410            nextCensus = censusSettings[(censusSettings.index(currentCensus)+1) % len(censusSettings)]
411            if nextCensus != dai.StereoDepthConfig.CensusTransform.KernelSize.AUTO:
412                censusGridSize = [(5,5), (7,7), (7,9)]
413                censusDefaultMask = [np.uint64(0XA82415), np.uint64(0XAA02A8154055), np.uint64(0X2AA00AA805540155)]
414                censusGrid = censusGridSize[nextCensus]
415                censusMask = censusDefaultMask[nextCensus]
416                StereoConfigHandler.censusMaskHandler = StereoConfigHandler.CensusMaskHandler("Census mask", censusGrid)
417                StereoConfigHandler.censusMaskHandler.setMask(censusMask)
418            else:
419                print("Census mask config is not available in AUTO census kernel mode. Change using the 'c' key")
420                StereoConfigHandler.config.censusTransform.kernelMask = 0
421                StereoConfigHandler.censusMaskHandler.destroyWindow()
422            print(f"Changing census transform to {nextCensus.name} from {currentCensus.name}")
423            StereoConfigHandler.config.censusTransform.kernelSize = nextCensus
424        elif key == ord("d"):
425            StereoConfigHandler.newConfig = True
426            dispRangeSettings = [dai.StereoDepthConfig.CostMatching.DisparityWidth.DISPARITY_64, dai.StereoDepthConfig.CostMatching.DisparityWidth.DISPARITY_96]
427            currentDispRange = StereoConfigHandler.config.costMatching.disparityWidth
428            nextDispRange = dispRangeSettings[(dispRangeSettings.index(currentDispRange)+1) % len(dispRangeSettings)]
429            print(f"Changing disparity range to {nextDispRange.name} from {currentDispRange.name}")
430            StereoConfigHandler.config.costMatching.disparityWidth = nextDispRange
431        elif key == ord("f"):
432            StereoConfigHandler.newConfig = True
433            StereoConfigHandler.config.costMatching.enableCompanding = not StereoConfigHandler.config.costMatching.enableCompanding
434            state = "on" if StereoConfigHandler.config.costMatching.enableCompanding else "off"
435            print(f"Companding {state}")
436        elif key == ord("v"):
437            StereoConfigHandler.newConfig = True
438            StereoConfigHandler.config.censusTransform.enableMeanMode = not StereoConfigHandler.config.censusTransform.enableMeanMode
439            state = "on" if StereoConfigHandler.config.censusTransform.enableMeanMode else "off"
440            print(f"Census transform mean mode {state}")
441        elif key == ord("1"):
442            StereoConfigHandler.newConfig = True
443            StereoConfigHandler.config.algorithmControl.enableLeftRightCheck = not StereoConfigHandler.config.algorithmControl.enableLeftRightCheck
444            state = "on" if StereoConfigHandler.config.algorithmControl.enableLeftRightCheck else "off"
445            print(f"LR-check {state}")
446        elif key == ord("2"):
447            StereoConfigHandler.newConfig = True
448            StereoConfigHandler.config.algorithmControl.enableSubpixel = not StereoConfigHandler.config.algorithmControl.enableSubpixel
449            state = "on" if StereoConfigHandler.config.algorithmControl.enableSubpixel else "off"
450            print(f"Subpixel {state}")
451        elif key == ord("3"):
452            StereoConfigHandler.newConfig = True
453            StereoConfigHandler.config.algorithmControl.enableExtended = not StereoConfigHandler.config.algorithmControl.enableExtended
454            state = "on" if StereoConfigHandler.config.algorithmControl.enableExtended else "off"
455            print(f"Extended {state}")
456
457        censusMaskChanged = False
458        if StereoConfigHandler.censusMaskHandler is not None:
459            censusMaskChanged = StereoConfigHandler.censusMaskHandler.isChanged()
460        if censusMaskChanged:
461            StereoConfigHandler.config.censusTransform.kernelMask = StereoConfigHandler.censusMaskHandler.getMask()
462            StereoConfigHandler.newConfig = True
463
464        StereoConfigHandler.sendConfig(stereoDepthConfigInQueue)
465
466    def sendConfig(stereoDepthConfigInQueue):
467        if StereoConfigHandler.newConfig:
468            StereoConfigHandler.newConfig = False
469            configMessage = dai.StereoDepthConfig()
470            configMessage.set(StereoConfigHandler.config)
471            stereoDepthConfigInQueue.send(configMessage)
472
473    def updateDefaultConfig(config):
474        StereoConfigHandler.config = config
475
476    def registerWindow(stream):
477        cv2.namedWindow(stream, cv2.WINDOW_NORMAL)
478
479        StereoConfigHandler.trConfidence.append(StereoConfigHandler.Trackbar("Disparity confidence", stream, 0, 255, StereoConfigHandler.config.costMatching.confidenceThreshold, StereoConfigHandler.trackbarConfidence))
480        StereoConfigHandler.trSigma.append(StereoConfigHandler.Trackbar("Bilateral sigma", stream, 0, 100, StereoConfigHandler.config.postProcessing.bilateralSigmaValue, StereoConfigHandler.trackbarSigma))
481        StereoConfigHandler.trLrCheck.append(StereoConfigHandler.Trackbar("LR-check threshold", stream, 0, 16, StereoConfigHandler.config.algorithmControl.leftRightCheckThreshold, StereoConfigHandler.trackbarLrCheckThreshold))
482        StereoConfigHandler.trFractionalBits.append(StereoConfigHandler.Trackbar("Subpixel fractional bits", stream, 3, 5, StereoConfigHandler.config.algorithmControl.subpixelFractionalBits, StereoConfigHandler.trackbarFractionalBits))
483        StereoConfigHandler.trDisparityShift.append(StereoConfigHandler.Trackbar("Disparity shift", stream, 0, 100, StereoConfigHandler.config.algorithmControl.disparityShift, StereoConfigHandler.trackbarDisparityShift))
484        StereoConfigHandler.trCenterAlignmentShift.append(StereoConfigHandler.Trackbar("Center alignment shift factor", stream, 0, 100, StereoConfigHandler.config.algorithmControl.centerAlignmentShiftFactor, StereoConfigHandler.trackbarCenterAlignmentShift))
485        StereoConfigHandler.trInvalidateEdgePixels.append(StereoConfigHandler.Trackbar("Invalidate edge pixels", stream, 0, 100, StereoConfigHandler.config.algorithmControl.numInvalidateEdgePixels, StereoConfigHandler.trackbarInvalidateEdgePixels))
486        StereoConfigHandler.trLineqAlpha.append(StereoConfigHandler.Trackbar("Linear equation alpha", stream, 0, 15, StereoConfigHandler.config.costMatching.linearEquationParameters.alpha, StereoConfigHandler.trackbarLineqAlpha))
487        StereoConfigHandler.trLineqBeta.append(StereoConfigHandler.Trackbar("Linear equation beta", stream, 0, 15, StereoConfigHandler.config.costMatching.linearEquationParameters.beta, StereoConfigHandler.trackbarLineqBeta))
488        StereoConfigHandler.trLineqThreshold.append(StereoConfigHandler.Trackbar("Linear equation threshold", stream, 0, 255, StereoConfigHandler.config.costMatching.linearEquationParameters.threshold, StereoConfigHandler.trackbarLineqThreshold))
489        StereoConfigHandler.trCostAggregationP1.append(StereoConfigHandler.Trackbar("Cost aggregation P1", stream, 0, 500, StereoConfigHandler.config.costAggregation.horizontalPenaltyCostP1, StereoConfigHandler.trackbarCostAggregationP1))
490        StereoConfigHandler.trCostAggregationP2.append(StereoConfigHandler.Trackbar("Cost aggregation P2", stream, 0, 500, StereoConfigHandler.config.costAggregation.horizontalPenaltyCostP2, StereoConfigHandler.trackbarCostAggregationP2))
491        StereoConfigHandler.trTemporalAlpha.append(StereoConfigHandler.Trackbar("Temporal filter alpha", stream, 0, 100, int(StereoConfigHandler.config.postProcessing.temporalFilter.alpha*100), StereoConfigHandler.trackbarTemporalFilterAlpha))
492        StereoConfigHandler.trTemporalDelta.append(StereoConfigHandler.Trackbar("Temporal filter delta", stream, 0, 100, StereoConfigHandler.config.postProcessing.temporalFilter.delta, StereoConfigHandler.trackbarTemporalFilterDelta))
493        StereoConfigHandler.trSpatialAlpha.append(StereoConfigHandler.Trackbar("Spatial filter alpha", stream, 0, 100, int(StereoConfigHandler.config.postProcessing.spatialFilter.alpha*100), StereoConfigHandler.trackbarSpatialFilterAlpha))
494        StereoConfigHandler.trSpatialDelta.append(StereoConfigHandler.Trackbar("Spatial filter delta", stream, 0, 100, StereoConfigHandler.config.postProcessing.spatialFilter.delta, StereoConfigHandler.trackbarSpatialFilterDelta))
495        StereoConfigHandler.trSpatialHoleFilling.append(StereoConfigHandler.Trackbar("Spatial filter hole filling radius", stream, 0, 16, StereoConfigHandler.config.postProcessing.spatialFilter.holeFillingRadius, StereoConfigHandler.trackbarSpatialFilterHoleFillingRadius))
496        StereoConfigHandler.trSpatialNumIterations.append(StereoConfigHandler.Trackbar("Spatial filter number of iterations", stream, 0, 4, StereoConfigHandler.config.postProcessing.spatialFilter.numIterations, StereoConfigHandler.trackbarSpatialFilterNumIterations))
497        StereoConfigHandler.trThresholdMinRange.append(StereoConfigHandler.Trackbar("Threshold filter min range", stream, 0, 65, StereoConfigHandler.config.postProcessing.thresholdFilter.minRange, StereoConfigHandler.trackbarThresholdMinRange))
498        StereoConfigHandler.trThresholdMaxRange.append(StereoConfigHandler.Trackbar("Threshold filter max range", stream, 0, 65, StereoConfigHandler.config.postProcessing.thresholdFilter.maxRange, StereoConfigHandler.trackbarThresholdMaxRange))
499        StereoConfigHandler.trSpeckleRange.append(StereoConfigHandler.Trackbar("Speckle filter range", stream, 0, 240, StereoConfigHandler.config.postProcessing.speckleFilter.speckleRange, StereoConfigHandler.trackbarSpeckleRange))
500        StereoConfigHandler.trDecimationFactor.append(StereoConfigHandler.Trackbar("Decimation factor", stream, 1, 4, StereoConfigHandler.config.postProcessing.decimationFilter.decimationFactor, StereoConfigHandler.trackbarDecimationFactor))
501
502    def __init__(self, config):
503        print("Control median filter using the 'm' key.")
504        print("Control census transform kernel size using the 'c' key.")
505        print("Control disparity search range using the 'd' key.")
506        print("Control disparity companding using the 'f' key.")
507        print("Control census transform mean mode using the 'v' key.")
508        print("Control depth alignment using the 'a' key.")
509        print("Control decimation algorithm using the 'a' key.")
510        print("Control temporal persistency mode using the 'r' key.")
511        print("Control spatial filter using the 'w' key.")
512        print("Control temporal filter using the 't' key.")
513        print("Control speckle filter using the 's' key.")
514        print("Control left-right check mode using the '1' key.")
515        print("Control subpixel mode using the '2' key.")
516        print("Control extended mode using the '3' key.")
517        if evaluation_mode:
518            print("Switch between images using '[' and ']' keys.")
519
520        StereoConfigHandler.config = config
521
522        if StereoConfigHandler.config.censusTransform.kernelSize != dai.StereoDepthConfig.CensusTransform.KernelSize.AUTO:
523            censusMask = StereoConfigHandler.config.censusTransform.kernelMask
524            censusGridSize = [(5,5), (7,7), (7,9)]
525            censusGrid = censusGridSize[StereoConfigHandler.config.censusTransform.kernelSize]
526            if StereoConfigHandler.config.censusTransform.kernelMask == 0:
527                censusDefaultMask = [np.uint64(0xA82415), np.uint64(0xAA02A8154055), np.uint64(0x2AA00AA805540155)]
528                censusMask = censusDefaultMask[StereoConfigHandler.config.censusTransform.kernelSize]
529            StereoConfigHandler.censusMaskHandler = StereoConfigHandler.CensusMaskHandler("Census mask", censusGrid)
530            StereoConfigHandler.censusMaskHandler.setMask(censusMask)
531        else:
532            print("Census mask config is not available in AUTO Census kernel mode. Change using the 'c' key")
533
534
535# StereoDepth initial config options.
536outDepth = True  # Disparity by default
537outConfidenceMap = True  # Output disparity confidence map
538outRectified = True   # Output and display rectified streams
539lrcheck = True   # Better handling for occlusions
540extended = False  # Closer-in minimum depth, disparity range is doubled. Unsupported for now.
541subpixel = True   # Better accuracy for longer distance, fractional disparity 32-levels
542
543width = 1280
544height = 800
545
546xoutStereoCfg = None
547
548# Create pipeline
549pipeline = dai.Pipeline()
550
551# Define sources and outputs
552stereo = pipeline.create(dai.node.StereoDepth)
553
554monoLeft = pipeline.create(dai.node.XLinkIn)
555monoRight = pipeline.create(dai.node.XLinkIn)
556xinStereoDepthConfig = pipeline.create(dai.node.XLinkIn)
557
558xoutLeft = pipeline.create(dai.node.XLinkOut)
559xoutRight = pipeline.create(dai.node.XLinkOut)
560xoutDepth = pipeline.create(dai.node.XLinkOut)
561xoutConfMap = pipeline.create(dai.node.XLinkOut)
562xoutDisparity = pipeline.create(dai.node.XLinkOut)
563xoutRectifLeft = pipeline.create(dai.node.XLinkOut)
564xoutRectifRight = pipeline.create(dai.node.XLinkOut)
565xoutStereoCfg = pipeline.create(dai.node.XLinkOut)
566if args.debug:
567    xoutDebugLrCheckIt1 = pipeline.create(dai.node.XLinkOut)
568    xoutDebugLrCheckIt2 = pipeline.create(dai.node.XLinkOut)
569    xoutDebugExtLrCheckIt1 = pipeline.create(dai.node.XLinkOut)
570    xoutDebugExtLrCheckIt2 = pipeline.create(dai.node.XLinkOut)
571if args.dumpdisparitycostvalues:
572    xoutDebugCostDump = pipeline.create(dai.node.XLinkOut)
573
574xinStereoDepthConfig.setStreamName("stereoDepthConfig")
575monoLeft.setStreamName("in_left")
576monoRight.setStreamName("in_right")
577
578xoutLeft.setStreamName("left")
579xoutRight.setStreamName("right")
580xoutDepth.setStreamName("depth")
581xoutConfMap.setStreamName("confidence_map")
582xoutDisparity.setStreamName("disparity")
583xoutRectifLeft.setStreamName("rectified_left")
584xoutRectifRight.setStreamName("rectified_right")
585xoutStereoCfg.setStreamName("stereo_cfg")
586if args.debug:
587    xoutDebugLrCheckIt1.setStreamName("disparity_lr_check_iteration1")
588    xoutDebugLrCheckIt2.setStreamName("disparity_lr_check_iteration2")
589    xoutDebugExtLrCheckIt1.setStreamName("disparity_ext_lr_check_iteration1")
590    xoutDebugExtLrCheckIt2.setStreamName("disparity_ext_lr_check_iteration2")
591if args.dumpdisparitycostvalues:
592    xoutDebugCostDump.setStreamName("disparity_cost_dump")
593
594# Properties
595stereo.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
596stereo.setRectifyEdgeFillColor(0) # Black, to better see the cutout
597stereo.setLeftRightCheck(lrcheck)
598stereo.setExtendedDisparity(extended)
599stereo.setSubpixel(subpixel)
600
601# Switching depthAlign mode at runtime is not supported while aligning to a specific camera is enabled
602# stereo.setDepthAlign(dai.CameraBoardSocket.LEFT)
603
604# allocates resources for worst case scenario
605# allowing runtime switch of stereo modes
606stereo.setRuntimeModeSwitch(True)
607
608# Linking
609if(args.swapLR):
610    monoLeft.out.link(stereo.right)
611    monoRight.out.link(stereo.left)
612else:
613    monoLeft.out.link(stereo.left)
614    monoRight.out.link(stereo.right)
615xinStereoDepthConfig.out.link(stereo.inputConfig)
616stereo.syncedLeft.link(xoutLeft.input)
617stereo.syncedRight.link(xoutRight.input)
618if outDepth:
619    stereo.depth.link(xoutDepth.input)
620if outConfidenceMap:
621    stereo.confidenceMap.link(xoutConfMap.input)
622stereo.disparity.link(xoutDisparity.input)
623if outRectified:
624    stereo.rectifiedLeft.link(xoutRectifLeft.input)
625    stereo.rectifiedRight.link(xoutRectifRight.input)
626stereo.outConfig.link(xoutStereoCfg.input)
627if args.debug:
628    stereo.debugDispLrCheckIt1.link(xoutDebugLrCheckIt1.input)
629    stereo.debugDispLrCheckIt2.link(xoutDebugLrCheckIt2.input)
630    stereo.debugExtDispLrCheckIt1.link(xoutDebugExtLrCheckIt1.input)
631    stereo.debugExtDispLrCheckIt2.link(xoutDebugExtLrCheckIt2.input)
632if args.dumpdisparitycostvalues:
633    stereo.debugDispCostDump.link(xoutDebugCostDump.input)
634
635
636StereoConfigHandler(stereo.initialConfig.get())
637StereoConfigHandler.registerWindow("Stereo control panel")
638
639# stereo.setPostProcessingHardwareResources(3, 3)
640if(args.calibration):
641    calibrationHandler = dai.CalibrationHandler(args.calibration)
642    pipeline.setCalibrationData(calibrationHandler)
643stereo.setInputResolution(width, height)
644stereo.setRectification(args.rectify)
645baseline = 75
646fov = 71.86
647focal = width / (2 * math.tan(fov / 2 / 180 * math.pi))
648
649stereo.setBaseline(baseline/10)
650stereo.setFocalLength(focal)
651
652streams = ['left', 'right']
653if outRectified:
654    streams.extend(["rectified_left", "rectified_right"])
655streams.append("disparity")
656if outDepth:
657    streams.append("depth")
658if outConfidenceMap:
659    streams.append("confidence_map")
660debugStreams = []
661if args.debug:
662    debugStreams.extend(["disparity_lr_check_iteration1", "disparity_lr_check_iteration2"])
663    debugStreams.extend(["disparity_ext_lr_check_iteration1", "disparity_ext_lr_check_iteration2"])
664if args.dumpdisparitycostvalues:
665    debugStreams.append("disparity_cost_dump")
666
667def convertToCv2Frame(name, image, config):
668
669    maxDisp = config.getMaxDisparity()
670    subpixelLevels = pow(2, config.get().algorithmControl.subpixelFractionalBits)
671    subpixel = config.get().algorithmControl.enableSubpixel
672    dispIntegerLevels = maxDisp if not subpixel else maxDisp / subpixelLevels
673
674    frame = image.getFrame()
675
676    # frame.tofile(name+".raw")
677
678    if name == "depth":
679        dispScaleFactor = baseline * focal
680        with np.errstate(divide="ignore"):
681            frame = dispScaleFactor / frame
682
683        frame = (frame * 255. / dispIntegerLevels).astype(np.uint8)
684        frame = cv2.applyColorMap(frame, cv2.COLORMAP_HOT)
685    elif "confidence_map" in name:
686        pass
687    elif name == "disparity_cost_dump":
688        # frame.tofile(name+".raw")
689        pass
690    elif "disparity" in name:
691        if 1: # Optionally, extend disparity range to better visualize it
692            frame = (frame * 255. / maxDisp).astype(np.uint8)
693        return frame
694        # if 1: # Optionally, apply a color map
695        #     frame = cv2.applyColorMap(frame, cv2.COLORMAP_HOT)
696
697    return frame
698
699class DatasetManager:
700    def __init__(self, path):
701        self.path = path
702        self.index = 0
703        self.names = [d for d in os.listdir(path) if os.path.isdir(os.path.join(path, d))]
704        if len(self.names) == 0:
705            raise RuntimeError("No dataset found at {}".format(path))
706
707    def get(self):
708        return os.path.join(self.path, self.names[self.index])
709
710    def get_name(self):
711        return self.names[self.index]
712
713    def next(self):
714        self.index = (self.index + 1) % len(self.names)
715        return self.get()
716
717    def prev(self):
718        self.index = (self.index - 1) % len(self.names)
719        return self.get()
720
721
722def read_pfm(file):
723    file = open(file, "rb")
724
725    color = None
726    width = None
727    height = None
728    scale = None
729    endian = None
730
731    header = file.readline().rstrip()
732    if header.decode("ascii") == "PF":
733        color = True
734    elif header.decode("ascii") == "Pf":
735        color = False
736    else:
737        raise Exception("Not a PFM file.")
738
739    dim_match = re.search(r"(\d+)\s(\d+)", file.readline().decode("ascii"))
740    if dim_match:
741        width, height = map(int, dim_match.groups())
742    else:
743        raise Exception("Malformed PFM header.")
744
745    scale = float(file.readline().rstrip())
746    if scale < 0: # little-endian
747        endian = "<"
748        scale = -scale
749    else:
750        endian = ">" # big-endian
751
752    data = np.fromfile(file, endian + "f")
753    shape = (height, width, 3) if color else (height, width)
754    return np.flip(np.reshape(data, shape), axis=0), scale
755
756def calculate_err_measures(gt_img, oak_img):
757    assert gt_img.shape == oak_img.shape
758
759    gt_mask = gt_img != np.inf
760    oak_mask = oak_img != np.inf
761    mask = gt_mask & oak_mask
762
763    gt_img[~gt_mask] = 0.
764    oak_img[~mask] = 0.
765    err = np.abs(gt_img - oak_img)
766
767    n = np.sum(gt_mask)
768    invalid = np.sum(gt_mask & ~oak_mask)
769
770    bad05 = np.sum(mask & (err > 0.5))
771    bad1 = np.sum(mask & (err > 1.))
772    bad2 = np.sum(mask & (err > 2.))
773    bad4 = np.sum(mask & (err > 4.))
774    sum_err = np.sum(err[mask])
775    sum_sq_err = np.sum(err[mask] ** 2)
776    errs = err[mask]
777
778    bad05_p = 100. * bad05 / n
779    total_bad05_p = 100. * (bad05 + invalid) / n
780    bad1_p = 100. * bad1 / n
781    total_bad1_p = 100. * (bad1 + invalid) / n
782    bad2_p = 100. * bad2 / n
783    total_bad2_p = 100. * (bad2 + invalid) / n
784    bad4_p = 100. * bad4 / n
785    total_bad4_p = 100. * (bad4 + invalid) / n
786    invalid_p = 100. * invalid / n
787    avg_err = sum_err / (n - invalid)
788    mse = sum_sq_err / (n - invalid)
789    a50 = np.percentile(errs, 50)
790    a90 = np.percentile(errs, 90)
791    a95 = np.percentile(errs, 95)
792    a99 = np.percentile(errs, 99)
793
794    return {
795        "bad0.5": bad05_p,
796        "total_bad0.5": total_bad05_p,
797        "bad1": bad1_p,
798        "total_bad1": total_bad1_p,
799        "bad2": bad2_p,
800        "total_bad2": total_bad2_p,
801        "bad4": bad4_p,
802        "total_bad4": total_bad4_p,
803        "invalid": invalid_p,
804        "avg_err": avg_err,
805        "mse": mse,
806        "a50": a50,
807        "a90": a90,
808        "a95": a95,
809        "a99": a99
810    }
811
812def show_evaluation(img_name, evals):
813    cv2.namedWindow("Evaluation", cv2.WINDOW_NORMAL)
814    font = cv2.FONT_HERSHEY_SIMPLEX
815    font_scale = 2
816    thickness = 3
817    color = (0, 0, 0)
818    lines = [
819        f"Name: {img_name}",
820        f"Bad0.5: {evals['bad0.5']:.2f}%",
821        f"Total Bad0.5: {evals['total_bad0.5']:.2f}%",
822        f"Bad1: {evals['bad1']:.2f}%",
823        f"Total Bad1: {evals['total_bad1']:.2f}%",
824        f"Bad2: {evals['bad2']:.2f}%",
825        f"Total Bad2: {evals['total_bad2']:.2f}%",
826        f"Bad4: {evals['bad4']:.2f}%",
827        f"Total Bad4: {evals['total_bad4']:.2f}%",
828        f"Invalid: {evals['invalid']:.2f}%",
829        f"Avg Err: {evals['avg_err']:.2f}",
830        f"MSE: {evals['mse']:.2f}",
831        f"A50: {evals['a50']:.2f}",
832        f"A90: {evals['a90']:.2f}",
833        f"A95: {evals['a95']:.2f}",
834        f"A99: {evals['a99']:.2f}"
835    ]
836    sizes = [cv2.getTextSize(line, font, font_scale, thickness) for line in lines]
837    sizes = [(size[0][0], size[0][1] + size[1], size[1]) for size in sizes]
838    max_width = max([size[0] for size in sizes])
839    total_height = sum([size[1] for size in sizes]) + (len(lines) - 1) * thickness
840    img = np.ones((total_height + thickness, max_width, 3), dtype=np.uint8) * 255
841    y = 0
842    for line, size in zip(lines, sizes):
843        cv2.putText(img, line, (0, y + size[1] - size[2]), font, font_scale, color, thickness)
844        y += size[1] + thickness
845    cv2.imshow("Evaluation", img)
846
847def show_debug_disparity(gt_img, oak_img):
848    def rescale_img(img):
849        img[img == np.inf] = 0.
850        img = cv2.resize(img, (1280, 800), interpolation=cv2.INTER_AREA)
851        return img.astype(np.uint16)
852
853    gt_img = rescale_img(gt_img)
854    oak_img = rescale_img(oak_img)
855    maxv = max(gt_img.max(), oak_img.max())
856    gt_img = (gt_img * 255. / maxv).astype(np.uint8)
857    oak_img = (oak_img * 255. / maxv).astype(np.uint8)
858    cv2.imshow("GT", gt_img)
859    cv2.imshow("OAK", oak_img)
860
861if evaluation_mode:
862    dataset = DatasetManager(args.evaluate)
863
864print("Connecting and starting the pipeline")
865# Connect to device and start pipeline
866with dai.Device(pipeline) as device:
867
868    stereoDepthConfigInQueue = device.getInputQueue("stereoDepthConfig")
869    inStreams = ["in_left", "in_right"]
870    inStreamsCameraID = [dai.CameraBoardSocket.CAM_B, dai.CameraBoardSocket.CAM_C]
871    in_q_list = []
872    for s in inStreams:
873        q = device.getInputQueue(s)
874        in_q_list.append(q)
875
876    # Create a receive queue for each stream
877    q_list = []
878    for s in streams:
879        q = device.getOutputQueue(s, 8, blocking=False)
880        q_list.append(q)
881
882    inCfg = device.getOutputQueue("stereo_cfg", 8, blocking=False)
883
884    # Need to set a timestamp for input frames, for the sync stage in Stereo node
885    timestamp_ms = 0
886    index = 0
887    prevQueues = q_list.copy()
888    while True:
889        # Handle input streams, if any
890        if in_q_list:
891            dataset_size = 1  # Number of image pairs
892            frame_interval_ms = 50
893            for i, q in enumerate(in_q_list):
894                path = os.path.join(dataset.get(), f"im{i}.png") if evaluation_mode else args.dataset + "/" + str(index) + "/" + q.getName() + ".png"
895                data = cv2.imread(path, cv2.IMREAD_GRAYSCALE)
896                data = cv2.resize(data, (width, height), interpolation = cv2.INTER_AREA)
897                data = data.reshape(height*width)
898                tstamp = datetime.timedelta(seconds = timestamp_ms // 1000,
899                                            milliseconds = timestamp_ms % 1000)
900                img = dai.ImgFrame()
901                img.setData(data)
902                img.setTimestamp(tstamp)
903                img.setInstanceNum(inStreamsCameraID[i])
904                img.setType(dai.ImgFrame.Type.RAW8)
905                img.setWidth(width)
906                img.setHeight(height)
907                q.send(img)
908                # print("Sent frame: {:25s}".format(path), "timestamp_ms:", timestamp_ms)
909            timestamp_ms += frame_interval_ms
910            index = (index + 1) % dataset_size
911            sleep(frame_interval_ms / 1000)
912
913        gt_disparity = None
914        if evaluation_mode:
915            # Load GT disparity
916            gt_disparity = read_pfm(os.path.join(dataset.get(), f"disp1.pfm"))[0]
917
918        # Handle output streams
919        currentConfig = inCfg.get()
920
921        lrCheckEnabled = currentConfig.get().algorithmControl.enableLeftRightCheck
922        extendedEnabled = currentConfig.get().algorithmControl.enableExtended
923        queues = q_list.copy()
924
925        if args.dumpdisparitycostvalues:
926            q = device.getOutputQueue("disparity_cost_dump", 8, blocking=False)
927            queues.append(q)
928
929        if args.debug:
930            q_list_debug = []
931
932            activeDebugStreams = []
933            if lrCheckEnabled:
934                activeDebugStreams.extend(["disparity_lr_check_iteration1", "disparity_lr_check_iteration2"])
935            if extendedEnabled:
936                activeDebugStreams.extend(["disparity_ext_lr_check_iteration1"])
937                if lrCheckEnabled:
938                    activeDebugStreams.extend(["disparity_ext_lr_check_iteration2"])
939
940            for s in activeDebugStreams:
941                q = device.getOutputQueue(s, 8, blocking=False)
942                q_list_debug.append(q)
943
944            queues.extend(q_list_debug)
945
946        def ListDiff(li1, li2):
947            return list(set(li1) - set(li2)) + list(set(li2) - set(li1))
948
949        diff = ListDiff(prevQueues, queues)
950        for s in diff:
951            name = s.getName()
952            cv2.destroyWindow(name)
953        prevQueues = queues.copy()
954
955        disparity = None
956        for q in queues:
957            if q.getName() in ["left", "right"]: continue
958            data = q.get()
959            if q.getName() == "disparity":
960                disparity = data.getFrame()
961            frame = convertToCv2Frame(q.getName(), data, currentConfig)
962            cv2.imshow(q.getName(), frame)
963
964        if disparity is not None and gt_disparity is not None:
965            subpixel_bits = 1 << currentConfig.get().algorithmControl.subpixelFractionalBits
966            subpixel_enabled = currentConfig.get().algorithmControl.enableSubpixel
967            width_scale = float(gt_disparity.shape[1]) / float(disparity.shape[1])
968
969            disparity = disparity.astype(np.float32)
970            if subpixel_enabled:
971                disparity = disparity / subpixel_bits
972            disparity = disparity * width_scale
973            disparity = cv2.resize(disparity, (gt_disparity.shape[1], gt_disparity.shape[0]), interpolation = cv2.INTER_LINEAR)
974            disparity[disparity == 0.] = np.inf
975
976            # show_debug_disparity(gt_disparity, disparity)
977            err_vals = calculate_err_measures(gt_disparity, disparity)
978            show_evaluation(dataset.get_name(), err_vals)
979
980        key = cv2.waitKey(1)
981        if key == ord("q"):
982            break
983        elif evaluation_mode and key == ord("["):
984            dataset.next()
985        elif evaluation_mode and key == ord("]"):
986            dataset.prev()
987
988        StereoConfigHandler.handleKeypress(key, stereoDepthConfigInQueue)