Merge pull request #220 from luxonis/fix_demo_disparity

Fix demo disparity

examples/03_depth_preview.py

@@ -4,6 +4,24 @@
 import depthai as dai
 import numpy as np
 
+
+'''
+If one or more of the additional depth modes (lrcheck, extended, subpixel)
+are enabled, then:
+ - depth output is FP16. TODO enable U16.
+ - median filtering is disabled on device. TODO enable.
+ - with subpixel, either depth or disparity has valid data.
+Otherwise, depth output is U16 (mm) and median is functional.
+But like on Gen1, either depth or disparity has valid data. TODO enable both.
+'''
+
+# Closer-in minimum depth, disparity range is doubled (from 95 to 190):
+extended_disparity = False
+# Better accuracy for longer distance, fractional disparity 32-levels:
+subpixel = False
+# Better handling for occlusions:
+lr_check = False
+
 # Start defining a pipeline
 pipeline = dai.Pipeline()
 
@@ -19,25 +37,25 @@
 # Create a node that will produce the depth map (using disparity output as it's easier to visualize depth this way)
 depth = pipeline.createStereoDepth()
 depth.setConfidenceThreshold(200)
+depth.setOutputDepth(False)
 # Options: MEDIAN_OFF, KERNEL_3x3, KERNEL_5x5, KERNEL_7x7 (default)
 median = dai.StereoDepthProperties.MedianFilter.KERNEL_7x7 # For depth filtering
 depth.setMedianFilter(median)
 
-'''
-If one or more of the additional depth modes (lrcheck, extended, subpixel)
-are enabled, then:
- - depth output is FP16. TODO enable U16.
- - median filtering is disabled on device. TODO enable.
- - with subpixel, either depth or disparity has valid data.
-Otherwise, depth output is U16 (mm) and median is functional.
-But like on Gen1, either depth or disparity has valid data. TODO enable both.
-'''
-# Better handling for occlusions:
-depth.setLeftRightCheck(False)
-# Closer-in minimum depth, disparity range is doubled:
-depth.setExtendedDisparity(False)
-# Better accuracy for longer distance, fractional disparity 32-levels:
-depth.setSubpixel(False)
+depth.setLeftRightCheck(lr_check)
+
+# Normal disparity values range from 0..95, will be used for normalization
+max_disparity = 95
+
+if extended_disparity: max_disparity *= 2 # Double the range
+depth.setExtendedDisparity(extended_disparity)
+
+if subpixel: max_disparity *= 32 # 5 fractional bits, x32
+depth.setSubpixel(subpixel)
+
+# When we get disparity to the host, we will multiply all values with the multiplier
+# for better visualization
+multiplier = 255 / max_disparity
 
 left.out.link(depth.left)
 right.out.link(depth.right)
@@ -54,12 +72,10 @@
 
     # Output queue will be used to get the disparity frames from the outputs defined above
     q = device.getOutputQueue(name="disparity", maxSize=4, blocking=False)
-
     while True:
         inDepth = q.get()  # blocking call, will wait until a new data has arrived
         frame = inDepth.getFrame()
-        frame = cv2.normalize(frame, None, 0, 255, cv2.NORM_MINMAX)
-
+        frame = (frame*multiplier).astype(np.uint8)
         # Available color maps: https://docs.opencv.org/3.4/d3/d50/group__imgproc__colormap.html
         frame = cv2.applyColorMap(frame, cv2.COLORMAP_JET)
 

examples/10_mono_depth_mobilenetssd.py

@@ -55,10 +55,9 @@
 manip.out.link(nn.input)
 
 # Create outputs
-depthOut = pipeline.createXLinkOut()
-depthOut.setStreamName("depth")
-
-stereo.disparity.link(depthOut.input)
+disparityOut = pipeline.createXLinkOut()
+disparityOut.setStreamName("disparity")
+stereo.disparity.link(disparityOut.input)
 
 xoutRight = pipeline.createXLinkOut()
 xoutRight.setStreamName("rectifiedRight")
@@ -79,7 +78,7 @@
 
     # Output queues will be used to get the grayscale / depth frames and nn data from the outputs defined above
     qRight = device.getOutputQueue("rectifiedRight", maxSize=4, blocking=False)
-    qDepth = device.getOutputQueue("depth", maxSize=4, blocking=False)
+    qDisparity = device.getOutputQueue("disparity", maxSize=4, blocking=False)
     qDet = device.getOutputQueue("nn", maxSize=4, blocking=False)
 
     rightFrame = None
@@ -104,11 +103,12 @@ def show(name, frame):
         # Show the frame
         cv2.imshow(name, frame)
 
+    disparity_multiplier = 255 / 95 # Disparity range is 0..95
     while True:
         # Instead of get (blocking), we use tryGet (nonblocking) which will return the available data or None otherwise
         inRight = qRight.tryGet()
         inDet = qDet.tryGet()
-        inDepth = qDepth.tryGet()
+        inDisparity = qDisparity.tryGet()
 
         if inRight is not None:
             rightFrame = inRight.getCvFrame()
@@ -124,13 +124,13 @@ def show(name, frame):
                     detection.xmin = 1 - detection.xmax
                     detection.xmax = 1 - swap
 
-        if inDepth is not None:
-            # Frame is transformed, the color map will be applied to highlight the depth info
+        if inDisparity is not None:
+            # Frame is transformed, normalized, and color map will be applied to highlight the depth info
+            disparityFrame = inDisparity.getFrame()
+            disparityFrame = (disparityFrame*disparity_multiplier).astype(np.uint8)
             # Available color maps: https://docs.opencv.org/3.4/d3/d50/group__imgproc__colormap.html
-            depthFrame = cv2.applyColorMap(inDepth.getFrame(), cv2.COLORMAP_JET)
-
-        if depthFrame is not None:
-            show("depth", depthFrame)
+            disparityFrame = cv2.applyColorMap(disparityFrame, cv2.COLORMAP_JET)
+            show("disparity", disparityFrame)
 
         if rightFrame is not None:
             show("rectified right", rightFrame)

examples/12_rgb_encoding_mono_mobilenet_depth.py

@@ -6,6 +6,8 @@
 import depthai as dai
 import numpy as np
 
+flipRectified = True
+
 # Get argument first
 nnPath = str((Path(__file__).parent / Path('models/mobilenet-ssd_openvino_2021.2_6shave.blob')).resolve().absolute())
 if len(sys.argv) > 1:
@@ -40,13 +42,16 @@
 depth.setConfidenceThreshold(255)
 # Note: the rectified streams are horizontally mirrored by default
 depth.setOutputRectified(True)
+depth.setRectifyMirrorFrame(False)
 depth.setRectifyEdgeFillColor(0) # Black, to better see the cutout
 camLeft.out.link(depth.left)
 camRight.out.link(depth.right)
+# Disparity range is 0..95, used for normalization
+disparity_multiplier = 255 / 95
 
-depthOut = pipeline.createXLinkOut()
-depthOut.setStreamName("depth")
-depth.disparity.link(depthOut.input)
+disparityOut = pipeline.createXLinkOut()
+disparityOut.setStreamName("disparity")
+depth.disparity.link(disparityOut.input)
 
 nn = pipeline.createMobileNetDetectionNetwork()
 nn.setConfidenceThreshold(0.5)
@@ -85,14 +90,14 @@
 
     queueSize = 8
     qRight = device.getOutputQueue("right", queueSize)
-    qDepth = device.getOutputQueue("depth", queueSize)
+    qDisparity = device.getOutputQueue("disparity", queueSize)
     qManip = device.getOutputQueue("manip", queueSize)
     qDet = device.getOutputQueue("nn", queueSize)
     qRgbEnc = device.getOutputQueue('h265', maxSize=30, blocking=True)
 
     frame = None
     frameManip = None
-    frameDepth = None
+    frameDisparity = None
     detections = []
     offsetX = (camRight.getResolutionWidth() - camRight.getResolutionHeight()) // 2
     croppedFrame = np.zeros((camRight.getResolutionHeight(), camRight.getResolutionHeight()))
@@ -111,21 +116,26 @@ def frameNorm(frame, bbox):
         inRight = qRight.tryGet()
         inManip = qManip.tryGet()
         inDet = qDet.tryGet()
-        inDepth = qDepth.tryGet()
+        inDisparity = qDisparity.tryGet()
 
         while qRgbEnc.has():
             qRgbEnc.get().getData().tofile(videoFile)
 
         if inRight is not None:
             frame = cv2.flip(inRight.getCvFrame(), 1)
+            if flipRectified:
+                frame = cv2.flip(frame, 1)
 
         if inManip is not None:
             frameManip = inManip.getCvFrame()
 
-        if inDepth is not None:
-            frameDepth = cv2.flip(inDepth.getFrame(), 1)
-            frameDepth = cv2.normalize(frameDepth, None, 0, 255, cv2.NORM_MINMAX)
-            frameDepth = cv2.applyColorMap(frameDepth, cv2.COLORMAP_JET)
+        if inDisparity is not None:
+            # Flip disparity frame, normalize it and apply color map for better visualization
+            frameDisparity = inDisparity.getFrame()
+            if flipRectified:
+                frameDisparity = cv2.flip(frameDisparity, 1)
+            frameDisparity = (frameDisparity*disparity_multiplier).astype(np.uint8)
+            frameDisparity = cv2.applyColorMap(frameDisparity, cv2.COLORMAP_JET)
 
         if inDet is not None:
             detections = inDet.detections
@@ -139,14 +149,14 @@ def frameNorm(frame, bbox):
                 cv2.putText(frame, f"{int(detection.confidence * 100)}%", (bbox[0] + 10, bbox[1] + 40), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255)
             cv2.imshow("right", frame)
 
-        if frameDepth is not None:
+        if frameDisparity is not None:
             for detection in detections:
                 bbox = frameNorm(croppedFrame, (detection.xmin, detection.ymin, detection.xmax, detection.ymax))
                 bbox[::2] += offsetX
-                cv2.rectangle(frameDepth, (bbox[0], bbox[1]), (bbox[2], bbox[3]), (255, 0, 0), 2)
-                cv2.putText(frameDepth, labelMap[detection.label], (bbox[0] + 10, bbox[1] + 20), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255)
-                cv2.putText(frameDepth, f"{int(detection.confidence * 100)}%", (bbox[0] + 10, bbox[1] + 40), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255)
-            cv2.imshow("depth", frameDepth)
+                cv2.rectangle(frameDisparity, (bbox[0], bbox[1]), (bbox[2], bbox[3]), (255, 0, 0), 2)
+                cv2.putText(frameDisparity, labelMap[detection.label], (bbox[0] + 10, bbox[1] + 20), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255)
+                cv2.putText(frameDisparity, f"{int(detection.confidence * 100)}%", (bbox[0] + 10, bbox[1] + 40), cv2.FONT_HERSHEY_TRIPLEX, 0.5, 255)
+            cv2.imshow("disparity", frameDisparity)
 
         if frameManip is not None:
             for detection in detections: