From 32eb2f618f3bc1a6490af3f7d7adb3cc74a5f03b Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Tue, 2 Aug 2022 12:27:48 +0200 Subject: [PATCH] WIP --- camera/depthai.py | 59 ++++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 58 insertions(+), 1 deletion(-) diff --git a/camera/depthai.py b/camera/depthai.py index 2e60965..69da145 100644 --- a/camera/depthai.py +++ b/camera/depthai.py @@ -6,9 +6,16 @@ import events.events_pb2 import depthai as dai import cv2 +import numpy as np logger = logging.getLogger(__name__) +# 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 = True +# Better handling for occlusions: +lr_check = True class FramePublisher: def __init__(self, mqtt_client: mqtt.Client, frame_topic: str, img_width: int, img_height: int): @@ -16,6 +23,7 @@ class FramePublisher: self._frame_topic = frame_topic self._img_width = img_width self._img_height = img_height + self._depth = None self._pipeline = self._configure_pipeline() def _configure_pipeline(self) -> dai.Pipeline: @@ -27,6 +35,45 @@ class FramePublisher: xout_rgb.setStreamName("rgb") + monoLeft = pipeline.create(dai.node.MonoCamera) + monoRight = pipeline.create(dai.node.MonoCamera) + depth = pipeline.create(dai.node.StereoDepth) + xout = pipeline.create(dai.node.XLinkOut) + self._depth = depth + + xout.setStreamName("disparity") + + # Properties + monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) + monoLeft.setBoardSocket(dai.CameraBoardSocket.LEFT) + monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P) + monoRight.setBoardSocket(dai.CameraBoardSocket.RIGHT) + + # Create a node that will produce the depth map (using disparity output as it's easier to visualize depth this way) + depth.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY) + # Options: MEDIAN_OFF, KERNEL_3x3, KERNEL_5x5, KERNEL_7x7 (default) + depth.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7) + depth.setLeftRightCheck(lr_check) + depth.setExtendedDisparity(extended_disparity) + depth.setSubpixel(subpixel) + + config = depth.initialConfig.get() + config.postProcessing.speckleFilter.enable = True + config.postProcessing.speckleFilter.speckleRange = 50 + config.postProcessing.temporalFilter.enable = False + config.postProcessing.spatialFilter.enable = False + config.postProcessing.spatialFilter.holeFillingRadius = 2 + config.postProcessing.spatialFilter.numIterations = 1 + #config.postProcessing.thresholdFilter.minRange = 400 + #config.postProcessing.thresholdFilter.maxRange = 15000 + config.postProcessing.decimationFilter.decimationFactor = 2 + depth.initialConfig.set(config) + + # Linking + monoLeft.out.link(depth.left) + monoRight.out.link(depth.right) + depth.disparity.link(xout.input) + # Properties cam_rgb.setBoardSocket(dai.CameraBoardSocket.RGB) cam_rgb.setPreviewSize(width=self._img_width, height=self._img_height) @@ -53,12 +100,22 @@ class FramePublisher: queue_size = 4 q_rgb = device.getOutputQueue("rgb", maxSize=queue_size, blocking=False) + # Output queue will be used to get the disparity frames from the outputs defined above + q_disparity = device.getOutputQueue(name="disparity", maxSize=4, blocking=False) + while True: try: logger.debug("wait for new frame") inRgb = q_rgb.get() # blocking call, will wait until a new data has arrived + inDisparity = q_disparity.get() + # im_resize = inRgb.getCvFrame() + im_resize = inDisparity.getCvFrame() - im_resize = inRgb.getCvFrame() + # Normalization for better visualization + im_resize = (im_resize * (255 / self._depth.initialConfig.getMaxDisparity())).astype(np.uint8) + + # Available color maps: https://docs.opencv.org/3.4/d3/d50/group__imgproc__colormap.html + # im_resize = cv2.applyColorMap(im_resize, cv2.COLORMAP_JET) is_success, im_buf_arr = cv2.imencode(".jpg", im_resize) byte_im = im_buf_arr.tobytes()