DepthAI Tutorials
DepthAI API References

ON THIS PAGE

  • Stereo Depth from host
  • Similar samples:
  • Setup
  • Source code

Stereo Depth from host

This example shows depth map from host using stereo images. There are 3 depth modes which you can select inside the code:
  • lr_check: used for better occlusion handling. For more information click here
  • extended_disparity: suitable for short range objects. For more information click here
  • subpixel: suitable for long range. For more information click here
Otherwise a median with kernel_7x7 is activated.

Similar samples:

Setup

Please run the install script to download all required dependencies. Please note that this script must be ran from git context, so you have to download the depthai-python repository first and then run the script
Command Line
1git clone https://github.com/luxonis/depthai-python.git
2cd depthai-python/examples
3python3 install_requirements.py
For additional information, please follow the installation guide.This example also requires dataset folder - you can download it from here

Source code

Python
Python
GitHub
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 = []