From 91edba2c62e80512d941efd8305ce2bf628e94ec Mon Sep 17 00:00:00 2001 From: Daniil Pastukhov Date: Tue, 20 Jun 2023 18:47:20 +0200 Subject: [PATCH 01/16] Add depth score --- .../src/depthai_sdk/classes/packets.py | 11 ++++ .../components/stereo_component.py | 33 ++++++++++- .../oak_outputs/xout/xout_depth.py | 6 +- .../oak_outputs/xout/xout_disparity.py | 57 ++++++++++++++++--- .../src/depthai_sdk/visualize/configs.py | 1 + .../src/depthai_sdk/visualize/visualizer.py | 3 +- 6 files changed, 99 insertions(+), 12 deletions(-) diff --git a/depthai_sdk/src/depthai_sdk/classes/packets.py b/depthai_sdk/src/depthai_sdk/classes/packets.py index d53d61bcd..c83a79d84 100644 --- a/depthai_sdk/src/depthai_sdk/classes/packets.py +++ b/depthai_sdk/src/depthai_sdk/classes/packets.py @@ -39,6 +39,7 @@ class _TrackingDetection(_Detection): class _TwoStageDetection(_Detection): nn_data: dai.NNData + class NNDataPacket: """ Contains only dai.NNData message @@ -50,6 +51,7 @@ def __init__(self, name: str, nn_data: dai.NNData): self.name = name self.msg = nn_data + class FramePacket: """ Contains only dai.ImgFrame message and cv2 frame, which is used by visualization logic. @@ -80,6 +82,7 @@ def __init__(self, self.color_frame = color_frame self.visualizer = visualizer + class DepthPacket(FramePacket): mono_frame: dai.ImgFrame @@ -88,6 +91,7 @@ def __init__(self, img_frame: dai.ImgFrame, mono_frame: Optional[dai.ImgFrame], depth_map: Optional[np.ndarray] = None, + confidence_map: Optional[np.ndarray] = None, visualizer: 'Visualizer' = None): super().__init__(name=name, msg=img_frame, @@ -98,6 +102,13 @@ def __init__(self, self.mono_frame = mono_frame self.depth_map = depth_map + self.confidence_map = confidence_map + self.depth_score = None + if self.confidence_map: + values = 1 - (self.confidence_map.getData() / 255) + values_no_outliers = values[np.logical_and(values > 0.0, values < 1.0)] + self.depth_score = np.sqrt(np.mean(values_no_outliers)) + class SpatialBbMappingPacket(FramePacket): """ diff --git a/depthai_sdk/src/depthai_sdk/components/stereo_component.py b/depthai_sdk/src/depthai_sdk/components/stereo_component.py index 1c813b6d8..216fbb2e9 100644 --- a/depthai_sdk/src/depthai_sdk/components/stereo_component.py +++ b/depthai_sdk/src/depthai_sdk/components/stereo_component.py @@ -113,6 +113,10 @@ def __init__(self, 'sigma': None } + # Output config + self.enable_depth_score = False + self.validate_calibration = False + self._undistortion_offset: Optional[int] = None if not self._replay: @@ -307,6 +311,23 @@ def config_wls(self, 'sigma': wls_sigma, } + def config_output(self, + depth_score: bool = None, + validate_calibration: bool = None + ) -> None: + """ + Configures output streams. + + Args: + depth_score: True to include depth score in the output packets. + validate_calibration: Check if the calibration is valid during the runtime (done on-host) and warn + the user if it's not. This can be used to detect if the calibration is invalid (e.g. due to temperature drift). + """ + if depth_score is not None: + self.enable_depth_score = depth_score + if validate_calibration is not None: + self.validate_calibration = validate_calibration + def set_colormap(self, colormap: dai.Colormap): """ Sets the colormap to use for colorizing the disparity map. Used for on-device postprocessing. @@ -432,6 +453,7 @@ def disparity(self, pipeline: dai.Pipeline, device: dai.Device) -> XoutBase: colormap=self._comp._postprocess_colormap, wls_config=self._comp.wls_config, ir_settings=self._comp.ir_settings, + confidence_map=self._try_get_confidence_map() ) return self._comp._create_xout(pipeline, out) @@ -464,7 +486,8 @@ def depth(self, pipeline: dai.Pipeline, device: dai.Device) -> XoutBase: colorize=self._comp._colorize, colormap=self._comp._postprocess_colormap, wls_config=self._comp.wls_config, - ir_settings=self._comp.ir_settings + ir_settings=self._comp.ir_settings, + confidence_map=self._try_get_confidence_map() ) return self._comp._create_xout(pipeline, out) @@ -489,3 +512,11 @@ def encoded(self, pipeline: dai.Pipeline, device: dai.Device) -> XoutBase: frame_shape=(1200, 800)) return self._comp._create_xout(pipeline, out) + + def _try_get_confidence_map(self): + confidence_map = None + if self._comp.enable_depth_score: + confidence_map = StreamXout(self._comp.node.id, + self._comp.node.confidenceMap, + name='depth_score') + return confidence_map diff --git a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py index cf774ac50..adf0cf06f 100644 --- a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py +++ b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py @@ -24,7 +24,8 @@ def __init__(self, colorize: StereoColor = None, colormap: int = None, wls_config: dict = None, - ir_settings: dict = None): + ir_settings: dict = None, + confidence_map: StreamXout = None): self.name = 'Depth' super().__init__(device=device, frames=frames, @@ -34,7 +35,8 @@ def __init__(self, colorize=colorize, colormap=colormap, wls_config=wls_config, - ir_settings=ir_settings) + ir_settings=ir_settings, + confidence_map=confidence_map) self.disp_scale_factor = dispScaleFactor diff --git a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py index 6856ff8dc..907234e19 100644 --- a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py +++ b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py @@ -2,6 +2,7 @@ import logging import warnings from collections import defaultdict +from functools import cached_property from typing import List, Optional import depthai as dai @@ -29,13 +30,17 @@ def __init__(self, colorize: StereoColor = None, colormap: int = None, wls_config: dict = None, - ir_settings: dict = None): + ir_settings: dict = None, + confidence_map: StreamXout = None): self.mono_frames = mono_frames self.multiplier = disp_factor self.fps = fps self.name = 'Disparity' self.device = device + self.confidence_map = confidence_map + self.fig, self.axes = None, None # for depth score visualization + self.colorize = colorize self.colormap = colormap self.use_wls_filter = wls_config['enabled'] @@ -127,12 +132,47 @@ def visualize(self, packet: DepthPacket): self._visualizer.add_circle(coords=(x, y), radius=3, color=(255, 255, 255), thickness=-1) self._visualizer.add_text(text=text, coords=(x, y - 10)) + if self._visualizer.config.stereo.depth_score and packet.confidence_map: + self.fig.canvas.draw() + self.axes.clear() + self.axes.hist(255 - packet.confidence_map.getData(), bins=3, color='blue', alpha=0.5) + self.axes.set_title(f'Depth score: {packet.depth_score:.2f}') + self.axes.set_xlabel('Depth score') + self.axes.set_ylabel('Frequency') + + # self.axes.text(0.5, 0.9, f'Overall depth score: {packet.depth_score:.2f}', ha='center', va='center', + # transform=self.axes.transAxes, fontsize=20) + # Convert plot to numpy array + img = np.fromstring(self.fig.canvas.tostring_rgb(), dtype=np.uint8, sep='') + img = img.reshape(self.fig.canvas.get_width_height()[::-1] + (3,)) + img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) + cv2.imshow(f'Depth score ({self.name})', img) + super().visualize(packet) def xstreams(self) -> List[StreamXout]: - if self.mono_frames is None: - return [self.frames] - return [self.frames, self.mono_frames] + streams = [self.frames] + if self.mono_frames is not None: + streams.append(self.mono_frames) + if self.confidence_map is not None: + streams.append(self.confidence_map) + + return streams + + def setup_visualize(self, + visualizer: 'Visualizer', + visualizer_enabled: bool, + name: str = None + ) -> None: + super().setup_visualize(visualizer, visualizer_enabled, name) + + if self.confidence_map: + from matplotlib import pyplot as plt + self.fig, self.axes = plt.subplots(1, 1, figsize=(8, 4), constrained_layout=False) + + @cached_property + def stream_names(self) -> List[str]: + return [s.name for s in self.xstreams()] def new_msg(self, name: str, msg: dai.Buffer) -> None: if name not in self._streams: @@ -144,9 +184,7 @@ def new_msg(self, name: str, msg: dai.Buffer) -> None: if seq not in self.msgs: self.msgs[seq] = dict() - if name == self.frames.name: - self.msgs[seq][name] = msg - elif name == self.mono_frames.name: + if name in self.stream_names: self.msgs[seq][name] = msg else: raise ValueError('Message from unknown stream name received by TwoStageSeqSync!') @@ -156,13 +194,16 @@ def new_msg(self, name: str, msg: dai.Buffer) -> None: if self.queue.full(): self.queue.get() # Get one, so queue isn't full - mono_frame = None + mono_frame, confidence_map = None, None if self.mono_frames is not None: mono_frame = self.msgs[seq][self.mono_frames.name] + if self.confidence_map is not None: + confidence_map = self.msgs[seq][self.confidence_map.name] packet = DepthPacket( self.get_packet_name(), img_frame=self.msgs[seq][self.frames.name], + confidence_map=confidence_map, mono_frame=mono_frame, visualizer=self._visualizer ) diff --git a/depthai_sdk/src/depthai_sdk/visualize/configs.py b/depthai_sdk/src/depthai_sdk/visualize/configs.py index 9d1e43a5c..50f9f1637 100644 --- a/depthai_sdk/src/depthai_sdk/visualize/configs.py +++ b/depthai_sdk/src/depthai_sdk/visualize/configs.py @@ -55,6 +55,7 @@ class StereoConfig: wls_filter: bool = False wls_lambda: float = 8000 wls_sigma: float = 1.5 + depth_score: bool = False @dataclass class DetectionConfig: diff --git a/depthai_sdk/src/depthai_sdk/visualize/visualizer.py b/depthai_sdk/src/depthai_sdk/visualize/visualizer.py index b4c257f37..0b849f9d2 100644 --- a/depthai_sdk/src/depthai_sdk/visualize/visualizer.py +++ b/depthai_sdk/src/depthai_sdk/visualize/visualizer.py @@ -318,7 +318,8 @@ def stereo(self, colormap: int = None, wls_filter: bool = None, wls_lambda: float = None, - wls_sigma: float = None): + wls_sigma: float = None, + depth_score: bool = None): kwargs = self._process_kwargs(locals()) if len(kwargs) > 0: From a29151b8ff8462cfbbc7ddec6b1eb721f6196fb4 Mon Sep 17 00:00:00 2001 From: Daniil Pastukhov Date: Mon, 26 Jun 2023 17:17:47 +0200 Subject: [PATCH 02/16] Add example for depth score --- depthai_sdk/examples/StereoComponent/depth_score.py | 9 +++++++++ 1 file changed, 9 insertions(+) create mode 100644 depthai_sdk/examples/StereoComponent/depth_score.py diff --git a/depthai_sdk/examples/StereoComponent/depth_score.py b/depthai_sdk/examples/StereoComponent/depth_score.py new file mode 100644 index 000000000..96269a056 --- /dev/null +++ b/depthai_sdk/examples/StereoComponent/depth_score.py @@ -0,0 +1,9 @@ +from depthai_sdk import OakCamera + +with OakCamera() as oak: + stereo = oak.create_stereo('800p', fps=60) + + stereo.config_output(depth_score=True) + + oak.visualize(stereo.out.depth, fps=True).stereo(depth_score=True) + oak.start(blocking=True) From 98f96161663ed8a86e203d9088a696b27023c209 Mon Sep 17 00:00:00 2001 From: Daniil Pastukhov Date: Mon, 26 Jun 2023 17:18:02 +0200 Subject: [PATCH 03/16] Remove sqrt from depth score computation --- depthai_sdk/src/depthai_sdk/classes/packets.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai_sdk/src/depthai_sdk/classes/packets.py b/depthai_sdk/src/depthai_sdk/classes/packets.py index c83a79d84..41612f62e 100644 --- a/depthai_sdk/src/depthai_sdk/classes/packets.py +++ b/depthai_sdk/src/depthai_sdk/classes/packets.py @@ -107,7 +107,7 @@ def __init__(self, if self.confidence_map: values = 1 - (self.confidence_map.getData() / 255) values_no_outliers = values[np.logical_and(values > 0.0, values < 1.0)] - self.depth_score = np.sqrt(np.mean(values_no_outliers)) + self.depth_score = np.mean(values_no_outliers) class SpatialBbMappingPacket(FramePacket): From 384c55d9c06ae92a884b529146dc6a7519123ceb Mon Sep 17 00:00:00 2001 From: Daniil Pastukhov Date: Mon, 26 Jun 2023 17:18:12 +0200 Subject: [PATCH 04/16] Update limits for auto IR --- .../oak_outputs/xout/xout_disparity.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py index 907234e19..8643f8de1 100644 --- a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py +++ b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py @@ -19,6 +19,11 @@ except ImportError: cv2 = None +IR_FLOOD_LIMIT = 765 +IR_FLOOD_STEP = IR_FLOOD_LIMIT / 4 +IR_DOT_LIMIT = 1200 +IR_DOT_STEP = IR_DOT_LIMIT / 4 + class XoutDisparity(XoutFrames, Clickable): def __init__(self, @@ -55,7 +60,8 @@ def __init__(self, self._converged_metric_value = None # Values that will be tested for function approximation - self._candidate_pairs = list(itertools.product(np.arange(0, 1201, 1200 / 4), np.arange(0, 1501, 1500 / 4))) + self._candidate_pairs = list(itertools.product(np.arange(0, IR_DOT_LIMIT + 1, IR_DOT_STEP), + np.arange(0, IR_FLOOD_LIMIT + 1, IR_FLOOD_STEP))) self._neighbourhood_pairs = [] self._candidate_idx, self._neighbour_idx = 0, 0 self._X, self._y = [], [] @@ -168,7 +174,7 @@ def setup_visualize(self, if self.confidence_map: from matplotlib import pyplot as plt - self.fig, self.axes = plt.subplots(1, 1, figsize=(8, 4), constrained_layout=False) + self.fig, self.axes = plt.subplots(1, 1, figsize=(5, 2), constrained_layout=False) @cached_property def stream_names(self) -> List[str]: @@ -281,8 +287,10 @@ def _check_consistency(self, frame): self._auto_ir_converged = False self._checking_neighbourhood = True self._neighbourhood_pairs = np.unique([ - [np.clip(self._dot_projector_brightness + i, 0, 1200), np.clip(self._flood_brightness + j, 0, 1500)] - for i, j in itertools.product([-300, 300], [-375, 375]) + [np.clip(self._dot_projector_brightness + i, 0, IR_DOT_LIMIT), + np.clip(self._flood_brightness + j, 0, IR_FLOOD_LIMIT)] + for i, j in itertools.product([-IR_DOT_STEP / 2, IR_DOT_STEP / 2], + [-IR_FLOOD_STEP / 2, IR_FLOOD_STEP / 2]) ], axis=0) self._neighbour_idx = 0 From e568e8c8b86b110927e98530e5c65d31c84e76cc Mon Sep 17 00:00:00 2001 From: Daniil Pastukhov Date: Tue, 27 Jun 2023 15:17:50 +0200 Subject: [PATCH 05/16] Address issue when OAK-D-SR crashed --- .../src/depthai_sdk/components/stereo_component.py | 4 ++-- .../src/depthai_sdk/oak_outputs/xout/xout_disparity.py | 9 +++++---- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/depthai_sdk/src/depthai_sdk/components/stereo_component.py b/depthai_sdk/src/depthai_sdk/components/stereo_component.py index 891597262..34174d4b5 100644 --- a/depthai_sdk/src/depthai_sdk/components/stereo_component.py +++ b/depthai_sdk/src/depthai_sdk/components/stereo_component.py @@ -128,7 +128,7 @@ def __init__(self, self._resolution = self._resolution or dai.MonoCameraProperties.SensorResolution.THE_400_P # Always use 1200p for OAK-D-LR and OAK-D-SR - if self._device.getDeviceName() in ['OAK-D-LR', 'OAK-D-SR']: + if self._device.getDeviceName() == 'OAK-D-LR': self._resolution = dai.MonoCameraProperties.SensorResolution.THE_1200_P if not self.left: @@ -138,7 +138,7 @@ def __init__(self, replay=self._replay) # AR0234 outputs 1200p, so we need to resize it to 800p on RVC2 - if self._device.getDeviceName() in ['OAK-D-LR', 'OAK-D-SR']: + if self._device.getDeviceName() == 'OAK-D-LR': if isinstance(self.left, CameraComponent) and isinstance(self.right, CameraComponent): self.left.config_color_camera(isp_scale=(2, 3)) self.right.config_color_camera(isp_scale=(2, 3)) diff --git a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py index fd4368e3b..8bacf96b1 100644 --- a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py +++ b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py @@ -100,8 +100,11 @@ def on_callback(self, packet) -> None: def visualize(self, packet: DepthPacket): frame = packet.frame - mono_frame = packet.mono_frame.getCvFrame() disparity_frame = (frame * self.multiplier).astype(np.uint8) + try: + mono_frame = packet.mono_frame.getCvFrame() + except AttributeError: + mono_frame = None stereo_config = self._visualizer.config.stereo @@ -117,7 +120,7 @@ def visualize(self, packet: DepthPacket): colormap = stereo_config.colormap colormap[0] = [0, 0, 0] # Invalidate pixels 0 to be black - if disparity_frame.ndim == 2 and mono_frame.ndim == 3: + if mono_frame and disparity_frame.ndim == 2 and mono_frame.ndim == 3: disparity_frame = disparity_frame[..., np.newaxis] if colorize == StereoColor.GRAY: @@ -150,8 +153,6 @@ def visualize(self, packet: DepthPacket): self.axes.set_xlabel('Depth score') self.axes.set_ylabel('Frequency') - # self.axes.text(0.5, 0.9, f'Overall depth score: {packet.depth_score:.2f}', ha='center', va='center', - # transform=self.axes.transAxes, fontsize=20) # Convert plot to numpy array img = np.fromstring(self.fig.canvas.tostring_rgb(), dtype=np.uint8, sep='') img = img.reshape(self.fig.canvas.get_width_height()[::-1] + (3,)) From 0987c1d2dc4e48847cca611309ea99f0bd721124 Mon Sep 17 00:00:00 2001 From: Daniil Pastukhov Date: Tue, 12 Sep 2023 18:34:01 +0200 Subject: [PATCH 06/16] Update depth score --- .../examples/StereoComponent/depth_score.py | 7 +++++-- depthai_sdk/src/depthai_sdk/classes/packets.py | 2 ++ .../depthai_sdk/components/stereo_component.py | 18 ++++++++---------- .../depthai_sdk/oak_outputs/xout/xout_depth.py | 2 ++ .../oak_outputs/xout/xout_disparity.py | 2 +- 5 files changed, 18 insertions(+), 13 deletions(-) diff --git a/depthai_sdk/examples/StereoComponent/depth_score.py b/depthai_sdk/examples/StereoComponent/depth_score.py index 96269a056..c779ac203 100644 --- a/depthai_sdk/examples/StereoComponent/depth_score.py +++ b/depthai_sdk/examples/StereoComponent/depth_score.py @@ -1,9 +1,12 @@ from depthai_sdk import OakCamera +def callback(packet): + print(packet.depth_score) + with OakCamera() as oak: stereo = oak.create_stereo('800p', fps=60) stereo.config_output(depth_score=True) - - oak.visualize(stereo.out.depth, fps=True).stereo(depth_score=True) + stereo.config_output(depth_score=True) + oak.callback(stereo.out.disparity, callback) oak.start(blocking=True) diff --git a/depthai_sdk/src/depthai_sdk/classes/packets.py b/depthai_sdk/src/depthai_sdk/classes/packets.py index 4e89600a7..b1b0d8850 100644 --- a/depthai_sdk/src/depthai_sdk/classes/packets.py +++ b/depthai_sdk/src/depthai_sdk/classes/packets.py @@ -192,6 +192,7 @@ def __init__(self, colormap: int = None, mono_frame: Optional[dai.ImgFrame] = None, disp_scale_factor=255 / 95, + confidence_map=None ): # DepthPacket.__init__(self, name=name, msg=img_frame) super().__init__( @@ -202,6 +203,7 @@ def __init__(self, colorize=colorize, colormap=colormap, mono_frame=mono_frame, + confidence_map=confidence_map ) self.disp_scale_factor = disp_scale_factor diff --git a/depthai_sdk/src/depthai_sdk/components/stereo_component.py b/depthai_sdk/src/depthai_sdk/components/stereo_component.py index 619f28a43..aaba14c22 100644 --- a/depthai_sdk/src/depthai_sdk/components/stereo_component.py +++ b/depthai_sdk/src/depthai_sdk/components/stereo_component.py @@ -459,6 +459,12 @@ def _mono_frames(self): mono_frames = StreamXout(self._right_stream) return mono_frames + def _try_get_confidence_map(self): + confidence_map = None + if self.enable_depth_score: + confidence_map = StreamXout(self.node.confidenceMap, name='depth_score') + return confidence_map + class Out: class DepthOut(ComponentOutput): def __call__(self, device: dai.Device) -> XoutBase: @@ -470,7 +476,7 @@ def __call__(self, device: dai.Device) -> XoutBase: colorize=self._comp._colorize, colormap=self._comp._postprocess_colormap, ir_settings=self._comp.ir_settings, - confidence_map=self._try_get_confidence_map() + confidence_map=self._comp._try_get_confidence_map() ).set_comp_out(self) @@ -486,7 +492,7 @@ def __call__(self, device: dai.Device, fourcc: Optional[str] = None) -> XoutBase colormap=self._comp._postprocess_colormap, wls_config=self._comp.wls_config, ir_settings=self._comp.ir_settings, - confidence_map=self._try_get_confidence_map() + confidence_map=self._comp._try_get_confidence_map() ).set_comp_out(self) class RectifiedLeftOut(ComponentOutput): @@ -515,11 +521,3 @@ def __init__(self, stereo_component: 'StereoComponent'): self.disparity = self.DisparityOut(stereo_component) self.encoded = self.EncodedOut(stereo_component) self.main = self.depth - - def _try_get_confidence_map(self): - confidence_map = None - if self._comp.enable_depth_score: - confidence_map = StreamXout(self._comp.node.id, - self._comp.node.confidenceMap, - name='depth_score') - return confidence_map diff --git a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py index 92329bb95..059642ed4 100644 --- a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py +++ b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py @@ -32,6 +32,7 @@ def __init__(self, def package(self, msgs: Dict) -> DisparityDepthPacket: mono_frame = msgs[self.mono_frames.name] if self.mono_frames else None + confidence_map = msgs[self.confidence_map.name] if self.confidence_map else None return DisparityDepthPacket( self.get_packet_name(), msgs[self.frames.name], @@ -39,4 +40,5 @@ def package(self, msgs: Dict) -> DisparityDepthPacket: colormap=self.colormap, mono_frame=mono_frame, disp_scale_factor=self.disp_scale_factor, + confidence_map=confidence_map ) diff --git a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py index fc7c41cf2..05281390a 100644 --- a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py +++ b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py @@ -87,7 +87,7 @@ def __init__(self, self.use_wls_filter = False XoutFrames.__init__(self, frames=frames) - XoutSeqSync.__init__(self, [frames, mono_frames]) + XoutSeqSync.__init__(self, self.xstreams()) def on_callback(self, packet) -> None: if self.ir_settings['auto_mode']: From 548e225158c61161c2255c8970d7f2021f37da83 Mon Sep 17 00:00:00 2001 From: Filip Jeretina <59307111+zrezke@users.noreply.github.com> Date: Fri, 15 Sep 2023 22:41:08 +0200 Subject: [PATCH 07/16] Port the existing tof component from the tof_support branch to latest develop (#1119) --- depthai_sdk/examples/ToFComponent/tof.py | 8 ++ .../depthai_sdk/components/tof_component.py | 82 +++++++++++++++++++ depthai_sdk/src/depthai_sdk/oak_camera.py | 12 +++ 3 files changed, 102 insertions(+) create mode 100644 depthai_sdk/examples/ToFComponent/tof.py create mode 100644 depthai_sdk/src/depthai_sdk/components/tof_component.py diff --git a/depthai_sdk/examples/ToFComponent/tof.py b/depthai_sdk/examples/ToFComponent/tof.py new file mode 100644 index 000000000..31b6ca2f0 --- /dev/null +++ b/depthai_sdk/examples/ToFComponent/tof.py @@ -0,0 +1,8 @@ +from depthai_sdk import OakCamera + +with OakCamera() as oak: + tof = oak.create_tof("cama") + depth_q = oak.queue(tof.out.depth).queue + amplitude_q = oak.queue(tof.out.amplitude).queue + oak.visualize([tof.out.depth, tof.out.amplitude]) + oak.start(blocking=True) diff --git a/depthai_sdk/src/depthai_sdk/components/tof_component.py b/depthai_sdk/src/depthai_sdk/components/tof_component.py new file mode 100644 index 000000000..ded0c3ce5 --- /dev/null +++ b/depthai_sdk/src/depthai_sdk/components/tof_component.py @@ -0,0 +1,82 @@ +from typing import List, Union + +import depthai as dai + +from depthai_sdk.components.component import Component, ComponentOutput +from depthai_sdk.oak_outputs.xout.xout_base import StreamXout, XoutBase +from depthai_sdk.oak_outputs.xout.xout_frames import XoutFrames +from depthai_sdk.oak_outputs.xout.xout_depth import XoutDisparityDepth +from depthai_sdk.components.parser import parse_camera_socket + +class ToFComponent(Component): + def __init__(self, + device: dai.Device, + pipeline: dai.Pipeline, + source: Union[str, dai.CameraBoardSocket, None] = None, + ): + super().__init__() + self.out = self.Out(self) + self._pipeline = pipeline + + if source is None: + source = self._find_tof(device) + elif isinstance(source, str): + source = parse_camera_socket(source) + elif isinstance(source, dai.CameraBoardSocket): + pass # This is what we want + else: + raise ValueError('source must be either None, str, or CameraBoardSocket!') + + self.camera_node = pipeline.create(dai.node.ColorCamera) + self.camera_node.setBoardSocket(source) + self.camera_socket = source + + self.node = pipeline.create(dai.node.ToF) + self.camera_node.raw.link(self.node.input) + + tofConfig = self.node.initialConfig.get() + # tofConfig.depthParams.freqModUsed = dai.RawToFConfig.DepthParams.TypeFMod.MIN + tofConfig.depthParams.freqModUsed = dai.RawToFConfig.DepthParams.TypeFMod.MAX + tofConfig.depthParams.avgPhaseShuffle = False + tofConfig.depthParams.minimumAmplitude = 3.0 + self.node.initialConfig.set(tofConfig) + + def _find_tof(self, device: dai.Device) -> dai.CameraBoardSocket: + # Use the first ToF sensor, usually, there will only be one + features = device.getConnectedCameraFeatures() + for cam_sensor in features: + if dai.CameraSensorType.TOF in cam_sensor.supportedTypes: + return cam_sensor.socket + raise RuntimeError(f'No ToF sensor found on this camera! {device.getConnectedCameraFeatures()}') + + class Out: + + class DepthOut(ComponentOutput): + def __call__(self, device: dai.Device) -> XoutBase: + return XoutDisparityDepth( + device=device, + frames=StreamXout(self._comp.node.depth, "tof_depth"), + dispScaleFactor=9500, + mono_frames=None, + ir_settings={ + "auto_mode": False + } + ).set_comp_out(self) + + class AmplitudeOut(ComponentOutput): + def __call__(self, device: dai.Device) -> XoutBase: + return XoutFrames( + frames=StreamXout(self._comp.node.amplitude, "tof_amplitude") + ).set_comp_out(self) + + class ErrorOut(ComponentOutput): + def __call__(self, device: dai.Device) -> XoutBase: + return XoutFrames( + frames=StreamXout(self._comp.node.error, "tof_error") + ).set_comp_out(self) + + def __init__(self, tof_component: 'ToFComponent'): + self.depth = self.DepthOut(tof_component) + self.amplitude = self.AmplitudeOut(tof_component) + self.error = self.ErrorOut(tof_component) + self.main = self.depth diff --git a/depthai_sdk/src/depthai_sdk/oak_camera.py b/depthai_sdk/src/depthai_sdk/oak_camera.py index d1e9fab51..9d15f1874 100644 --- a/depthai_sdk/src/depthai_sdk/oak_camera.py +++ b/depthai_sdk/src/depthai_sdk/oak_camera.py @@ -30,6 +30,7 @@ from depthai_sdk.components.camera_component import CameraComponent from depthai_sdk.components.component import Component, ComponentOutput from depthai_sdk.components.imu_component import IMUComponent +from depthai_sdk.components.tof_component import ToFComponent from depthai_sdk.components.nn_component import NNComponent from depthai_sdk.components.parser import ( parse_usb_speed, @@ -389,6 +390,17 @@ def create_stereo(self, """ return self.stereo(resolution, fps, left, right, encode) + def create_tof(self, source: Union[str, dai.CameraBoardSocket, None] = None) -> ToFComponent: + """ + Create ToF component. + + Args: + source (str / dai.CameraBoardSocket): ToF camera source + """ + comp = ToFComponent(self.device, self.pipeline, source) + self._components.append(comp) + return comp + def create_imu(self) -> IMUComponent: """ Create IMU component From 7acd6d6f49c7de1a882bbdb00319c75ece701a77 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Fri, 29 Sep 2023 00:15:14 +0200 Subject: [PATCH 08/16] Fix BB mappings (resize_to_aspect_ratio) for letterbox/crop --- depthai_sdk/src/depthai_sdk/visualize/bbox.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/depthai_sdk/src/depthai_sdk/visualize/bbox.py b/depthai_sdk/src/depthai_sdk/visualize/bbox.py index db2152ef2..58e6d9949 100644 --- a/depthai_sdk/src/depthai_sdk/visualize/bbox.py +++ b/depthai_sdk/src/depthai_sdk/visualize/bbox.py @@ -195,16 +195,18 @@ def resize_to_aspect_ratio(self, bb = BoundingBox((0, 0, 1, 1)) if resize_mode == ResizeMode.LETTERBOX: - padding = (old_aspect_ratio - new_aspect_ratio) / 2 - if padding > 0: - bb = BoundingBox((0, -padding, 1, 1 + padding)) + if old_aspect_ratio < new_aspect_ratio: + padding = (new_aspect_ratio - old_aspect_ratio) / (2 * old_aspect_ratio) + bb = BoundingBox((-padding, 0, 1 + padding, 1)) else: - bb = BoundingBox((padding, 0, 1 - padding, 1)) + padding = (old_aspect_ratio - new_aspect_ratio) / (2 * new_aspect_ratio) + bb = BoundingBox((0, -padding, 1, 1 + padding)) elif resize_mode in [ResizeMode.CROP, ResizeMode.FULL_CROP]: - cropping = (1 - (new_aspect_ratio / old_aspect_ratio)) / 2 - if cropping < 0: - bb = BoundingBox((0, -cropping, 1, 1 + cropping)) + if old_aspect_ratio < new_aspect_ratio: + cropping = (1 - (old_aspect_ratio / new_aspect_ratio)) / 2 + bb = BoundingBox((0, cropping, 1, 1 - cropping)) else: + cropping = (1 - (new_aspect_ratio / old_aspect_ratio)) / 2 bb = BoundingBox((cropping, 0, 1 - cropping, 1)) else: # ResizeMode.STRETCH doesn't require any adjustments, as the aspect ratio is the same From 1cc223282510306582181ce71f4fa77227596a05 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Fri, 6 Oct 2023 15:32:28 +0200 Subject: [PATCH 09/16] Updated collision avoidance visualization --- .../examples/mixed/collision_avoidance.py | 74 ++++++++++--------- 1 file changed, 38 insertions(+), 36 deletions(-) diff --git a/depthai_sdk/examples/mixed/collision_avoidance.py b/depthai_sdk/examples/mixed/collision_avoidance.py index eb74b4d30..3c7320d1d 100644 --- a/depthai_sdk/examples/mixed/collision_avoidance.py +++ b/depthai_sdk/examples/mixed/collision_avoidance.py @@ -1,6 +1,7 @@ from depthai_sdk import OakCamera from depthai_sdk.visualize.configs import StereoColor from depthai_sdk.classes.packets import DisparityDepthPacket +from depthai_sdk.visualize.visualizers.opencv_visualizer import OpenCvVisualizer import math import depthai as dai import cv2 @@ -10,38 +11,9 @@ CRITICAL = 500 # 50cm, red slc_data = [] +fontType = cv2.FONT_HERSHEY_TRIPLEX -def cb(packet: DisparityDepthPacket): - global slc_data - fontType = cv2.FONT_HERSHEY_TRIPLEX - - depthFrameColor = packet.visualizer.draw(packet.frame) - - for depthData in slc_data: - roi = depthData.config.roi - roi = roi.denormalize(width=depthFrameColor.shape[1], height=depthFrameColor.shape[0]) - - xmin = int(roi.topLeft().x) - ymin = int(roi.topLeft().y) - xmax = int(roi.bottomRight().x) - ymax = int(roi.bottomRight().y) - - coords = depthData.spatialCoordinates - distance = math.sqrt(coords.x ** 2 + coords.y ** 2 + coords.z ** 2) - - if distance == 0: # Invalid - continue - - if distance < CRITICAL: - color = (0, 0, 255) - cv2.rectangle(depthFrameColor, (xmin, ymin), (xmax, ymax), color, thickness=4) - cv2.putText(depthFrameColor, "{:.1f}m".format(distance/1000), (xmin + 10, ymin + 20), fontType, 0.5, color) - elif distance < WARNING: - color = (0, 140, 255) - cv2.rectangle(depthFrameColor, (xmin, ymin), (xmax, ymax), color, thickness=2) - cv2.putText(depthFrameColor, "{:.1f}m".format(distance/1000), (xmin + 10, ymin + 20), fontType, 0.5, color) - - cv2.imshow('0_depth', depthFrameColor) + with OakCamera() as oak: stereo = oak.create_stereo('720p') @@ -54,7 +26,6 @@ def cb(packet: DisparityDepthPacket): stereo.config_postprocessing(colorize=StereoColor.RGBD, colormap=cv2.COLORMAP_BONE) stereo.config_stereo(confidence=50, lr_check=True, extended=True) - oak.visualize([stereo], fps=True, callback=cb) slc = oak.pipeline.create(dai.node.SpatialLocationCalculator) for x in range(15): @@ -73,10 +44,41 @@ def cb(packet: DisparityDepthPacket): slc_out.setStreamName('slc') slc.out.link(slc_out.input) + stereoQ = oak.queue(stereo.out.depth).get_queue() + oak.start() # Start the pipeline (upload it to the OAK) - q = oak.device.getOutputQueue('slc') # Create output queue after calling start() + slcQ = oak.device.getOutputQueue('slc') # Create output queue after calling start() + vis = OpenCvVisualizer() while oak.running(): - if q.has(): - slc_data = q.get().getSpatialLocations() - oak.poll() \ No newline at end of file + oak.poll() + packet: DisparityDepthPacket = stereoQ.get() + slc_data = slcQ.get().getSpatialLocations() + + depthFrameColor = packet.get_colorized_frame(vis) + + for depthData in slc_data: + roi = depthData.config.roi + roi = roi.denormalize(width=depthFrameColor.shape[1], height=depthFrameColor.shape[0]) + + xmin = int(roi.topLeft().x) + ymin = int(roi.topLeft().y) + xmax = int(roi.bottomRight().x) + ymax = int(roi.bottomRight().y) + + coords = depthData.spatialCoordinates + distance = math.sqrt(coords.x ** 2 + coords.y ** 2 + coords.z ** 2) + + if distance == 0: # Invalid + continue + + if distance < CRITICAL: + color = (0, 0, 255) + cv2.rectangle(depthFrameColor, (xmin, ymin), (xmax, ymax), color, thickness=4) + cv2.putText(depthFrameColor, "{:.1f}m".format(distance/1000), (xmin + 10, ymin + 20), fontType, 0.5, color) + elif distance < WARNING: + color = (0, 140, 255) + cv2.rectangle(depthFrameColor, (xmin, ymin), (xmax, ymax), color, thickness=2) + cv2.putText(depthFrameColor, "{:.1f}m".format(distance/1000), (xmin + 10, ymin + 20), fontType, 0.5, color) + + cv2.imshow('Frame', depthFrameColor) From 8287e5d830086a2ec3caa1eb668c474569c6cab6 Mon Sep 17 00:00:00 2001 From: moratom <47612463+moratom@users.noreply.github.com> Date: Thu, 2 Nov 2023 17:34:36 +0100 Subject: [PATCH 10/16] Change queue size to 1 --- depthai_sdk/src/depthai_sdk/oak_camera.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/depthai_sdk/src/depthai_sdk/oak_camera.py b/depthai_sdk/src/depthai_sdk/oak_camera.py index 9d15f1874..73780ba5e 100644 --- a/depthai_sdk/src/depthai_sdk/oak_camera.py +++ b/depthai_sdk/src/depthai_sdk/oak_camera.py @@ -503,7 +503,7 @@ def start(self, blocking=False): for xlink_name in self._new_msg_callbacks: try: - self.device.getOutputQueue(xlink_name, maxSize=4, blocking=False).addCallback(self._new_oak_msg) + self.device.getOutputQueue(xlink_name, maxSize=1, blocking=False).addCallback(self._new_oak_msg) # TODO: make this nicer, have self._new_msg_callbacks know whether it's replay or not except Exception as e: if self.replay: From 57b1fb64169c6464c8544b4f6b711a31fe78d49d Mon Sep 17 00:00:00 2001 From: Erol444 Date: Tue, 5 Mar 2024 20:26:30 +0100 Subject: [PATCH 11/16] Fix recording encoded disparity --- depthai_sdk/src/depthai_sdk/components/stereo_component.py | 1 + depthai_sdk/src/depthai_sdk/oak_camera.py | 2 +- depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py | 3 ++- depthai_sdk/src/depthai_sdk/recorders/abstract_recorder.py | 2 +- 4 files changed, 5 insertions(+), 3 deletions(-) diff --git a/depthai_sdk/src/depthai_sdk/components/stereo_component.py b/depthai_sdk/src/depthai_sdk/components/stereo_component.py index aaba14c22..33f7145f2 100644 --- a/depthai_sdk/src/depthai_sdk/components/stereo_component.py +++ b/depthai_sdk/src/depthai_sdk/components/stereo_component.py @@ -489,6 +489,7 @@ def __call__(self, device: dai.Device, fourcc: Optional[str] = None) -> XoutBase disp_factor=255.0 / self._comp.node.getMaxDisparity(), mono_frames=self._comp._mono_frames(), colorize=self._comp._colorize, + fourcc=fourcc, colormap=self._comp._postprocess_colormap, wls_config=self._comp.wls_config, ir_settings=self._comp.ir_settings, diff --git a/depthai_sdk/src/depthai_sdk/oak_camera.py b/depthai_sdk/src/depthai_sdk/oak_camera.py index 73780ba5e..102e19872 100644 --- a/depthai_sdk/src/depthai_sdk/oak_camera.py +++ b/depthai_sdk/src/depthai_sdk/oak_camera.py @@ -348,7 +348,7 @@ def stereo(self, fps (float): If monochrome cameras aren't already passed, create them and set specified FPS left (CameraComponent/dai.node.MonoCamera): Pass the camera object (component/node) that will be used for stereo camera. right (CameraComponent/dai.node.MonoCamera): Pass the camera object (component/node) that will be used for stereo camera. - encode (bool/str/Profile): Whether we want to enable video encoding (accessible via StereoComponent.out.encoded). If True, it will use h264 codec. + encode (bool/str/Profile): Whether we want to enable video encoding (accessible via StereoComponent.out.encoded). If True, it will use MJPEG. """ if left is None: left = self.camera(source="left", resolution=resolution, fps=fps) diff --git a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py index 05281390a..16a94b1d6 100644 --- a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py +++ b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py @@ -31,6 +31,7 @@ def __init__(self, frames: StreamXout, disp_factor: float, mono_frames: Optional[StreamXout], + fourcc: str = None, colorize: StereoColor = None, colormap: int = None, wls_config: dict = None, @@ -86,7 +87,7 @@ def __init__(self, ) self.use_wls_filter = False - XoutFrames.__init__(self, frames=frames) + XoutFrames.__init__(self, frames=frames, fourcc=fourcc) XoutSeqSync.__init__(self, self.xstreams()) def on_callback(self, packet) -> None: diff --git a/depthai_sdk/src/depthai_sdk/recorders/abstract_recorder.py b/depthai_sdk/src/depthai_sdk/recorders/abstract_recorder.py index 6b593300e..41af09add 100644 --- a/depthai_sdk/src/depthai_sdk/recorders/abstract_recorder.py +++ b/depthai_sdk/src/depthai_sdk/recorders/abstract_recorder.py @@ -35,7 +35,7 @@ def __init__(self, xout: outputs.xout_base.XoutBase): if isinstance(xout, outputs.xout_depth.XoutDisparityDepth): self.xlink_name = xout.frames.name self.type = self.StreamType.DEPTH # TODO is depth raw or should it be DEPTH? - elif isinstance(xout, outputs.xout_disparity.XoutDisparity): + elif isinstance(xout, outputs.xout_disparity.XoutDisparity) and xout._fourcc is None: self.xlink_name = xout.frames.name self.type = self.StreamType.RAW elif isinstance(xout, outputs.xout_frames.XoutFrames): From c4663ec890339898ddcb2e3cbb0eca34e44cdc14 Mon Sep 17 00:00:00 2001 From: jakaskerl Date: Tue, 2 Apr 2024 16:30:30 +0200 Subject: [PATCH 12/16] Updated calibrate.py --- calibrate.py | 45 +++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 41 insertions(+), 4 deletions(-) diff --git a/calibrate.py b/calibrate.py index 11854405f..ff754fcf7 100755 --- a/calibrate.py +++ b/calibrate.py @@ -353,7 +353,6 @@ def __init__(self): self.device = dai.Device() self.enablePolygonsDisplay = self.args.enablePolygonsDisplay self.board_name = None - cameraProperties = self.device.getConnectedCameraFeatures() calibData = self.device.readCalibration() eeprom = calibData.getEepromData() #TODO Change only in getDeviceName in next revision. @@ -375,7 +374,7 @@ def __init__(self): print(f"Device name: {detection}") detection = detection.split("-") except: - cameraProperties = self.device.getConnectedCameraFeatures() + self.cameraProperties = self.device.getConnectedCameraFeatures() calibData = self.device.readCalibration() eeprom = calibData.getEepromData() eeprom.productName = eeprom.productName.replace(" ", "-").upper() @@ -432,8 +431,13 @@ def __init__(self): name = self.board_config['cameras'][cam_id]['name'] self.coverageImages[name] = None - cameraProperties = self.device.getConnectedCameraFeatures() - for properties in cameraProperties: + if (self._set_camera_features()): + print("Camera features set using board config.") + print(f"self.cameraProperties: {self.cameraProperties}") + else: + self.cameraProperties = self.device.getConnectedCameraFeatures() + + for properties in self.cameraProperties: for in_cam in self.board_config['cameras'].keys(): cam_info = self.board_config['cameras'][in_cam] if cam_info["name"] not in self.args.disableCamera: @@ -455,6 +459,39 @@ def mouse_event_callback(self, event, x, y, flags, param): if event == cv2.EVENT_LBUTTONDOWN: self.mouseTrigger = True + def _set_camera_features(self): + """ + Create mock camera properties using manually specified board config + """ + + self.cameraProperties = self.device.getConnectedCameraFeatures() + + for cam_id in self.board_config['cameras']: + try: + sensor_model = self.board_config['cameras'][cam_id]["model"] + config_type = self.board_config['cameras'][cam_id]["type"] + except KeyError: + print(f"Model not found for {cam_id}, skipping...") + return False + + if sensor_model in camToRgbRes: + supportedTypes = [dai.CameraSensorType.COLOR] + elif sensor_model in camToMonoRes: + supportedTypes = [dai.CameraSensorType.MONO] + else: + print(f"Model {sensor_model} not supported") + return False + + if supportedTypes[0].name != config_type.upper(): + raise ValueError(f"Mismatch in camera type for {cam_id} with model {sensor_model} and type {config_type}, please fix the board config.") + + for cam in self.cameraProperties: + if stringToCam[cam_id] == cam.socket: + cam.sensorName = sensor_model + cam.supportedTypes = supportedTypes + + return True + def startPipeline(self): pipeline = self.create_pipeline() self.device.startPipeline(pipeline) From 5d3aa6d697581e775cffc355d1aff643d3cff971 Mon Sep 17 00:00:00 2001 From: zrezke Date: Wed, 24 Apr 2024 15:56:51 +0200 Subject: [PATCH 13/16] Updated tof_component.py to the latest tof_decoding pipeline --- depthai_sdk/requirements.txt | 2 +- .../src/depthai_sdk/components/tof_component.py | 12 +++--------- 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/depthai_sdk/requirements.txt b/depthai_sdk/requirements.txt index 01580b840..739860656 100644 --- a/depthai_sdk/requirements.txt +++ b/depthai_sdk/requirements.txt @@ -4,7 +4,7 @@ opencv-contrib-python>4 blobconverter>=1.4.1 pytube>=12.1.0 --extra-index-url https://artifacts.luxonis.com/artifactory/luxonis-python-snapshot-local/ -depthai==2.22.0 +depthai PyTurboJPEG==1.6.4 marshmallow==3.17.0 xmltodict diff --git a/depthai_sdk/src/depthai_sdk/components/tof_component.py b/depthai_sdk/src/depthai_sdk/components/tof_component.py index ded0c3ce5..0b6851799 100644 --- a/depthai_sdk/src/depthai_sdk/components/tof_component.py +++ b/depthai_sdk/src/depthai_sdk/components/tof_component.py @@ -3,10 +3,11 @@ import depthai as dai from depthai_sdk.components.component import Component, ComponentOutput +from depthai_sdk.components.parser import parse_camera_socket from depthai_sdk.oak_outputs.xout.xout_base import StreamXout, XoutBase -from depthai_sdk.oak_outputs.xout.xout_frames import XoutFrames from depthai_sdk.oak_outputs.xout.xout_depth import XoutDisparityDepth -from depthai_sdk.components.parser import parse_camera_socket +from depthai_sdk.oak_outputs.xout.xout_frames import XoutFrames + class ToFComponent(Component): def __init__(self, @@ -34,13 +35,6 @@ def __init__(self, self.node = pipeline.create(dai.node.ToF) self.camera_node.raw.link(self.node.input) - tofConfig = self.node.initialConfig.get() - # tofConfig.depthParams.freqModUsed = dai.RawToFConfig.DepthParams.TypeFMod.MIN - tofConfig.depthParams.freqModUsed = dai.RawToFConfig.DepthParams.TypeFMod.MAX - tofConfig.depthParams.avgPhaseShuffle = False - tofConfig.depthParams.minimumAmplitude = 3.0 - self.node.initialConfig.set(tofConfig) - def _find_tof(self, device: dai.Device) -> dai.CameraBoardSocket: # Use the first ToF sensor, usually, there will only be one features = device.getConnectedCameraFeatures() From dd1a6d8a797107d24b9b91b7b63c3fcffb932712 Mon Sep 17 00:00:00 2001 From: zrezke Date: Sat, 27 Apr 2024 20:21:40 +0200 Subject: [PATCH 14/16] Added tof control --- .../depthai_sdk/components/tof_component.py | 41 ++++++++++++------- .../src/depthai_sdk/components/tof_control.py | 39 ++++++++++++++++++ 2 files changed, 65 insertions(+), 15 deletions(-) create mode 100644 depthai_sdk/src/depthai_sdk/components/tof_control.py diff --git a/depthai_sdk/src/depthai_sdk/components/tof_component.py b/depthai_sdk/src/depthai_sdk/components/tof_component.py index 0b6851799..1dfe03c2f 100644 --- a/depthai_sdk/src/depthai_sdk/components/tof_component.py +++ b/depthai_sdk/src/depthai_sdk/components/tof_component.py @@ -7,14 +7,16 @@ from depthai_sdk.oak_outputs.xout.xout_base import StreamXout, XoutBase from depthai_sdk.oak_outputs.xout.xout_depth import XoutDisparityDepth from depthai_sdk.oak_outputs.xout.xout_frames import XoutFrames +from depthai_sdk.components.tof_control import ToFControl class ToFComponent(Component): - def __init__(self, - device: dai.Device, - pipeline: dai.Pipeline, - source: Union[str, dai.CameraBoardSocket, None] = None, - ): + def __init__( + self, + device: dai.Device, + pipeline: dai.Pipeline, + source: Union[str, dai.CameraBoardSocket, None] = None, + ): super().__init__() self.out = self.Out(self) self._pipeline = pipeline @@ -24,16 +26,20 @@ def __init__(self, elif isinstance(source, str): source = parse_camera_socket(source) elif isinstance(source, dai.CameraBoardSocket): - pass # This is what we want + pass # This is what we want else: - raise ValueError('source must be either None, str, or CameraBoardSocket!') + raise ValueError("source must be either None, str, or CameraBoardSocket!") + self.control = ToFControl(device) self.camera_node = pipeline.create(dai.node.ColorCamera) self.camera_node.setBoardSocket(source) self.camera_socket = source self.node = pipeline.create(dai.node.ToF) + self._control_in = pipeline.create(dai.node.XLinkIn) self.camera_node.raw.link(self.node.input) + self._control_in.setStreamName(f"{self.node.id}_inputControl") + self._control_in.out.link(self.node.inputConfig) def _find_tof(self, device: dai.Device) -> dai.CameraBoardSocket: # Use the first ToF sensor, usually, there will only be one @@ -41,7 +47,14 @@ def _find_tof(self, device: dai.Device) -> dai.CameraBoardSocket: for cam_sensor in features: if dai.CameraSensorType.TOF in cam_sensor.supportedTypes: return cam_sensor.socket - raise RuntimeError(f'No ToF sensor found on this camera! {device.getConnectedCameraFeatures()}') + raise RuntimeError( + f"No ToF sensor found on this camera! {device.getConnectedCameraFeatures()}" + ) + + def on_pipeline_started(self, device: dai.Device) -> None: + self.control.set_input_queue( + device.getInputQueue(self._control_in.getStreamName()) + ) class Out: @@ -52,24 +65,22 @@ def __call__(self, device: dai.Device) -> XoutBase: frames=StreamXout(self._comp.node.depth, "tof_depth"), dispScaleFactor=9500, mono_frames=None, - ir_settings={ - "auto_mode": False - } - ).set_comp_out(self) + ir_settings={"auto_mode": False}, + ).set_comp_out(self) class AmplitudeOut(ComponentOutput): def __call__(self, device: dai.Device) -> XoutBase: return XoutFrames( frames=StreamXout(self._comp.node.amplitude, "tof_amplitude") - ).set_comp_out(self) + ).set_comp_out(self) class ErrorOut(ComponentOutput): def __call__(self, device: dai.Device) -> XoutBase: return XoutFrames( frames=StreamXout(self._comp.node.error, "tof_error") - ).set_comp_out(self) + ).set_comp_out(self) - def __init__(self, tof_component: 'ToFComponent'): + def __init__(self, tof_component: "ToFComponent"): self.depth = self.DepthOut(tof_component) self.amplitude = self.AmplitudeOut(tof_component) self.error = self.ErrorOut(tof_component) diff --git a/depthai_sdk/src/depthai_sdk/components/tof_control.py b/depthai_sdk/src/depthai_sdk/components/tof_control.py new file mode 100644 index 000000000..d7797cd13 --- /dev/null +++ b/depthai_sdk/src/depthai_sdk/components/tof_control.py @@ -0,0 +1,39 @@ +import depthai as dai +from itertools import cycle +import logging + +logger = logging.getLogger(__name__) + +LIMITS = { + "confidence_threshold": (0, 255), + "bilateral_sigma": (0, 255), + "range": (0, 65535), + "lrc_threshold": (0, 10), + "dot_projector": (0, 1200), + "illumination_led": (0, 1500), +} + + +def clamp(value, min_value, max_value): + return max(min(value, max_value), min_value) + + +class ToFControl: + def __init__(self, device: dai.Device): + self.queue = None + ctrl = dai.StereoDepthConfig() + self.raw_cfg = ctrl.get() + + def set_input_queue(self, queue: dai.DataInputQueue): + self.queue = queue + + def send_controls(self, tof_control: dai.RawToFConfig): + """ + Send controls to the ToF node. + """ + if self.queue is None: + logger.error("Cannot send controls when replaying.") + return + + logger.info(f"Sending controls to ToF node: {tof_control}") + self.queue.send(tof_control) From 91c455c9a6a3c3476cf33fd2680a2b79af25297f Mon Sep 17 00:00:00 2001 From: zrezke Date: Mon, 27 May 2024 10:21:19 +0200 Subject: [PATCH 15/16] Added ToF alignment, ideally would add sync node too --- .../depthai_sdk/components/tof_component.py | 30 +++++++++++++++++-- depthai_sdk/src/depthai_sdk/oak_camera.py | 4 +-- 2 files changed, 30 insertions(+), 4 deletions(-) diff --git a/depthai_sdk/src/depthai_sdk/components/tof_component.py b/depthai_sdk/src/depthai_sdk/components/tof_component.py index 1dfe03c2f..177e8a3d0 100644 --- a/depthai_sdk/src/depthai_sdk/components/tof_component.py +++ b/depthai_sdk/src/depthai_sdk/components/tof_component.py @@ -1,8 +1,9 @@ -from typing import List, Union +from typing import List, Optional, Union import depthai as dai from depthai_sdk.components.component import Component, ComponentOutput +from depthai_sdk.components.camera_component import CameraComponent from depthai_sdk.components.parser import parse_camera_socket from depthai_sdk.oak_outputs.xout.xout_base import StreamXout, XoutBase from depthai_sdk.oak_outputs.xout.xout_depth import XoutDisparityDepth @@ -11,11 +12,15 @@ class ToFComponent(Component): + + _align: Optional[dai.node.ImageAlign] = None + def __init__( self, device: dai.Device, pipeline: dai.Pipeline, source: Union[str, dai.CameraBoardSocket, None] = None, + align_to: Optional[CameraComponent] = None, ): super().__init__() self.out = self.Out(self) @@ -41,6 +46,11 @@ def __init__( self._control_in.setStreamName(f"{self.node.id}_inputControl") self._control_in.out.link(self.node.inputConfig) + if align_to is not None: + self._align = pipeline.create(dai.node.ImageAlign) + self.node.depth.link(self._align.input) + align_to.node.isp.link(self._align.inputAlignTo) + def _find_tof(self, device: dai.Device) -> dai.CameraBoardSocket: # Use the first ToF sensor, usually, there will only be one features = device.getConnectedCameraFeatures() @@ -51,6 +61,15 @@ def _find_tof(self, device: dai.Device) -> dai.CameraBoardSocket: f"No ToF sensor found on this camera! {device.getConnectedCameraFeatures()}" ) + def set_align_to(self, align_to: CameraComponent) -> None: + if self._align is None: + self._align = self._pipeline.create(dai.node.ImageAlign) + self.node.depth.link(self._align.input) + if align_to.is_mono(): + align_to.node.out.link(self._align.inputAlignTo) + else: + align_to.node.isp.link(self._align.inputAlignTo) + def on_pipeline_started(self, device: dai.Device) -> None: self.control.set_input_queue( device.getInputQueue(self._control_in.getStreamName()) @@ -62,7 +81,14 @@ class DepthOut(ComponentOutput): def __call__(self, device: dai.Device) -> XoutBase: return XoutDisparityDepth( device=device, - frames=StreamXout(self._comp.node.depth, "tof_depth"), + frames=StreamXout( + ( + self._comp.node.depth + if self._comp._align is None + else self._comp._align.outputAligned + ), + "tof_depth", + ), dispScaleFactor=9500, mono_frames=None, ir_settings={"auto_mode": False}, diff --git a/depthai_sdk/src/depthai_sdk/oak_camera.py b/depthai_sdk/src/depthai_sdk/oak_camera.py index 102e19872..f7e602800 100644 --- a/depthai_sdk/src/depthai_sdk/oak_camera.py +++ b/depthai_sdk/src/depthai_sdk/oak_camera.py @@ -390,14 +390,14 @@ def create_stereo(self, """ return self.stereo(resolution, fps, left, right, encode) - def create_tof(self, source: Union[str, dai.CameraBoardSocket, None] = None) -> ToFComponent: + def create_tof(self, source: Union[str, dai.CameraBoardSocket, None] = None, align_to: Optional[CameraComponent] = None) -> ToFComponent: """ Create ToF component. Args: source (str / dai.CameraBoardSocket): ToF camera source """ - comp = ToFComponent(self.device, self.pipeline, source) + comp = ToFComponent(self.device, self.pipeline, source, align_to) self._components.append(comp) return comp From 9c7f6e4d01001d666bd0c90481041727b55a704d Mon Sep 17 00:00:00 2001 From: Erol444 Date: Fri, 31 May 2024 17:05:47 +0200 Subject: [PATCH 16/16] Added align_frame to packets, added aligned_to to tof packet (disparityDepth). Created tof align with color example --- .../examples/ToFComponent/tof_align.py | 19 +++++++++++++++++++ .../src/depthai_sdk/classes/packets.py | 16 ++++++++-------- .../depthai_sdk/components/camera_helper.py | 1 + .../components/stereo_component.py | 14 +++++++------- .../depthai_sdk/components/tof_component.py | 5 +++-- .../oak_outputs/xout/xout_depth.py | 8 ++++---- .../oak_outputs/xout/xout_disparity.py | 16 ++++++++-------- 7 files changed, 50 insertions(+), 29 deletions(-) create mode 100644 depthai_sdk/examples/ToFComponent/tof_align.py diff --git a/depthai_sdk/examples/ToFComponent/tof_align.py b/depthai_sdk/examples/ToFComponent/tof_align.py new file mode 100644 index 000000000..2f876c988 --- /dev/null +++ b/depthai_sdk/examples/ToFComponent/tof_align.py @@ -0,0 +1,19 @@ +from depthai_sdk import OakCamera +from depthai_sdk.classes.packets import DisparityDepthPacket +import cv2 +from depthai_sdk.visualize.visualizer import Visualizer + +with OakCamera() as oak: + cam_c = oak.create_camera('CAM_C') + tof = oak.create_tof("CAM_A", align_to=cam_c) + depth_q = oak.queue(tof.out.depth).queue + + vis = Visualizer() # Only for depth colorization + oak.start() + while oak.running(): + depth: DisparityDepthPacket = depth_q.get() + colored_depth = depth.get_colorized_frame(vis) + cv2.imshow("depth", colored_depth) + cv2.imshow('Weighted', cv2.addWeighted(depth.aligned_frame.getCvFrame(), 0.5, colored_depth, 0.5, 0)) + if cv2.waitKey(1) == ord('q'): + break diff --git a/depthai_sdk/src/depthai_sdk/classes/packets.py b/depthai_sdk/src/depthai_sdk/classes/packets.py index b1b0d8850..2932ab234 100644 --- a/depthai_sdk/src/depthai_sdk/classes/packets.py +++ b/depthai_sdk/src/depthai_sdk/classes/packets.py @@ -119,14 +119,14 @@ def __init__(self, disparity_map: Optional[np.ndarray] = None, colorize: StereoColor = None, colormap: int = None, - mono_frame: Optional[dai.ImgFrame] = None, + aligned_frame: Optional[dai.ImgFrame] = None, confidence_map: Optional[np.ndarray] = None ): """ disparity_map might be filtered, eg. if WLS filter is enabled """ super().__init__(name=name, msg=img) - self.mono_frame = mono_frame + self.aligned_frame = aligned_frame self.disparity_map = disparity_map self.multiplier = multiplier self.colorize = colorize @@ -150,9 +150,9 @@ def get_colorized_frame(self, visualizer) -> np.ndarray: colorized_disp = frame * self.multiplier try: - mono_frame = self.mono_frame.getCvFrame() + aligned_frame = self.aligned_frame.getCvFrame() except AttributeError: - mono_frame = None + aligned_frame = None stereo_config = visualizer.config.stereo @@ -163,7 +163,7 @@ def get_colorized_frame(self, visualizer) -> np.ndarray: colormap = stereo_config.colormap colormap[0] = [0, 0, 0] # Invalidate pixels 0 to be black - if mono_frame is not None and colorized_disp.ndim == 2 and mono_frame.ndim == 3: + if aligned_frame is not None and colorized_disp.ndim == 2 and aligned_frame.ndim == 3: colorized_disp = colorized_disp[..., np.newaxis] if colorize == StereoColor.GRAY: @@ -172,7 +172,7 @@ def get_colorized_frame(self, visualizer) -> np.ndarray: colorized_disp = cv2.applyColorMap(colorized_disp.astype(np.uint8), colormap) elif colorize == StereoColor.RGBD: colorized_disp = cv2.applyColorMap( - (colorized_disp + mono_frame * 0.5).astype(np.uint8), colormap + (colorized_disp + aligned_frame * 0.5).astype(np.uint8), colormap ) return colorized_disp @@ -190,7 +190,7 @@ def __init__(self, img_frame: dai.ImgFrame, colorize: StereoColor = None, colormap: int = None, - mono_frame: Optional[dai.ImgFrame] = None, + aligned_frame: Optional[dai.ImgFrame] = None, disp_scale_factor=255 / 95, confidence_map=None ): @@ -202,7 +202,7 @@ def __init__(self, multiplier=255 / 95, colorize=colorize, colormap=colormap, - mono_frame=mono_frame, + aligned_frame=aligned_frame, confidence_map=confidence_map ) self.disp_scale_factor = disp_scale_factor diff --git a/depthai_sdk/src/depthai_sdk/components/camera_helper.py b/depthai_sdk/src/depthai_sdk/components/camera_helper.py index 1f4215084..826d9ef4e 100644 --- a/depthai_sdk/src/depthai_sdk/components/camera_helper.py +++ b/depthai_sdk/src/depthai_sdk/components/camera_helper.py @@ -221,6 +221,7 @@ def getClosesResolution(sensor: dai.CameraFeatures, desired, i = (width, 0) if width is not None else (height, 1) resolutions = [get_sensor_resolution(type, conf.width, conf.height) for conf in sensor.configs if conf.type == type] + resolutions = [res for res in resolutions if res is not None] for (res, size) in resolutions: err = abs(size[i] - desired) diff --git a/depthai_sdk/src/depthai_sdk/components/stereo_component.py b/depthai_sdk/src/depthai_sdk/components/stereo_component.py index 33f7145f2..1afff2683 100644 --- a/depthai_sdk/src/depthai_sdk/components/stereo_component.py +++ b/depthai_sdk/src/depthai_sdk/components/stereo_component.py @@ -450,14 +450,14 @@ def get_fourcc(self) -> Optional[str]: Available outputs (to the host) of this component """ - def _mono_frames(self): + def _aligned_frames(self): """ - Create mono frames output if WLS filter is enabled or colorize is set to RGBD + Create aligned frames output if WLS filter is enabled or colorize is set to RGBD """ - mono_frames = None + aligned_frame = None if self.wls_config['enabled'] or self._colorize == StereoColor.RGBD: - mono_frames = StreamXout(self._right_stream) - return mono_frames + aligned_frame = StreamXout(self._right_stream) + return aligned_frame def _try_get_confidence_map(self): confidence_map = None @@ -472,7 +472,7 @@ def __call__(self, device: dai.Device) -> XoutBase: device=device, frames=StreamXout(self._comp.depth), dispScaleFactor=depth_to_disp_factor(device, self._comp.node), - mono_frames=self._comp._mono_frames(), + aligned_frame=self._comp._aligned_frames(), colorize=self._comp._colorize, colormap=self._comp._postprocess_colormap, ir_settings=self._comp.ir_settings, @@ -487,7 +487,7 @@ def __call__(self, device: dai.Device, fourcc: Optional[str] = None) -> XoutBase frames=StreamXout(self._comp.encoder.bitstream) if fourcc else StreamXout(self._comp.disparity), disp_factor=255.0 / self._comp.node.getMaxDisparity(), - mono_frames=self._comp._mono_frames(), + aligned_frame=self._comp._aligned_frames(), colorize=self._comp._colorize, fourcc=fourcc, colormap=self._comp._postprocess_colormap, diff --git a/depthai_sdk/src/depthai_sdk/components/tof_component.py b/depthai_sdk/src/depthai_sdk/components/tof_component.py index 177e8a3d0..48c270de8 100644 --- a/depthai_sdk/src/depthai_sdk/components/tof_component.py +++ b/depthai_sdk/src/depthai_sdk/components/tof_component.py @@ -48,8 +48,9 @@ def __init__( if align_to is not None: self._align = pipeline.create(dai.node.ImageAlign) + self._align_to_output = align_to.node.isp self.node.depth.link(self._align.input) - align_to.node.isp.link(self._align.inputAlignTo) + self._align_to_output.link(self._align.inputAlignTo) def _find_tof(self, device: dai.Device) -> dai.CameraBoardSocket: # Use the first ToF sensor, usually, there will only be one @@ -89,8 +90,8 @@ def __call__(self, device: dai.Device) -> XoutBase: ), "tof_depth", ), + aligned_frame=StreamXout(self._comp._align_to_output, "aligned_stream") if self._comp._align else None, dispScaleFactor=9500, - mono_frames=None, ir_settings={"auto_mode": False}, ).set_comp_out(self) diff --git a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py index 059642ed4..d418735a3 100644 --- a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py +++ b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_depth.py @@ -13,7 +13,7 @@ def __init__(self, device: dai.Device, frames: StreamXout, dispScaleFactor: float, - mono_frames: Optional[StreamXout], + aligned_frame: Optional[StreamXout], colorize: StereoColor = None, colormap: int = None, ir_settings: dict = None, @@ -22,7 +22,7 @@ def __init__(self, super().__init__(device=device, frames=frames, disp_factor=255 / 95, - mono_frames=mono_frames, + aligned_frame=aligned_frame, colorize=colorize, colormap=colormap, ir_settings=ir_settings, @@ -31,14 +31,14 @@ def __init__(self, self.disp_scale_factor = dispScaleFactor def package(self, msgs: Dict) -> DisparityDepthPacket: - mono_frame = msgs[self.mono_frames.name] if self.mono_frames else None + aligned_frame = msgs[self.aligned_frame.name] if self.aligned_frame else None confidence_map = msgs[self.confidence_map.name] if self.confidence_map else None return DisparityDepthPacket( self.get_packet_name(), msgs[self.frames.name], colorize=self.colorize, colormap=self.colormap, - mono_frame=mono_frame, + aligned_frame=aligned_frame, disp_scale_factor=self.disp_scale_factor, confidence_map=confidence_map ) diff --git a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py index 16a94b1d6..52decaea5 100644 --- a/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py +++ b/depthai_sdk/src/depthai_sdk/oak_outputs/xout/xout_disparity.py @@ -30,14 +30,14 @@ def __init__(self, device: dai.Device, frames: StreamXout, disp_factor: float, - mono_frames: Optional[StreamXout], + aligned_frame: Optional[StreamXout], fourcc: str = None, colorize: StereoColor = None, colormap: int = None, wls_config: dict = None, ir_settings: dict = None, confidence_map: StreamXout = None): - self.mono_frames = mono_frames + self.aligned_frame = aligned_frame self.name = 'Disparity' self.multiplier = disp_factor self.device = device @@ -96,8 +96,8 @@ def on_callback(self, packet) -> None: def xstreams(self) -> List[StreamXout]: streams = [self.frames] - if self.mono_frames is not None: - streams.append(self.mono_frames) + if self.aligned_frame is not None: + streams.append(self.aligned_frame) if self.confidence_map is not None: streams.append(self.confidence_map) @@ -105,7 +105,7 @@ def xstreams(self) -> List[StreamXout]: def package(self, msgs: Dict) -> DisparityPacket: img_frame = msgs[self.frames.name] - mono_frame = msgs[self.mono_frames.name] if self.mono_frames else None + aligned_frame = msgs[self.aligned_frame.name] if self.aligned_frame else None confidence_map = msgs[self.confidence_map.name] if self.confidence_map else None # TODO: refactor the mess below packet = DisparityPacket( @@ -116,7 +116,7 @@ def package(self, msgs: Dict) -> DisparityPacket: colorize=self.colorize, colormap=self.colormap, confidence_map=confidence_map, - mono_frame=mono_frame, + aligned_frame=aligned_frame, ) packet._get_codec = self.get_codec @@ -127,10 +127,10 @@ def package(self, msgs: Dict) -> DisparityPacket: if disparity_frame is None: return None - if mono_frame and self.use_wls_filter: + if aligned_frame and self.use_wls_filter: # Perform WLS filtering # If we have wls enabled, it means CV2 is installed - disparity_frame = self.wls_filter.filter(disparity_frame, mono_frame.getCvFrame()) + disparity_frame = self.wls_filter.filter(disparity_frame, aligned_frame.getCvFrame()) packet.disparity_map = disparity_frame