robocars/robocar-oak-camera
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat(disparity): fine tuning mode and post-processing filters

Browse Source
This commit is contained in:
Cyrille Nofficial
2024-01-26 12:06:01 +01:00
parent 1ee37f65af
commit 33791d0014
3 changed files with 508 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

219
camera/cli.py
View File

@@ -5,13 +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 oak_pipeline import DisparityProcessor
from camera.oak_pipeline import StereoDepthPostFilter, MedianFilter, SpeckleFilter, TemporalFilter, SpatialFilter, \
ThresholdFilter, DecimationFilter
CAMERA_EXPOSITION_DEFAULT = "default"
CAMERA_EXPOSITION_8300US = "8300us"
@@ -50,7 +53,7 @@ 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("-o", "---mqtt-topic-robocar-disparity",
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",
@@ -71,6 +74,123 @@ def _parse_args_cli() -> argparse.Namespace:
type=str,
default="info",
choices=["info", "debug"])
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
@@ -116,6 +236,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,
@@ -126,7 +247,11 @@ def execute_from_command_line() -> None:
img_height=args.image_height,
fps=args.camera_fps,
),
depth_source=cam.DepthSource(pipeline=pipeline),
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),
disparity_processor=disparity_processor)
def sigterm_handler(signum: int, frame: typing.Optional[
@@ -152,3 +277,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()

161
camera/oak_pipeline.py
View File

@@ -266,11 +266,163 @@ 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
lr_check: bool = True,
*filters: StereoDepthPostFilter
) -> None:
"""
# Closer-in minimum depth, disparity range is doubled (from 95 to 190):
@@ -302,6 +454,13 @@ class DepthSource(Source):
self._depth.setExtendedDisparity(extended_disparity)
self._depth.setSubpixel(subpixel)
if len(filters) > 0:
# Configure post-processing filters
config = self._depth.initialConfig.get()
for filter in filters:
filter.apply(config)
self._depth.initialConfig.set(config)
def get_stream_name(self) -> str:
return self._xout_disparity.getStreamName()