disable disparity frames

Dockerfile

@@ -1,10 +1,10 @@
-FROM docker.io/library/python:3.11-slim as base
+FROM docker.io/library/python:3.12-slim as base
 
 # Configure piwheels repo to use pre-compiled numpy wheels for arm
 RUN echo -n "[global]\n" > /etc/pip.conf &&\
     echo -n "extra-index-url = https://www.piwheels.org/simple https://git.cyrilix.bzh/api/packages/robocars/pypi/simple \n" >> /etc/pip.conf
 
-RUN apt-get update && apt-get install -y libgl1 libglib2.0-0
+RUN apt-get update && apt-get install -y libgl1 libglib2.0-0 procps cmake g++ gcc
 
 #################
 FROM base as model-builder

README.md

@@ -9,3 +9,136 @@ To build images, run script:
 ```bash 
 ./build-docker.sh
 ```
+
+## Usage
+
+```shell
+usage: cli.py [-h] [-u MQTT_USERNAME] [-p MQTT_PASSWORD] [-b MQTT_BROKER_HOST]
+              [-P MQTT_BROKER_PORT] [-C MQTT_CLIENT_ID]
+              [-c MQTT_TOPIC_ROBOCAR_OAK_CAMERA]
+              [-o MQTT_TOPIC_ROBOCAR_OBJECTS] [-t OBJECTS_THRESHOLD]
+              [-d MQTT_TOPIC_ROBOCAR_DISPARITY] [-f CAMERA_FPS]
+              [--camera-tuning-exposition {default,500us,8300us}]
+              [-H IMAGE_HEIGHT] [-W IMAGE_WIDTH] [--log {info,debug}]
+              [--stereo-mode-lr-check] [--stereo-mode-extended-disparity]
+              [--stereo-mode-subpixel]
+              [--stereo-post-processing-median-filter]
+              [--stereo-post-processing-median-value {MEDIAN_OFF,KERNEL_3x3,KERNEL_5x5,KERNEL_7x7}]
+              [--stereo-post-processing-speckle-filter]
+              [--stereo-post-processing-speckle-enable STEREO_POST_PROCESSING_SPECKLE_ENABLE]
+              [--stereo-post-processing-speckle-range STEREO_POST_PROCESSING_SPECKLE_RANGE]
+              [--stereo-post-processing-temporal-filter]
+              [--stereo-post-processing-temporal-persistency-mode {PERSISTENCY_OFF,VALID_8_OUT_OF_8,VALID_2_IN_LAST_3,VALID_2_IN_LAST_4,VALID_2_OUT_OF_8,VALID_1_IN_LAST_2,VALID_1_IN_LAST_5,VALID_1_IN_LAST_8,PERSISTENCY_INDEFINITELY}]
+              [--stereo-post-processing-temporal-alpha STEREO_POST_PROCESSING_TEMPORAL_ALPHA]
+              [--stereo-post-processing-temporal-delta STEREO_POST_PROCESSING_TEMPORAL_DELTA]
+              [--stereo-post-processing-spatial-filter]
+              [--stereo-post-processing-spatial-enable STEREO_POST_PROCESSING_SPATIAL_ENABLE]
+              [--stereo-post-processing-spatial-hole-filling-radius STEREO_POST_PROCESSING_SPATIAL_HOLE_FILLING_RADIUS]
+              [--stereo-post-processing-spatial-alpha STEREO_POST_PROCESSING_SPATIAL_ALPHA]
+              [--stereo-post-processing-spatial-delta STEREO_POST_PROCESSING_SPATIAL_DELTA]
+              [--stereo-post-processing-spatial-num-iterations STEREO_POST_PROCESSING_SPATIAL_NUM_ITERATIONS]
+              [--stereo-post-processing-threshold-filter]
+              [--stereo-post-processing-threshold-min-range STEREO_POST_PROCESSING_THRESHOLD_MIN_RANGE]
+              [--stereo-post-processing-threshold-max-range STEREO_POST_PROCESSING_THRESHOLD_MAX_RANGE]
+              [--stereo-post-processing-decimation-filter]
+              [--stereo-post-processing-decimation-decimal-factor {1,2,3,4}]
+              [--stereo-post-processing-decimation-mode {PIXEL_SKIPPING,NON_ZERO_MEDIAN,NON_ZERO_MEAN}]
+
+options:
+  -h, --help            show this help message and exit
+  -u MQTT_USERNAME, --mqtt-username MQTT_USERNAME
+                        MQTT user
+  -p MQTT_PASSWORD, --mqtt-password MQTT_PASSWORD
+                        MQTT password
+  -b MQTT_BROKER_HOST, --mqtt-broker-host MQTT_BROKER_HOST
+                        MQTT broker host
+  -P MQTT_BROKER_PORT, --mqtt-broker-port MQTT_BROKER_PORT
+                        MQTT broker port
+  -C MQTT_CLIENT_ID, --mqtt-client-id MQTT_CLIENT_ID
+                        MQTT client id
+  -c MQTT_TOPIC_ROBOCAR_OAK_CAMERA, --mqtt-topic-robocar-oak-camera MQTT_TOPIC_ROBOCAR_OAK_CAMERA
+                        MQTT topic where to publish robocar-oak-camera frames
+  -o MQTT_TOPIC_ROBOCAR_OBJECTS, ---mqtt-topic-robocar-objects MQTT_TOPIC_ROBOCAR_OBJECTS
+                        MQTT topic where to publish objects detection results
+  -t OBJECTS_THRESHOLD, --objects-threshold OBJECTS_THRESHOLD
+                        threshold to filter detected objects
+  -d MQTT_TOPIC_ROBOCAR_DISPARITY, ---mqtt-topic-robocar-disparity MQTT_TOPIC_ROBOCAR_DISPARITY
+                        MQTT topic where to publish disparity results
+  -f CAMERA_FPS, --camera-fps CAMERA_FPS
+                        set rate at which camera should produce frames
+  --camera-tuning-exposition {default,500us,8300us}
+                        override camera exposition configuration
+  -H IMAGE_HEIGHT, --image-height IMAGE_HEIGHT
+                        image height
+  -W IMAGE_WIDTH, --image-width IMAGE_WIDTH
+                        image width
+  --log {info,debug}    Log level
+  --stereo-mode-lr-check
+                        remove incorrectly calculated disparity pixels due to
+                        occlusions at object borders
+  --stereo-mode-extended-disparity
+                        allows detecting closer distance objects for the given
+                        baseline. This increases the maximum disparity search
+                        from 96 to 191, meaning the range is now: [0..190]
+  --stereo-mode-subpixel
+                        iimproves the precision and is especially useful for
+                        long range measurements
+  --stereo-post-processing-median-filter
+                        enable post-processing median filter
+  --stereo-post-processing-median-value {MEDIAN_OFF,KERNEL_3x3,KERNEL_5x5,KERNEL_7x7}
+                        Median filter config
+  --stereo-post-processing-speckle-filter
+                        enable post-processing speckle filter
+  --stereo-post-processing-speckle-enable STEREO_POST_PROCESSING_SPECKLE_ENABLE
+                        enable post-processing speckle filter
+  --stereo-post-processing-speckle-range STEREO_POST_PROCESSING_SPECKLE_RANGE
+                        Speckle search range
+  --stereo-post-processing-temporal-filter
+                        enable post-processing temporal filter
+  --stereo-post-processing-temporal-persistency-mode {PERSISTENCY_OFF,VALID_8_OUT_OF_8,VALID_2_IN_LAST_3,VALID_2_IN_LAST_4,VALID_2_OUT_OF_8,VALID_1_IN_LAST_2,VALID_1_IN_LAST_5,VALID_1_IN_LAST_8,PERSISTENCY_INDEFINITELY}
+                        Persistency mode.
+  --stereo-post-processing-temporal-alpha STEREO_POST_PROCESSING_TEMPORAL_ALPHA
+                        The Alpha factor in an exponential moving average with
+                        Alpha=1 - no filter. Alpha = 0 - infinite filter.
+                        Determines the extent of the temporal history that
+                        should be averaged.
+  --stereo-post-processing-temporal-delta STEREO_POST_PROCESSING_TEMPORAL_DELTA
+                        Step-size boundary. Establishes the threshold used to
+                        preserve surfaces (edges). If the disparity value
+                        between neighboring pixels exceed the disparity
+                        threshold set by this delta parameter, then filtering
+                        will be temporarily disabled. Default value 0 means
+                        auto: 3 disparity integer levels. In case of subpixel
+                        mode it’s 3*number of subpixel levels.
+  --stereo-post-processing-spatial-filter
+                        enable post-processing spatial filter
+  --stereo-post-processing-spatial-enable STEREO_POST_PROCESSING_SPATIAL_ENABLE
+                        Whether to enable or disable the filter
+  --stereo-post-processing-spatial-hole-filling-radius STEREO_POST_PROCESSING_SPATIAL_HOLE_FILLING_RADIUS
+                        An in-place heuristic symmetric hole-filling mode
+                        applied horizontally during the filter passes
+  --stereo-post-processing-spatial-alpha STEREO_POST_PROCESSING_SPATIAL_ALPHA
+                        The Alpha factor in an exponential moving average with
+                        Alpha=1 - no filter. Alpha = 0 - infinite filter
+  --stereo-post-processing-spatial-delta STEREO_POST_PROCESSING_SPATIAL_DELTA
+                        Step-size boundary. Establishes the threshold used to
+                        preserve edges
+  --stereo-post-processing-spatial-num-iterations STEREO_POST_PROCESSING_SPATIAL_NUM_ITERATIONS
+                        Number of iterations over the image in both horizontal
+                        and vertical direction
+  --stereo-post-processing-threshold-filter
+                        enable post-processing threshold filter
+  --stereo-post-processing-threshold-min-range STEREO_POST_PROCESSING_THRESHOLD_MIN_RANGE
+                        Minimum range in depth units. Depth values under this
+                        value are invalidated
+  --stereo-post-processing-threshold-max-range STEREO_POST_PROCESSING_THRESHOLD_MAX_RANGE
+                        Maximum range in depth units. Depth values over this
+                        value are invalidated.
+  --stereo-post-processing-decimation-filter
+                        enable post-processing decimation filter
+  --stereo-post-processing-decimation-decimal-factor {1,2,3,4}
+                        Decimation factor
+  --stereo-post-processing-decimation-mode {PIXEL_SKIPPING,NON_ZERO_MEDIAN,NON_ZERO_MEAN}
+                        Decimation algorithm type
+
+```

camera/cli.py

@@ -5,12 +5,16 @@ import argparse
 import logging
 import os
 import signal
-import typing, types
+import types
+import typing
+from typing import List
 
 import depthai as dai
 import paho.mqtt.client as mqtt
 
 from camera import oak_pipeline as cam
+from camera.oak_pipeline import StereoDepthPostFilter, MedianFilter, SpeckleFilter, TemporalFilter, SpatialFilter, \
+    ThresholdFilter, DecimationFilter
 
 CAMERA_EXPOSITION_DEFAULT = "default"
 CAMERA_EXPOSITION_8300US = "8300us"
@@ -49,6 +53,9 @@ def _parse_args_cli() -> argparse.Namespace:
                         help="threshold to filter detected objects",
                         type=float,
                         default=_get_env_float_value("OBJECTS_THRESHOLD", 0.2))
+    parser.add_argument("-d", "---mqtt-topic-robocar-disparity",
+                        help="MQTT topic where to publish disparity results",
+                        default=_get_env_value("MQTT_TOPIC_DISPARITY", "/disparity"))
     parser.add_argument("-f", "--camera-fps",
                         help="set rate at which camera should produce frames",
                         type=int,
@@ -67,6 +74,126 @@ def _parse_args_cli() -> argparse.Namespace:
                         type=str,
                         default="info",
                         choices=["info", "debug"])
+
+    parser.add_argument("--disable-disparity", action="store_true",
+                    help="enable disparity frame",
+                    default=False
+                    )
+    parser.add_argument("--stereo-mode-lr-check",
+                        help="remove incorrectly calculated disparity pixels due to occlusions at object borders",
+                        default=False, action="store_true"
+                        )
+    parser.add_argument("--stereo-mode-extended-disparity",
+                        help="allows detecting closer distance objects for the given baseline. This increases the maximum disparity search from 96 to 191, meaning the range is now: [0..190]",
+                        default=False, action="store_true"
+                        )
+    parser.add_argument("--stereo-mode-subpixel",
+                        help="iimproves the precision and is especially useful for long range measurements",
+                        default=False, action="store_true"
+                        )
+
+
+    parser.add_argument("--stereo-post-processing-median-filter",
+                        help="enable post-processing median filter",
+                        default=False, action="store_true"
+                        )
+    parser.add_argument("--stereo-post-processing-median-value",
+                        help="Median filter config ",
+                        type=str,
+                        choices=["MEDIAN_OFF", "KERNEL_3x3", "KERNEL_5x5", "KERNEL_7x7"],
+                        default="KERNEL_7x7",
+                        )
+    parser.add_argument("--stereo-post-processing-speckle-filter",
+                        help="enable post-processing speckle filter",
+                        default=False, action="store_true"
+                        )
+    parser.add_argument("--stereo-post-processing-speckle-enable",
+                        help="enable post-processing speckle filter",
+                        type=bool, default=False
+                        )
+    parser.add_argument("--stereo-post-processing-speckle-range",
+                        help="Speckle search range",
+                        type=int, default=50
+                        )
+
+    parser.add_argument("--stereo-post-processing-temporal-filter",
+                        help="enable post-processing temporal filter",
+                        default=False, action="store_true"
+                        )
+    parser.add_argument("--stereo-post-processing-temporal-persistency-mode",
+                        help="Persistency mode.",
+                        type=str, default="VALID_2_IN_LAST_4",
+                        choices=["PERSISTENCY_OFF", "VALID_8_OUT_OF_8", "VALID_2_IN_LAST_3", "VALID_2_IN_LAST_4",
+                                 "VALID_2_OUT_OF_8", "VALID_1_IN_LAST_2", "VALID_1_IN_LAST_5", "VALID_1_IN_LAST_8",
+                                 "PERSISTENCY_INDEFINITELY"]
+                        )
+    parser.add_argument("--stereo-post-processing-temporal-alpha",
+                        help="The Alpha factor in an exponential moving average with Alpha=1 - no filter. "
+                             "Alpha = 0 - infinite filter. Determines the extent of the temporal history that should be "
+                             "averaged. ",
+                        type=float, default=0.4,
+                        )
+    parser.add_argument("--stereo-post-processing-temporal-delta",
+                        help="Step-size boundary. Establishes the threshold used to preserve surfaces (edges). "
+                             "If the disparity value between neighboring pixels exceed the disparity threshold set by "
+                             "this delta parameter, then filtering will be temporarily disabled. Default value 0 means "
+                             "auto: 3 disparity integer levels. In case of subpixel mode it’s 3*number of subpixel "
+                             "levels.",
+                        type=int, default=0,
+                        )
+
+    parser.add_argument("--stereo-post-processing-spatial-filter",
+                        help="enable post-processing spatial filter",
+                        default=False, action="store_true"
+                        )
+    parser.add_argument("--stereo-post-processing-spatial-enable",
+                        help="Whether to enable or disable the filter",
+                        type=bool, default=False,
+                        )
+    parser.add_argument("--stereo-post-processing-spatial-hole-filling-radius",
+                        help="An in-place heuristic symmetric hole-filling mode applied horizontally during the filter passes",
+                        type=int, default=2,
+                        )
+    parser.add_argument("--stereo-post-processing-spatial-alpha",
+                        help="The Alpha factor in an exponential moving average with Alpha=1 - no filter. Alpha = 0 - infinite filter",
+                        type=float, default=0.5,
+                        )
+    parser.add_argument("--stereo-post-processing-spatial-delta",
+                        help="Step-size boundary. Establishes the threshold used to preserve edges",
+                        type=int, default=0,
+                        )
+    parser.add_argument("--stereo-post-processing-spatial-num-iterations",
+                        help="Number of iterations over the image in both horizontal and vertical direction",
+                        type=int, default=1,
+                        )
+
+    parser.add_argument("--stereo-post-processing-threshold-filter",
+                        help="enable post-processing threshold filter",
+                        default=False, action="store_true"
+                        )
+    parser.add_argument("--stereo-post-processing-threshold-min-range",
+                        help="Minimum range in depth units. Depth values under this value are invalidated",
+                        type=int, default=500,
+                        )
+    parser.add_argument("--stereo-post-processing-threshold-max-range",
+                        help="Maximum range in depth units. Depth values over this value are invalidated.",
+                        type=int, default=15000,
+                        )
+
+    parser.add_argument("--stereo-post-processing-decimation-filter",
+                        help="enable post-processing decimation filter",
+                        default=False, action="store_true"
+                        )
+    parser.add_argument("--stereo-post-processing-decimation-decimal-factor",
+                        help="Decimation factor",
+                        type=int, default=1, choices=[1, 2, 3, 4]
+                        )
+    parser.add_argument("--stereo-post-processing-decimation-mode",
+                        help="Decimation algorithm type",
+                        type=str, default="PIXEL_SKIPPING",
+                        choices=["PIXEL_SKIPPING", "NON_ZERO_MEDIAN", "NON_ZERO_MEAN"]
+                        )
+
     args = parser.parse_args()
     return args
 
@@ -104,6 +231,16 @@ def execute_from_command_line() -> None:
     object_processor = cam.ObjectProcessor(mqtt_client=client,
                                            objects_topic=args.mqtt_topic_robocar_objects,
                                            objects_threshold=args.objects_threshold)
+    if args.disable_disparity == False:
+        depth_source = cam.DepthSource(pipeline=pipeline,
+                                       extended_disparity=args.stereo_mode_extended_disparity,
+                                       subpixel=args.stereo_mode_subpixel,
+                                       lr_check=args.stereo_mode_lr_check,
+                                       stereo_filters=stereo_filters),
+        disparity_processor = cam.DisparityProcessor(mqtt_client=client, disparity_topic=args.mqtt_topic_robocar_disparity)
+    else:
+        disparity_processor = None
+        depth_source = None
 
     pipeline = dai.Pipeline()
     if args.camera_tuning_exposition == CAMERA_EXPOSITION_500US:
@@ -111,6 +248,7 @@ def execute_from_command_line() -> None:
     elif args.camera_tuning_exposition == CAMERA_EXPOSITION_8300US:
         pipeline.setCameraTuningBlobPath('/camera_tuning/tuning_exp_limit_8300us.bin')
 
+    stereo_filters = _get_stereo_filters(args)
 
     pipeline_controller = cam.PipelineController(pipeline=pipeline,
                                                  frame_processor=frame_processor,
@@ -120,7 +258,9 @@ def execute_from_command_line() -> None:
                                                                          img_width=args.image_width,
                                                                          img_height=args.image_height,
                                                                          fps=args.camera_fps,
-                                                                         ))
+                                                                         ),
+                                                 depth_source=depth_source,
+                                                 disparity_processor=disparity_processor)
 
     def sigterm_handler(signum: int, frame: typing.Optional[
         types.FrameType]) -> None:  # pylint: disable=unused-argument  # need to implement handler signature
@@ -145,3 +285,89 @@ def _get_env_int_value(env_var: str, default_value: int) -> int:
 def _get_env_float_value(env_var: str, default_value: float) -> float:
     value = _get_env_value(env_var, str(default_value))
     return float(value)
+
+
+def _get_stereo_filters(args: argparse.Namespace) -> List[StereoDepthPostFilter]:
+    filters = []
+
+    if args.stereo_post_processing_median_filter:
+        if args.stereo_post_processing_median_value == "MEDIAN_OFF":
+            value = dai.MedianFilter.MEDIAN_OFF
+        elif args.stereo_post_processing_median_value == "KERNEL_3x3":
+            value = dai.MedianFilter.KERNEL_3x3
+        elif args.stereo_post_processing_median_value == "KERNEL_5x5":
+            value = dai.MedianFilter.KERNEL_5x5
+        elif args.stereo_post_processing_median_value == "KERNEL_7x7":
+            value = dai.MedianFilter.KERNEL_7x7
+        else:
+            value = dai.MedianFilter.KERNEL_7x7
+
+        filters.append(MedianFilter(value=value))
+
+    if args.stereo_post_processing_speckle_filter:
+        filters.append(SpeckleFilter(enable=args.stereo_post_processing_speckle_enable,
+                                     speckle_range=args.stereo_post_processing_speckle_range))
+
+    if args.stereo_post_processing_temporal_filter:
+        if args.stereo_post_processing_temporal_persistency-mode == "PERSISTENCY_OFF":
+            mode=dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.PERSISTENCY_OFF
+        elif args.stereo_post_processing_temporal_persistency-mode == "VALID_8_OUT_OF_8":
+            mode=dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_8_OUT_OF_8
+        elif args.stereo_post_processing_temporal_persistency-mode == "VALID_2_IN_LAST_3":
+            mode=dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_IN_LAST_3
+        elif args.stereo_post_processing_temporal_persistency-mode == "VALID_2_IN_LAST_4":
+            mode=dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_IN_LAST_4
+        elif args.stereo_post_processing_temporal_persistency-mode == "VALID_2_OUT_OF_8":
+            mode=dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_OUT_OF_8
+        elif args.stereo_post_processing_temporal_persistency-mode == "VALID_1_IN_LAST_2":
+            mode=dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_2
+        elif args.stereo_post_processing_temporal_persistency-mode == "VALID_1_IN_LAST_5":
+            mode=dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_5
+        elif args.stereo_post_processing_temporal_persistency-mode == "VALID_1_IN_LAST_8":
+            mode=dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_1_IN_LAST_8
+        elif args.stereo_post_processing_temporal_persistency-mode == "PERSISTENCY_INDEFINITELY":
+            mode=dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.PERSISTENCY_INDEFINITELY
+        else:
+            mode=dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_IN_LAST_4
+
+        filters.append(TemporalFilter(
+            enable=args.stereo_post_processing_temporal_enable,
+            persistencyMode=mode,
+            alpha=args.stereo_post_processing_temporal_alpha,
+            delta=args.stereo_post_processing_temporal_delta
+        ))
+
+        if args.stereo_post_processing_spatial_filter:
+            filters.append(SpatialFilter(enable=args.stereo_post_processing_spatial_enable,
+                                         hole_filling_radius=args.stereo_post_processing_spatial_hole_filling_radius,
+                                         alpha=args.stereo_post_processing_spatial_alpha,
+                                         delta=args.stereo_post_processing_spatial_delta,
+                                         num_iterations=args.stereo_post_processing_spatial_num_iterations,
+                                         ))
+
+        if args.stereo_post_processing_threshold_filter:
+            filters.append(ThresholdFilter(
+                min_range=args.stereo_post_processing_threshold_min_range,
+                max_range=args.stereo_post_processing_threshold_max_range,
+            ))
+
+        if args.stereo_post_processing_decimation_filter:
+
+            if args.stereo_post_processing_decimation_mode == "PIXEL_SKIPPING":
+                mode=dai.RawStereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.PIXEL_SKIPPING
+            if args.stereo_post_processing_decimation_mode == "NON_ZERO_MEDIAN":
+                mode=dai.RawStereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.NON_ZERO_MEDIAN
+            if args.stereo_post_processing_decimation_mode == "NON_ZERO_MEAN":
+                mode=dai.RawStereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.NON_ZERO_MEAN
+            else:
+                mode=dai.RawStereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.PIXEL_SKIPPING
+
+            filters.append(DecimationFilter(
+                decimation_factor=args.stereo_post_processing_decimation_decimal_factor,
+                mode=mode
+            ))
+
+    return filters
+
+if __name__ == '__main__':
+    execute_from_command_line()

camera/oak_pipeline.py

@@ -6,8 +6,8 @@ import datetime
 import logging
 import pathlib
 import time
-import typing
 from dataclasses import dataclass
+from typing import List, Any
 
 import cv2
 import depthai as dai
@@ -26,6 +26,8 @@ _NN_HEIGHT = 192
 _PREVIEW_WIDTH = 640
 _PREVIEW_HEIGHT = 480
 
+_CAMERA_BASELINE_IN_MM = 75
+
 
 class ObjectProcessor:
     """
@@ -95,7 +97,7 @@ class FrameProcessor:
         self._mqtt_client = mqtt_client
         self._frame_topic = frame_topic
 
-    def process(self, img: dai.ImgFrame) -> typing.Any:
+    def process(self, img: dai.ImgFrame) -> Any:
         """
         Publish camera frames
         :param img: image read from camera
@@ -124,6 +126,37 @@ class FrameProcessor:
         return frame_msg.id
 
 
+class DisparityProcessor:
+    """
+       Processor for camera frames
+       """
+
+    def __init__(self, mqtt_client: mqtt.Client, disparity_topic: str):
+        self._mqtt_client = mqtt_client
+        self._disparity_topic = disparity_topic
+
+    def process(self, img: dai.ImgFrame, frame_ref: evt.FrameRef, focal_length_in_pixels: float,
+                baseline_mm: float = _CAMERA_BASELINE_IN_MM) -> None:
+        im_frame = img.getCvFrame()
+        is_success, im_buf_arr = cv2.imencode(".jpg", im_frame)
+        if not is_success:
+            raise FrameProcessError("unable to process to encode frame to jpg")
+        byte_im = im_buf_arr.tobytes()
+
+        disparity_msg = evt.DisparityMessage()
+        disparity_msg.disparity = byte_im
+        disparity_msg.frame_ref.name = frame_ref.name
+        disparity_msg.frame_ref.id = frame_ref.id
+        disparity_msg.frame_ref.created_at.FromDatetime(frame_ref.created_at.ToDatetime())
+        disparity_msg.focal_length_in_pixels = focal_length_in_pixels
+        disparity_msg.baseline_in_mm = baseline_mm
+
+        self._mqtt_client.publish(topic=self._disparity_topic,
+                                  payload=disparity_msg.SerializeToString(),
+                                  qos=0,
+                                  retain=False)
+
+
 class Source(abc.ABC):
     """Base class for image source"""
 
@@ -233,6 +266,210 @@ class CameraSource(Source):
         return manip
 
 
+class StereoDepthPostFilter(abc.ABC):
+    @abc.abstractmethod
+    def apply(self, config: dai.RawStereoDepthConfig) -> None:
+        pass
+
+
+class MedianFilter(StereoDepthPostFilter):
+    """
+    This is a non-edge preserving Median filter, which can be used to reduce noise and smoothen the depth map.
+    Median filter is implemented in hardware, so it’s the fastest filter.
+    """
+    def __init__(self, value: dai.MedianFilter = dai.MedianFilter.KERNEL_7x7) -> None:
+        self._value = value
+
+    def apply(self, config: dai.RawStereoDepthConfig) -> None:
+        config.postProcessing.median.value = self._value
+
+
+class SpeckleFilter(StereoDepthPostFilter):
+    """
+    Speckle Filter is used to reduce the speckle noise. Speckle noise is a region with huge variance between
+    neighboring disparity/depth pixels, and speckle filter tries to filter this region.
+    """
+    def __init__(self, enable: bool = True, speckle_range: int = 50) -> None:
+        """
+        :param enable: Whether to enable or disable the filter.
+        :param speckle_range: Speckle search range.
+        """
+        self._enable = enable
+        self._speckle_range = speckle_range
+
+    def apply(self, config: dai.RawStereoDepthConfig) -> None:
+        config.postProcessing.speckleFilter.enable = self._enable
+        config.postProcessing.speckleFilter.speckleRange = self._speckle_range
+
+
+class TemporalFilter(StereoDepthPostFilter):
+    """
+    Temporal Filter is intended to improve the depth data persistency by manipulating per-pixel values based on
+    previous frames. The filter performs a single pass on the data, adjusting the depth values while also updating the
+    tracking history. In cases where the pixel data is missing or invalid, the filter uses a user-defined persistency
+    mode to decide whether the missing value should be rectified with stored data. Note that due to its reliance on
+    historic data the filter may introduce visible blurring/smearing artifacts, and therefore is best-suited for
+    static scenes.
+    """
+    def __init__(self,
+                 enable: bool = True,
+                 persistencyMode: dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode=dai.RawStereoDepthConfig.PostProcessing.TemporalFilter.PersistencyMode.VALID_2_IN_LAST_4,
+                 alpha: float = 0.4,
+                 delta: int = 0):
+        """
+        :param enable: Whether to enable or disable the filter.
+        :param persistencyMode: Persistency mode. If the current disparity/depth value is invalid, it will be replaced
+        by an older value, based on persistency mode.
+        :param alpha: The Alpha factor in an exponential moving average with Alpha=1 - no filter.
+        Alpha = 0 - infinite filter. Determines the extent of the temporal history that should be averaged.
+        :param delta: Step-size boundary. Establishes the threshold used to preserve surfaces (edges).
+        If the disparity value between neighboring pixels exceed the disparity threshold set by this delta parameter,
+        then filtering will be temporarily disabled. Default value 0 means auto: 3 disparity integer levels.
+        In case of subpixel mode it’s 3*number of subpixel levels.
+        """
+        self._enable = enable
+        self._persistencyMode = persistencyMode
+        self._alpha = alpha
+        self._delta = delta
+
+    def apply(self, config: dai.RawStereoDepthConfig) -> None:
+        config.postProcessing.temporalFilter.enable = self._enable
+        config.postProcessing.temporalFilter.persistencyMode = self._persistencyMode
+        config.postProcessing.temporalFilter.alpha = self._alpha
+        config.postProcessing.temporalFilter.delta = self._delta
+
+
+class SpatialFilter(StereoDepthPostFilter):
+    """
+    Spatial Edge-Preserving Filter will fill invalid depth pixels with valid neighboring depth pixels. It performs a
+    series of 1D horizontal and vertical passes or iterations, to enhance the smoothness of the reconstructed data.
+    """
+    def __init__(self,
+                 enable: bool = True,
+                 hole_filling_radius: int = 2,
+                 alpha: float = 0.5,
+                 delta: int = 0,
+                 num_iterations: int = 1):
+        """
+        :param enable: Whether to enable or disable the filter.
+        :param hole_filling_radius: An in-place heuristic symmetric hole-filling mode applied horizontally during
+        the filter passes. Intended to rectify minor artefacts with minimal performance impact. Search radius for
+        hole filling.
+        :param alpha: The Alpha factor in an exponential moving average with Alpha=1 - no filter.
+        Alpha = 0 - infinite filter. Determines the amount of smoothing.
+        :param delta: Step-size boundary. Establishes the threshold used to preserve “edges”. If the disparity value
+        between neighboring pixels exceed the disparity threshold set by this delta parameter, then filtering will be
+        temporarily disabled. Default value 0 means auto: 3 disparity integer levels. In case of subpixel mode it’s
+        3*number of subpixel levels.
+        :param num_iterations: Number of iterations over the image in both horizontal and vertical direction.
+        """
+        self._enable = enable
+        self._hole_filling_radius = hole_filling_radius
+        self._alpha = alpha
+        self._delta = delta
+        self._num_iterations = num_iterations
+
+    def apply(self, config: dai.RawStereoDepthConfig) -> None:
+        config.postProcessing.spatialFilter.enable = self._enable
+        config.postProcessing.spatialFilter.holeFillingRadius = self._hole_filling_radius
+        config.postProcessing.spatialFilter.alpha = self._alpha
+        config.postProcessing.spatialFilter.delta = self._delta
+        config.postProcessing.spatialFilter.numIterations = self._num_iterations
+
+
+class ThresholdFilter(StereoDepthPostFilter):
+    """
+    Threshold Filter filters out all disparity/depth pixels outside the configured min/max threshold values.
+    """
+    def __init__(self, min_range: int = 400, max_range: int = 15000):
+        """
+        :param min_range: Minimum range in depth units. Depth values under this value are invalidated.
+        :param max_range: Maximum range in depth units. Depth values over this value are invalidated.
+        """
+        self._min_range = min_range
+        self._max_range = max_range
+
+    def apply(self, config: dai.RawStereoDepthConfig) -> None:
+        config.postProcessing.thresholdFilter.minRange = self._min_range
+        config.postProcessing.thresholdFilter.maxRange = self._max_range
+
+
+class DecimationFilter(StereoDepthPostFilter):
+    """
+    Decimation Filter will sub-samples the depth map, which means it reduces the depth scene complexity and allows
+    other filters to run faster. Setting decimationFactor to 2 will downscale 1280x800 depth map to 640x400.
+    """
+    def __init__(self,
+                 decimation_factor: int = 1,
+                 decimation_mode: dai.RawStereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode = dai.RawStereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.PIXEL_SKIPPING
+                 ):
+        """
+        :param decimation_factor: Decimation factor. Valid values are 1,2,3,4. Disparity/depth map x/y resolution will
+        be decimated with this value.
+        :param decimation_mode: Decimation algorithm type.
+        """
+        self._decimation_factor = decimation_factor
+        self._mode = decimation_mode
+
+    def apply(self, config: dai.RawStereoDepthConfig) -> None:
+        config.postProcessing.decimationFilter.decimationFactor = self._decimation_factor
+        config.postProcessing.decimationFilter.decimationMode = self._mode
+
+
+class DepthSource(Source):
+    def __init__(self, pipeline: dai.Pipeline,
+                 extended_disparity: bool = False,
+                 subpixel: bool = False,
+                 lr_check: bool = True,
+                 stereo_filters: List[StereoDepthPostFilter] = []
+                 ) -> None:
+        """
+        # 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 = True
+        """
+        self._monoLeft = pipeline.create(dai.node.MonoCamera)
+        self._monoRight = pipeline.create(dai.node.MonoCamera)
+        self._depth = pipeline.create(dai.node.StereoDepth)
+        self._xout_disparity = pipeline.create(dai.node.XLinkOut)
+
+        self._xout_disparity.setStreamName("disparity")
+
+        # Properties
+        self._monoLeft.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
+        self._monoLeft.setCamera("left")
+        self._monoLeft.out.link(self._depth.left)
+        self._monoRight.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
+        self._monoRight.setCamera("right")
+        self._monoRight.out.link(self._depth.right)
+
+        # Create a node that will produce the depth map
+        # (using disparity output as it's easier to visualize depth this way)
+        self._depth.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
+        # Options: MEDIAN_OFF, KERNEL_3x3, KERNEL_5x5, KERNEL_7x7 (default)
+        self._depth.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7)
+        self._depth.setLeftRightCheck(lr_check)
+        self._depth.setExtendedDisparity(extended_disparity)
+        self._depth.setSubpixel(subpixel)
+        self._depth.disparity.link(self._xout_disparity.input)
+
+        if len(stereo_filters) > 0:
+            # Configure post-processing filters
+            config = self._depth.initialConfig.get()
+            for filter in stereo_filters:
+                filter.apply(config)
+            self._depth.initialConfig.set(config)
+
+    def get_stream_name(self) -> str:
+        return self._xout_disparity.getStreamName()
+
+    def link(self, input_node: dai.Node.Input) -> None:
+        self._depth.disparity.link(input_node)
+
+
 @dataclass
 class MqttConfig:
     """MQTT configuration"""
@@ -272,8 +509,8 @@ class MqttSource(Source):
 
     @staticmethod
     # pylint: disable=unused-argument
-    def _on_connect(client: mqtt.Client, userdata: MqttConfig, flags: typing.Any,
-                    result_connection: typing.Any) -> None:
+    def _on_connect(client: mqtt.Client, userdata: MqttConfig, flags: Any,
+                    result_connection: Any) -> None:
         # if we lose the connection and reconnect then subscriptions will be renewed.
         client.subscribe(topic=userdata.topic, qos=userdata.qos)
 
@@ -305,15 +542,19 @@ class PipelineController:
     """
 
     def __init__(self, frame_processor: FrameProcessor,
-                 object_processor: ObjectProcessor, camera: Source, object_node: ObjectDetectionNN,
+                 object_processor: ObjectProcessor, disparity_processor: DisparityProcessor,
+                 camera: Source, depth_source: Source, object_node: ObjectDetectionNN,
                  pipeline: dai.Pipeline):
         self._frame_processor = frame_processor
         self._object_processor = object_processor
+        self._disparity_processor = disparity_processor
         self._camera = camera
+        self._depth_source = depth_source
         self._object_node = object_node
         self._stop = False
         self._pipeline = pipeline
         self._configure_pipeline()
+        self._focal_length_in_pixels: float | None = None
 
     def _configure_pipeline(self) -> None:
         logger.info("configure pipeline")
@@ -337,6 +578,11 @@ class PipelineController:
             logger.info('Connected cameras: %s', str(dev.getConnectedCameras()))
             logger.info("output queues found: %s", str(''.join(dev.getOutputQueueNames())))  # type: ignore
 
+            calib_data = dev.readCalibration()
+            intrinsics = calib_data.getCameraIntrinsics(dai.CameraBoardSocket.CAM_C)
+            self._focal_length_in_pixels = intrinsics[0][0]
+            logger.info('Right mono camera focal length in pixels: %s', self._focal_length_in_pixels)
+
             dev.startPipeline()
             # Queues
             queue_size = 4
@@ -344,6 +590,11 @@ class PipelineController:
                                        blocking=False)
             q_nn = dev.getOutputQueue(name=self._object_node.get_stream_name(), maxSize=queue_size,  # type: ignore
                                       blocking=False)
+            if self._disparity_processor is not None:
+                q_disparity = dev.getOutputQueue(name=self._depth_source.get_stream_name(), maxSize=queue_size,  # type: ignore
+                                                 blocking=False)
+            else:
+                q_disparity = None
 
             start_time = time.time()
             counter = 0
@@ -355,7 +606,7 @@ class PipelineController:
                     logger.info("stop loop event")
                     return
                 try:
-                    self._loop_on_camera_events(q_nn, q_rgb)
+                    self._loop_on_camera_events(q_nn, q_rgb, q_disparity)
                 # pylint: disable=broad-except # bad frame or event must not stop loop
                 except Exception as ex:
                     logger.exception("unexpected error: %s", str(ex))
@@ -369,8 +620,7 @@ class PipelineController:
                     display_time = time.time()
                     logger.info("fps: %s", fps)
 
-
-    def _loop_on_camera_events(self, q_nn: dai.DataOutputQueue, q_rgb: dai.DataOutputQueue) -> None:
+    def _loop_on_camera_events(self, q_nn: dai.DataOutputQueue, q_rgb: dai.DataOutputQueue, q_disparity: dai.DataOutputQueue) -> None:
         logger.debug("wait for new frame")
 
         # Wait for frame
@@ -390,6 +640,12 @@ class PipelineController:
         self._object_processor.process(in_nn, frame_ref)
         logger.debug("objects processed")
 
+        logger.debug("process disparity")
+        if self._disparity_processor is not None:
+            in_disparity: dai.ImgFrame = q_disparity.get()  # type: ignore
+            self._disparity_processor.process(in_disparity, frame_ref=frame_ref,
+                                              focal_length_in_pixels=self._focal_length_in_pixels)
+        logger.debug("disparity processed")
 
     def stop(self) -> None:
         """

camera/tests/test_depthai.py

@@ -10,7 +10,7 @@ import paho.mqtt.client as mqtt
 import pytest
 import pytest_mock
 
-import camera.depthai
+from camera.oak_pipeline import DisparityProcessor, ObjectProcessor, FrameProcessor, FrameProcessError
 
 Object = dict[str, float]
 
@@ -76,16 +76,16 @@ class TestObjectProcessor:
         return m
 
     @pytest.fixture
-    def object_processor(self, mqtt_client: mqtt.Client) -> camera.depthai.ObjectProcessor:
-        return camera.depthai.ObjectProcessor(mqtt_client, "topic/object", 0.2)
+    def object_processor(self, mqtt_client: mqtt.Client) -> ObjectProcessor:
+        return ObjectProcessor(mqtt_client, "topic/object", 0.2)
 
-    def test_process_without_object(self, object_processor: camera.depthai.ObjectProcessor, mqtt_client: mqtt.Client,
+    def test_process_without_object(self, object_processor: ObjectProcessor, mqtt_client: mqtt.Client,
                                     raw_objects_empty: dai.NNData, frame_ref: events.events_pb2.FrameRef) -> None:
         object_processor.process(raw_objects_empty, frame_ref)
         publish_mock: unittest.mock.MagicMock = mqtt_client.publish  # type: ignore
         publish_mock.assert_not_called()
 
-    def test_process_with_object_with_low_score(self, object_processor: camera.depthai.ObjectProcessor,
+    def test_process_with_object_with_low_score(self, object_processor: ObjectProcessor,
                                                 mqtt_client: mqtt.Client, raw_objects_one: dai.NNData,
                                                 frame_ref: events.events_pb2.FrameRef) -> None:
         object_processor._objects_threshold = 0.9
@@ -94,7 +94,7 @@ class TestObjectProcessor:
         publish_mock.assert_not_called()
 
     def test_process_with_one_object(self,
-                                     object_processor: camera.depthai.ObjectProcessor, mqtt_client: mqtt.Client,
+                                     object_processor: ObjectProcessor, mqtt_client: mqtt.Client,
                                      raw_objects_one: dai.NNData, frame_ref: events.events_pb2.FrameRef,
                                      object1: Object) -> None:
         object_processor.process(raw_objects_one, frame_ref)
@@ -120,10 +120,10 @@ class TestObjectProcessor:
 
 class TestFrameProcessor:
     @pytest.fixture
-    def frame_processor(self, mqtt_client: mqtt.Client) -> camera.depthai.FrameProcessor:
-        return camera.depthai.FrameProcessor(mqtt_client, "topic/frame")
+    def frame_processor(self, mqtt_client: mqtt.Client) -> FrameProcessor:
+        return FrameProcessor(mqtt_client, "topic/frame")
 
-    def test_process(self, frame_processor: camera.depthai.FrameProcessor, mocker: pytest_mock.MockerFixture,
+    def test_process(self, frame_processor: FrameProcessor, mocker: pytest_mock.MockerFixture,
                      mqtt_client: mqtt.Client) -> None:
         img: dai.ImgFrame = mocker.MagicMock()
         mocker.patch(target="cv2.imencode").return_value = (True, np.array(b"img content"))
@@ -145,12 +145,57 @@ class TestFrameProcessor:
         assert now - datetime.timedelta(
             milliseconds=10) < frame_msg.id.created_at.ToDatetime() < now + datetime.timedelta(milliseconds=10)
 
-    def test_process_error(self, frame_processor: camera.depthai.FrameProcessor, mocker: pytest_mock.MockerFixture,
+    def test_process_error(self, frame_processor: FrameProcessor, mocker: pytest_mock.MockerFixture,
                            mqtt_client: mqtt.Client) -> None:
         img: dai.ImgFrame = mocker.MagicMock()
         mocker.patch(target="cv2.imencode").return_value = (False, None)
 
-        with pytest.raises(camera.depthai.FrameProcessError) as ex:
+        with pytest.raises(FrameProcessError) as ex:
             _ = frame_processor.process(img)
         exception_raised = ex.value
         assert exception_raised.message == "unable to process to encode frame to jpg"
+
+
+class TestDisparityProcessor:
+    @pytest.fixture
+    def frame_ref(self) -> events.events_pb2.FrameRef:
+        now = datetime.datetime.now()
+        frame_msg = events.events_pb2.FrameMessage()
+        frame_msg.id.name = "robocar-oak-camera-oak"
+        frame_msg.id.id = str(int(now.timestamp() * 1000))
+        frame_msg.id.created_at.FromDatetime(now)
+        return frame_msg.id
+
+    @pytest.fixture
+    def disparity_processor(self, mqtt_client: mqtt.Client) -> DisparityProcessor:
+        return DisparityProcessor(mqtt_client, "topic/disparity")
+
+    def test_process(self, disparity_processor: DisparityProcessor, mocker: pytest_mock.MockerFixture,
+                     frame_ref: events.events_pb2.FrameRef,
+                     mqtt_client: mqtt.Client) -> None:
+        img: dai.ImgFrame = mocker.MagicMock()
+        mocker.patch(target="cv2.imencode").return_value = (True, np.array(b"img content"))
+
+        disparity_processor.process(img, frame_ref, 42)
+
+        pub_mock: unittest.mock.MagicMock = mqtt_client.publish  # type: ignore
+        pub_mock.assert_called_once_with(payload=unittest.mock.ANY, qos=0, retain=False, topic="topic/disparity")
+        payload = pub_mock.call_args.kwargs['payload']
+        disparity_msg = events.events_pb2.DisparityMessage()
+        disparity_msg.ParseFromString(payload)
+
+        assert disparity_msg.frame_ref == frame_ref
+        assert disparity_msg.disparity == b"img content"
+        assert disparity_msg.focal_length_in_pixels == 42
+        assert disparity_msg.baseline_in_mm == 75
+
+    def test_process_error(self, disparity_processor: DisparityProcessor, mocker: pytest_mock.MockerFixture,
+                           frame_ref: events.events_pb2.FrameRef,
+                           mqtt_client: mqtt.Client) -> None:
+        img: dai.ImgFrame = mocker.MagicMock()
+        mocker.patch(target="cv2.imencode").return_value = (False, None)
+
+        with pytest.raises(FrameProcessError) as ex:
+            disparity_processor.process(img, frame_ref, 42)
+        exception_raised = ex.value
+        assert exception_raised.message == "unable to process to encode frame to jpg"

pyproject.toml

@@ -9,14 +9,14 @@ packages = [
 ]
 
 [tool.poetry.dependencies]
-python = "^3.11"
-paho-mqtt = "^1.6.1"
-depthai = "^2.22.0"
+python = "^3.12"
+paho-mqtt = "^1.6"
+depthai = "^2"
 protobuf3 = "^0.2.1"
 google = "^3.0.0"
-protobuf = "^4.21.8"
-opencv-python-headless = "^4.6.0.66"
-robocar-protobuf = {version = "^1.3.0", source = "robocar"}
+protobuf = "^4.21"
+opencv-python-headless = "^4.6.0"
+robocar-protobuf = {version = "^1.6", source = "robocar"}
 
 
 [tool.poetry.group.test.dependencies]