Compare commits
No commits in common. "b2f2570968ac1cfa0fff2598b9d1028bf3dbbe40" and "552f69e46e38b1276915ac4d7b7dd6d49a219d14" have entirely different histories.
b2f2570968
...
552f69e46e
@ -1,10 +1,10 @@
|
|||||||
FROM docker.io/library/python:3.12-slim as base
|
FROM docker.io/library/python:3.11-slim as base
|
||||||
|
|
||||||
# Configure piwheels repo to use pre-compiled numpy wheels for arm
|
# Configure piwheels repo to use pre-compiled numpy wheels for arm
|
||||||
RUN echo -n "[global]\n" > /etc/pip.conf &&\
|
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
|
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 procps cmake g++ gcc
|
RUN apt-get update && apt-get install -y libgl1 libglib2.0-0
|
||||||
|
|
||||||
#################
|
#################
|
||||||
FROM base as model-builder
|
FROM base as model-builder
|
||||||
|
133
README.md
133
README.md
@ -9,136 +9,3 @@ To build images, run script:
|
|||||||
```bash
|
```bash
|
||||||
./build-docker.sh
|
./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
|
|
||||||
|
|
||||||
```
|
|
230
camera/cli.py
230
camera/cli.py
@ -5,16 +5,12 @@ import argparse
|
|||||||
import logging
|
import logging
|
||||||
import os
|
import os
|
||||||
import signal
|
import signal
|
||||||
import types
|
import typing, types
|
||||||
import typing
|
|
||||||
from typing import List
|
|
||||||
|
|
||||||
import depthai as dai
|
import depthai as dai
|
||||||
import paho.mqtt.client as mqtt
|
import paho.mqtt.client as mqtt
|
||||||
|
|
||||||
from camera import oak_pipeline as cam
|
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_DEFAULT = "default"
|
||||||
CAMERA_EXPOSITION_8300US = "8300us"
|
CAMERA_EXPOSITION_8300US = "8300us"
|
||||||
@ -53,9 +49,6 @@ def _parse_args_cli() -> argparse.Namespace:
|
|||||||
help="threshold to filter detected objects",
|
help="threshold to filter detected objects",
|
||||||
type=float,
|
type=float,
|
||||||
default=_get_env_float_value("OBJECTS_THRESHOLD", 0.2))
|
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",
|
parser.add_argument("-f", "--camera-fps",
|
||||||
help="set rate at which camera should produce frames",
|
help="set rate at which camera should produce frames",
|
||||||
type=int,
|
type=int,
|
||||||
@ -74,126 +67,6 @@ def _parse_args_cli() -> argparse.Namespace:
|
|||||||
type=str,
|
type=str,
|
||||||
default="info",
|
default="info",
|
||||||
choices=["info", "debug"])
|
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()
|
args = parser.parse_args()
|
||||||
return args
|
return args
|
||||||
|
|
||||||
@ -231,16 +104,6 @@ def execute_from_command_line() -> None:
|
|||||||
object_processor = cam.ObjectProcessor(mqtt_client=client,
|
object_processor = cam.ObjectProcessor(mqtt_client=client,
|
||||||
objects_topic=args.mqtt_topic_robocar_objects,
|
objects_topic=args.mqtt_topic_robocar_objects,
|
||||||
objects_threshold=args.objects_threshold)
|
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()
|
pipeline = dai.Pipeline()
|
||||||
if args.camera_tuning_exposition == CAMERA_EXPOSITION_500US:
|
if args.camera_tuning_exposition == CAMERA_EXPOSITION_500US:
|
||||||
@ -248,7 +111,6 @@ def execute_from_command_line() -> None:
|
|||||||
elif args.camera_tuning_exposition == CAMERA_EXPOSITION_8300US:
|
elif args.camera_tuning_exposition == CAMERA_EXPOSITION_8300US:
|
||||||
pipeline.setCameraTuningBlobPath('/camera_tuning/tuning_exp_limit_8300us.bin')
|
pipeline.setCameraTuningBlobPath('/camera_tuning/tuning_exp_limit_8300us.bin')
|
||||||
|
|
||||||
stereo_filters = _get_stereo_filters(args)
|
|
||||||
|
|
||||||
pipeline_controller = cam.PipelineController(pipeline=pipeline,
|
pipeline_controller = cam.PipelineController(pipeline=pipeline,
|
||||||
frame_processor=frame_processor,
|
frame_processor=frame_processor,
|
||||||
@ -258,9 +120,7 @@ def execute_from_command_line() -> None:
|
|||||||
img_width=args.image_width,
|
img_width=args.image_width,
|
||||||
img_height=args.image_height,
|
img_height=args.image_height,
|
||||||
fps=args.camera_fps,
|
fps=args.camera_fps,
|
||||||
),
|
))
|
||||||
depth_source=depth_source,
|
|
||||||
disparity_processor=disparity_processor)
|
|
||||||
|
|
||||||
def sigterm_handler(signum: int, frame: typing.Optional[
|
def sigterm_handler(signum: int, frame: typing.Optional[
|
||||||
types.FrameType]) -> None: # pylint: disable=unused-argument # need to implement handler signature
|
types.FrameType]) -> None: # pylint: disable=unused-argument # need to implement handler signature
|
||||||
@ -285,89 +145,3 @@ def _get_env_int_value(env_var: str, default_value: int) -> int:
|
|||||||
def _get_env_float_value(env_var: str, default_value: float) -> float:
|
def _get_env_float_value(env_var: str, default_value: float) -> float:
|
||||||
value = _get_env_value(env_var, str(default_value))
|
value = _get_env_value(env_var, str(default_value))
|
||||||
return float(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()
|
|
@ -6,8 +6,8 @@ import datetime
|
|||||||
import logging
|
import logging
|
||||||
import pathlib
|
import pathlib
|
||||||
import time
|
import time
|
||||||
|
import typing
|
||||||
from dataclasses import dataclass
|
from dataclasses import dataclass
|
||||||
from typing import List, Any
|
|
||||||
|
|
||||||
import cv2
|
import cv2
|
||||||
import depthai as dai
|
import depthai as dai
|
||||||
@ -26,8 +26,6 @@ _NN_HEIGHT = 192
|
|||||||
_PREVIEW_WIDTH = 640
|
_PREVIEW_WIDTH = 640
|
||||||
_PREVIEW_HEIGHT = 480
|
_PREVIEW_HEIGHT = 480
|
||||||
|
|
||||||
_CAMERA_BASELINE_IN_MM = 75
|
|
||||||
|
|
||||||
|
|
||||||
class ObjectProcessor:
|
class ObjectProcessor:
|
||||||
"""
|
"""
|
||||||
@ -97,7 +95,7 @@ class FrameProcessor:
|
|||||||
self._mqtt_client = mqtt_client
|
self._mqtt_client = mqtt_client
|
||||||
self._frame_topic = frame_topic
|
self._frame_topic = frame_topic
|
||||||
|
|
||||||
def process(self, img: dai.ImgFrame) -> Any:
|
def process(self, img: dai.ImgFrame) -> typing.Any:
|
||||||
"""
|
"""
|
||||||
Publish camera frames
|
Publish camera frames
|
||||||
:param img: image read from camera
|
:param img: image read from camera
|
||||||
@ -126,37 +124,6 @@ class FrameProcessor:
|
|||||||
return frame_msg.id
|
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):
|
class Source(abc.ABC):
|
||||||
"""Base class for image source"""
|
"""Base class for image source"""
|
||||||
|
|
||||||
@ -266,210 +233,6 @@ class CameraSource(Source):
|
|||||||
return manip
|
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
|
@dataclass
|
||||||
class MqttConfig:
|
class MqttConfig:
|
||||||
"""MQTT configuration"""
|
"""MQTT configuration"""
|
||||||
@ -509,8 +272,8 @@ class MqttSource(Source):
|
|||||||
|
|
||||||
@staticmethod
|
@staticmethod
|
||||||
# pylint: disable=unused-argument
|
# pylint: disable=unused-argument
|
||||||
def _on_connect(client: mqtt.Client, userdata: MqttConfig, flags: Any,
|
def _on_connect(client: mqtt.Client, userdata: MqttConfig, flags: typing.Any,
|
||||||
result_connection: Any) -> None:
|
result_connection: typing.Any) -> None:
|
||||||
# if we lose the connection and reconnect then subscriptions will be renewed.
|
# if we lose the connection and reconnect then subscriptions will be renewed.
|
||||||
client.subscribe(topic=userdata.topic, qos=userdata.qos)
|
client.subscribe(topic=userdata.topic, qos=userdata.qos)
|
||||||
|
|
||||||
@ -542,19 +305,15 @@ class PipelineController:
|
|||||||
"""
|
"""
|
||||||
|
|
||||||
def __init__(self, frame_processor: FrameProcessor,
|
def __init__(self, frame_processor: FrameProcessor,
|
||||||
object_processor: ObjectProcessor, disparity_processor: DisparityProcessor,
|
object_processor: ObjectProcessor, camera: Source, object_node: ObjectDetectionNN,
|
||||||
camera: Source, depth_source: Source, object_node: ObjectDetectionNN,
|
|
||||||
pipeline: dai.Pipeline):
|
pipeline: dai.Pipeline):
|
||||||
self._frame_processor = frame_processor
|
self._frame_processor = frame_processor
|
||||||
self._object_processor = object_processor
|
self._object_processor = object_processor
|
||||||
self._disparity_processor = disparity_processor
|
|
||||||
self._camera = camera
|
self._camera = camera
|
||||||
self._depth_source = depth_source
|
|
||||||
self._object_node = object_node
|
self._object_node = object_node
|
||||||
self._stop = False
|
self._stop = False
|
||||||
self._pipeline = pipeline
|
self._pipeline = pipeline
|
||||||
self._configure_pipeline()
|
self._configure_pipeline()
|
||||||
self._focal_length_in_pixels: float | None = None
|
|
||||||
|
|
||||||
def _configure_pipeline(self) -> None:
|
def _configure_pipeline(self) -> None:
|
||||||
logger.info("configure pipeline")
|
logger.info("configure pipeline")
|
||||||
@ -578,11 +337,6 @@ class PipelineController:
|
|||||||
logger.info('Connected cameras: %s', str(dev.getConnectedCameras()))
|
logger.info('Connected cameras: %s', str(dev.getConnectedCameras()))
|
||||||
logger.info("output queues found: %s", str(''.join(dev.getOutputQueueNames()))) # type: ignore
|
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()
|
dev.startPipeline()
|
||||||
# Queues
|
# Queues
|
||||||
queue_size = 4
|
queue_size = 4
|
||||||
@ -590,11 +344,6 @@ class PipelineController:
|
|||||||
blocking=False)
|
blocking=False)
|
||||||
q_nn = dev.getOutputQueue(name=self._object_node.get_stream_name(), maxSize=queue_size, # type: ignore
|
q_nn = dev.getOutputQueue(name=self._object_node.get_stream_name(), maxSize=queue_size, # type: ignore
|
||||||
blocking=False)
|
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()
|
start_time = time.time()
|
||||||
counter = 0
|
counter = 0
|
||||||
@ -606,7 +355,7 @@ class PipelineController:
|
|||||||
logger.info("stop loop event")
|
logger.info("stop loop event")
|
||||||
return
|
return
|
||||||
try:
|
try:
|
||||||
self._loop_on_camera_events(q_nn, q_rgb, q_disparity)
|
self._loop_on_camera_events(q_nn, q_rgb)
|
||||||
# pylint: disable=broad-except # bad frame or event must not stop loop
|
# pylint: disable=broad-except # bad frame or event must not stop loop
|
||||||
except Exception as ex:
|
except Exception as ex:
|
||||||
logger.exception("unexpected error: %s", str(ex))
|
logger.exception("unexpected error: %s", str(ex))
|
||||||
@ -620,7 +369,8 @@ class PipelineController:
|
|||||||
display_time = time.time()
|
display_time = time.time()
|
||||||
logger.info("fps: %s", fps)
|
logger.info("fps: %s", fps)
|
||||||
|
|
||||||
def _loop_on_camera_events(self, q_nn: dai.DataOutputQueue, q_rgb: dai.DataOutputQueue, q_disparity: dai.DataOutputQueue) -> None:
|
|
||||||
|
def _loop_on_camera_events(self, q_nn: dai.DataOutputQueue, q_rgb: dai.DataOutputQueue) -> None:
|
||||||
logger.debug("wait for new frame")
|
logger.debug("wait for new frame")
|
||||||
|
|
||||||
# Wait for frame
|
# Wait for frame
|
||||||
@ -640,12 +390,6 @@ class PipelineController:
|
|||||||
self._object_processor.process(in_nn, frame_ref)
|
self._object_processor.process(in_nn, frame_ref)
|
||||||
logger.debug("objects processed")
|
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:
|
def stop(self) -> None:
|
||||||
"""
|
"""
|
||||||
|
@ -10,7 +10,7 @@ import paho.mqtt.client as mqtt
|
|||||||
import pytest
|
import pytest
|
||||||
import pytest_mock
|
import pytest_mock
|
||||||
|
|
||||||
from camera.oak_pipeline import DisparityProcessor, ObjectProcessor, FrameProcessor, FrameProcessError
|
import camera.depthai
|
||||||
|
|
||||||
Object = dict[str, float]
|
Object = dict[str, float]
|
||||||
|
|
||||||
@ -76,16 +76,16 @@ class TestObjectProcessor:
|
|||||||
return m
|
return m
|
||||||
|
|
||||||
@pytest.fixture
|
@pytest.fixture
|
||||||
def object_processor(self, mqtt_client: mqtt.Client) -> ObjectProcessor:
|
def object_processor(self, mqtt_client: mqtt.Client) -> camera.depthai.ObjectProcessor:
|
||||||
return ObjectProcessor(mqtt_client, "topic/object", 0.2)
|
return camera.depthai.ObjectProcessor(mqtt_client, "topic/object", 0.2)
|
||||||
|
|
||||||
def test_process_without_object(self, object_processor: ObjectProcessor, mqtt_client: mqtt.Client,
|
def test_process_without_object(self, object_processor: camera.depthai.ObjectProcessor, mqtt_client: mqtt.Client,
|
||||||
raw_objects_empty: dai.NNData, frame_ref: events.events_pb2.FrameRef) -> None:
|
raw_objects_empty: dai.NNData, frame_ref: events.events_pb2.FrameRef) -> None:
|
||||||
object_processor.process(raw_objects_empty, frame_ref)
|
object_processor.process(raw_objects_empty, frame_ref)
|
||||||
publish_mock: unittest.mock.MagicMock = mqtt_client.publish # type: ignore
|
publish_mock: unittest.mock.MagicMock = mqtt_client.publish # type: ignore
|
||||||
publish_mock.assert_not_called()
|
publish_mock.assert_not_called()
|
||||||
|
|
||||||
def test_process_with_object_with_low_score(self, object_processor: ObjectProcessor,
|
def test_process_with_object_with_low_score(self, object_processor: camera.depthai.ObjectProcessor,
|
||||||
mqtt_client: mqtt.Client, raw_objects_one: dai.NNData,
|
mqtt_client: mqtt.Client, raw_objects_one: dai.NNData,
|
||||||
frame_ref: events.events_pb2.FrameRef) -> None:
|
frame_ref: events.events_pb2.FrameRef) -> None:
|
||||||
object_processor._objects_threshold = 0.9
|
object_processor._objects_threshold = 0.9
|
||||||
@ -94,7 +94,7 @@ class TestObjectProcessor:
|
|||||||
publish_mock.assert_not_called()
|
publish_mock.assert_not_called()
|
||||||
|
|
||||||
def test_process_with_one_object(self,
|
def test_process_with_one_object(self,
|
||||||
object_processor: ObjectProcessor, mqtt_client: mqtt.Client,
|
object_processor: camera.depthai.ObjectProcessor, mqtt_client: mqtt.Client,
|
||||||
raw_objects_one: dai.NNData, frame_ref: events.events_pb2.FrameRef,
|
raw_objects_one: dai.NNData, frame_ref: events.events_pb2.FrameRef,
|
||||||
object1: Object) -> None:
|
object1: Object) -> None:
|
||||||
object_processor.process(raw_objects_one, frame_ref)
|
object_processor.process(raw_objects_one, frame_ref)
|
||||||
@ -120,10 +120,10 @@ class TestObjectProcessor:
|
|||||||
|
|
||||||
class TestFrameProcessor:
|
class TestFrameProcessor:
|
||||||
@pytest.fixture
|
@pytest.fixture
|
||||||
def frame_processor(self, mqtt_client: mqtt.Client) -> FrameProcessor:
|
def frame_processor(self, mqtt_client: mqtt.Client) -> camera.depthai.FrameProcessor:
|
||||||
return FrameProcessor(mqtt_client, "topic/frame")
|
return camera.depthai.FrameProcessor(mqtt_client, "topic/frame")
|
||||||
|
|
||||||
def test_process(self, frame_processor: FrameProcessor, mocker: pytest_mock.MockerFixture,
|
def test_process(self, frame_processor: camera.depthai.FrameProcessor, mocker: pytest_mock.MockerFixture,
|
||||||
mqtt_client: mqtt.Client) -> None:
|
mqtt_client: mqtt.Client) -> None:
|
||||||
img: dai.ImgFrame = mocker.MagicMock()
|
img: dai.ImgFrame = mocker.MagicMock()
|
||||||
mocker.patch(target="cv2.imencode").return_value = (True, np.array(b"img content"))
|
mocker.patch(target="cv2.imencode").return_value = (True, np.array(b"img content"))
|
||||||
@ -145,57 +145,12 @@ class TestFrameProcessor:
|
|||||||
assert now - datetime.timedelta(
|
assert now - datetime.timedelta(
|
||||||
milliseconds=10) < frame_msg.id.created_at.ToDatetime() < now + datetime.timedelta(milliseconds=10)
|
milliseconds=10) < frame_msg.id.created_at.ToDatetime() < now + datetime.timedelta(milliseconds=10)
|
||||||
|
|
||||||
def test_process_error(self, frame_processor: FrameProcessor, mocker: pytest_mock.MockerFixture,
|
def test_process_error(self, frame_processor: camera.depthai.FrameProcessor, mocker: pytest_mock.MockerFixture,
|
||||||
mqtt_client: mqtt.Client) -> None:
|
mqtt_client: mqtt.Client) -> None:
|
||||||
img: dai.ImgFrame = mocker.MagicMock()
|
img: dai.ImgFrame = mocker.MagicMock()
|
||||||
mocker.patch(target="cv2.imencode").return_value = (False, None)
|
mocker.patch(target="cv2.imencode").return_value = (False, None)
|
||||||
|
|
||||||
with pytest.raises(FrameProcessError) as ex:
|
with pytest.raises(camera.depthai.FrameProcessError) as ex:
|
||||||
_ = frame_processor.process(img)
|
_ = frame_processor.process(img)
|
||||||
exception_raised = ex.value
|
exception_raised = ex.value
|
||||||
assert exception_raised.message == "unable to process to encode frame to jpg"
|
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"
|
|
||||||
|
1286
poetry.lock
generated
1286
poetry.lock
generated
File diff suppressed because it is too large
Load Diff
@ -9,14 +9,14 @@ packages = [
|
|||||||
]
|
]
|
||||||
|
|
||||||
[tool.poetry.dependencies]
|
[tool.poetry.dependencies]
|
||||||
python = "^3.12"
|
python = "^3.11"
|
||||||
paho-mqtt = "^1.6"
|
paho-mqtt = "^1.6.1"
|
||||||
depthai = "^2"
|
depthai = "^2.22.0"
|
||||||
protobuf3 = "^0.2.1"
|
protobuf3 = "^0.2.1"
|
||||||
google = "^3.0.0"
|
google = "^3.0.0"
|
||||||
protobuf = "^4.21"
|
protobuf = "^4.21.8"
|
||||||
opencv-python-headless = "^4.6.0"
|
opencv-python-headless = "^4.6.0.66"
|
||||||
robocar-protobuf = {version = "^1.6", source = "robocar"}
|
robocar-protobuf = {version = "^1.3.0", source = "robocar"}
|
||||||
|
|
||||||
|
|
||||||
[tool.poetry.group.test.dependencies]
|
[tool.poetry.group.test.dependencies]
|
||||||
|
Loading…
Reference in New Issue
Block a user