diff --git a/docs/source/index.rst b/docs/source/index.rst index 8f8bd1dae..a85fedc02 100644 --- a/docs/source/index.rst +++ b/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 diff --git a/docs/source/samples/30_stereo_depth_from_host.rst b/docs/source/samples/30_stereo_depth_from_host.rst new file mode 100644 index 000000000..9a866a5ff --- /dev/null +++ b/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 `__ + + +Source code +########### + +Also `available on GitHub `__ + +.. literalinclude:: ../../../examples/30_stereo_depth_from_host.py + :language: python + :linenos: + +.. include:: /includes/footer-short.rst diff --git a/examples/07_mono_full_resolution_saver.py b/examples/07_mono_full_resolution_saver.py index 83749f01c..746aef249 100755 --- a/examples/07_mono_full_resolution_saver.py +++ b/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) diff --git a/examples/30_stereo_depth_from_host.py b/examples/30_stereo_depth_from_host.py new file mode 100755 index 000000000..bdd6af3da --- /dev/null +++ b/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 diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 563add984..b504319d1 100644 --- a/examples/CMakeLists.txt +++ b/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) diff --git a/examples/models/.gitignore b/examples/models/.gitignore index 2d85c3ce1..cb1534774 100644 --- a/examples/models/.gitignore +++ b/examples/models/.gitignore @@ -1,2 +1,3 @@ *.blob -*.mp4 \ No newline at end of file +*.mp4 +dataset/ \ No newline at end of file diff --git a/examples/models/stereo-dataset/model.yml b/examples/models/stereo-dataset/model.yml new file mode 100644 index 000000000..65b98e1d7 --- /dev/null +++ b/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/