Stereo depth from host

docs/source/index.rst

@@ -105,6 +105,7 @@ Now, pick a tutorial or code sample and start utilizing Gen2 capabilities
    samples/29_1_object_tracker.rst
    samples/29_2_spatial_object_tracker.rst
    samples/29_3_object_tracker_video.rst
+   samples/30_stereo_depth_from_host.rst
 
 .. toctree::
    :maxdepth: 1

docs/source/samples/30_stereo_depth_from_host.rst

@@ -0,0 +1,24 @@
+30 - 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: left-right check, extended (for closer distance), subpixel (for longer distance). Otherwise a median with kernel_7x7 is activated.
+
+Setup
+#####
+
+.. include::  /includes/install_from_pypi.rst
+
+This example also requires dataset folder - you can download it from
+`here <https://drive.google.com/file/d/1mPyghc_0odGtSU2cROS1uLD-qwRU8aug>`__
+
+
+Source code
+###########
+
+Also `available on GitHub <https://github.com/luxonis/depthai-python/blob/main/examples/30_stereo_depth_from_host.py>`__
+
+.. literalinclude:: ../../../examples/30_stereo_depth_from_host.py
+   :language: python
+   :linenos:
+
+.. include::  /includes/footer-short.rst

examples/07_mono_full_resolution_saver.py

@@ -23,7 +23,7 @@
 with dai.Device(pipeline) as device:
     # Start pipeline
     device.startPipeline()
-        
+
     # Output queue will be used to get the grayscale frames from the output defined above
     qRight = device.getOutputQueue(name="right", maxSize=4, blocking=False)
 

examples/30_stereo_depth_from_host.py

@@ -0,0 +1,188 @@
+#!/usr/bin/env python3
+
+import cv2
+import numpy as np
+import depthai as dai
+from time import sleep
+import datetime
+import argparse
+from pathlib import Path
+
+datasetDefault = str((Path(__file__).parent / Path('models/dataset')).resolve().absolute())
+parser = argparse.ArgumentParser()
+parser.add_argument('-dataset', nargs='?', help="Path to recorded frames", default=datasetDefault)
+args = parser.parse_args()
+
+if not Path(datasetDefault).exists():
+    import sys
+    raise FileNotFoundError(f'Required file/s not found, please run "{sys.executable} install_requirements.py"')
+
+
+# StereoDepth config options.
+out_depth = False  # Disparity by default
+out_rectified = True   # Output and display rectified streams
+lrcheck = True   # Better handling for occlusions
+extended = False  # Closer-in minimum depth, disparity range is doubled
+subpixel = True   # Better accuracy for longer distance, fractional disparity 32-levels
+median = dai.StereoDepthProperties.MedianFilter.KERNEL_7x7
+
+# Sanitize some incompatible options
+if lrcheck or extended or subpixel:
+    median = dai.StereoDepthProperties.MedianFilter.MEDIAN_OFF
+
+print("StereoDepth config options: ")
+print("Left-Right check: ", lrcheck)
+print("Extended disparity: ", extended)
+print("Subpixel: ", subpixel)
+print("Median filtering: ", median)
+
+right_intrinsic = [[860.0, 0.0, 640.0], [0.0, 860.0, 360.0], [0.0, 0.0, 1.0]]
+
+
+def create_stereo_depth_pipeline():
+    print("Creating Stereo Depth pipeline: ", end='')
+
+    print("XLINK IN -> STEREO -> XLINK OUT")
+    pipeline = dai.Pipeline()
+
+    camLeft = pipeline.createXLinkIn()
+    camRight = pipeline.createXLinkIn()
+    stereo = pipeline.createStereoDepth()
+    xoutLeft = pipeline.createXLinkOut()
+    xoutRight = pipeline.createXLinkOut()
+    xoutDepth = pipeline.createXLinkOut()
+    xoutDisparity = pipeline.createXLinkOut()
+    xoutRectifLeft = pipeline.createXLinkOut()
+    xoutRectifRight = pipeline.createXLinkOut()
+
+    camLeft.setStreamName('in_left')
+    camRight.setStreamName('in_right')
+
+    stereo.setOutputDepth(out_depth)
+    stereo.setOutputRectified(out_rectified)
+    stereo.setConfidenceThreshold(200)
+    stereo.setRectifyEdgeFillColor(0) # Black, to better see the cutout
+    stereo.setMedianFilter(median) # KERNEL_7x7 default
+    stereo.setLeftRightCheck(lrcheck)
+    stereo.setExtendedDisparity(extended)
+    stereo.setSubpixel(subpixel)
+
+    stereo.setEmptyCalibration() # Set if the input frames are already rectified
+    stereo.setInputResolution(1280, 720)
+
+    xoutLeft.setStreamName('left')
+    xoutRight.setStreamName('right')
+    xoutDepth.setStreamName('depth')
+    xoutDisparity.setStreamName('disparity')
+    xoutRectifLeft.setStreamName('rectified_left')
+    xoutRectifRight.setStreamName('rectified_right')
+
+    camLeft.out.link(stereo.left)
+    camRight.out.link(stereo.right)
+    stereo.syncedLeft.link(xoutLeft.input)
+    stereo.syncedRight.link(xoutRight.input)
+    stereo.depth.link(xoutDepth.input)
+    stereo.disparity.link(xoutDisparity.input)
+    stereo.rectifiedLeft.link(xoutRectifLeft.input)
+    stereo.rectifiedRight.link(xoutRectifRight.input)
+
+    streams = ['left', 'right']
+    if out_rectified:
+        streams.extend(['rectified_left', 'rectified_right'])
+    streams.extend(['disparity', 'depth'])
+
+    return pipeline, streams
+
+
+def convert_to_cv2_frame(name, image):
+    baseline = 75 #mm
+    focal = right_intrinsic[0][0]
+    max_disp = 96
+    disp_type = np.uint8
+    disp_levels = 1
+    if (extended):
+        max_disp *= 2
+    if (subpixel):
+        max_disp *= 32
+        disp_type = np.uint16
+        disp_levels = 32
+
+    data, w, h = image.getData(), image.getWidth(), image.getHeight()
+    if name == 'depth':
+        # this contains FP16 with (lrcheck or extended or subpixel)
+        frame = np.array(data).astype(np.uint8).view(np.uint16).reshape((h, w))
+    elif name == 'disparity':
+        disp = np.array(data).astype(np.uint8).view(disp_type).reshape((h, w))
+
+        # Compute depth from disparity
+        with np.errstate(divide='ignore'):
+            depth = (disp_levels * baseline * focal / disp).astype(np.uint16)
+
+        if 1: # Optionally, extend disparity range to better visualize it
+            frame = (disp * 255. / max_disp).astype(np.uint8)
+
+        if 1: # Optionally, apply a color map
+            frame = cv2.applyColorMap(frame, cv2.COLORMAP_HOT)
+
+    else: # mono streams / single channel
+        frame = np.array(data).reshape((h, w)).astype(np.uint8)
+        if name.startswith('rectified_'):
+            frame = cv2.flip(frame, 1)
+        if name == 'rectified_right':
+            last_rectif_right = frame
+    return frame
+
+
+pipeline, streams = create_stereo_depth_pipeline()
+
+print("Creating DepthAI device")
+with dai.Device(pipeline) as device:
+    print("Starting pipeline")
+    device.startPipeline()
+
+    inStreams = ['in_right', 'in_left']
+    inStreamsCameraID = [dai.CameraBoardSocket.RIGHT, dai.CameraBoardSocket.LEFT]
+    in_q_list = []
+    for s in inStreams:
+        q = device.getInputQueue(s)
+        in_q_list.append(q)
+
+    # Create a receive queue for each stream
+    q_list = []
+    for s in streams:
+        q = device.getOutputQueue(s, 8, blocking=False)
+        q_list.append(q)
+
+    # Need to set a timestamp for input frames, for the sync stage in Stereo node
+    timestamp_ms = 0
+    index = 0
+    while True:
+        # Handle input streams, if any
+        if in_q_list:
+            dataset_size = 2  # Number of image pairs
+            frame_interval_ms = 500
+            for i, q in enumerate(in_q_list):
+                path = args.dataset + '/' + str(index) + '/' + q.getName() + '.png'
+                data = cv2.imread(path, cv2.IMREAD_GRAYSCALE).reshape(720*1280)
+                tstamp = datetime.timedelta(seconds = timestamp_ms // 1000,
+                                            milliseconds = timestamp_ms % 1000)
+                img = dai.ImgFrame()
+                img.setData(data)
+                img.setTimestamp(tstamp)
+                img.setInstanceNum(inStreamsCameraID[i])
+                img.setWidth(1280)
+                img.setHeight(720)
+                q.send(img)
+                if timestamp_ms == 0:  # Send twice for first iteration
+                    q.send(img)
+                print("Sent frame: {:25s}".format(path), 'timestamp_ms:', timestamp_ms)
+            timestamp_ms += frame_interval_ms
+            index = (index + 1) % dataset_size
+            sleep(frame_interval_ms / 1000)
+        # Handle output streams
+        for q in q_list:
+            if q.getName() in ['left', 'right', 'depth']: continue
+            frame = convert_to_cv2_frame(q.getName(), q.get())
+            cv2.imshow(q.getName(), frame)
+        if cv2.waitKey(1) == ord('q'):
+            break

examples/CMakeLists.txt

@@ -119,3 +119,4 @@ add_python_example(28_camera_video_example 28_camera_video_example.py)
 add_python_example(29_1_object_tracker 29_1_object_tracker.py)
 add_python_example(29_2_spatial_object_tracker 29_2_spatial_object_tracker.py)
 add_python_example(29_3_object_tracker_video 29_3_object_tracker_video.py)
+add_python_example(30_stereo_depth_from_host 30_stereo_depth_from_host.py)

examples/models/.gitignore

@@ -1,2 +1,3 @@
 *.blob
-*.mp4
+*.mp4
+dataset/

examples/models/stereo-dataset/model.yml

@@ -0,0 +1,31 @@
+# Copyright (c) 2021 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+description: >-
+  stereo dataset to test frames sent from host
+task_type: object_attributes
+files:
+  - name: dataset.zip
+    size: 2361554
+    sha256: 19abe0e4126fc32ea45441abaaf5bed39603493530f616f416531c1c2fe05b86
+    source:
+      $type: google_drive
+      id: 1mPyghc_0odGtSU2cROS1uLD-qwRU8aug
+postprocessing:
+  - $type: unpack_archive
+    format: zip
+    file: dataset.zip
+
+framework: dldt
+license: https://vision.middlebury.edu/stereo/data/scenes2014/