From 29ca4797b267f9db087b451a79d0b81c7e26a65c Mon Sep 17 00:00:00 2001 From: Erol444 Date: Tue, 25 Jun 2024 20:49:01 +0200 Subject: [PATCH 1/4] Added ToF configuration function, support ToF depth from pointcloud, pin numpy below 2.0, depthai-sdk to 1.15 --- depthai_sdk/requirements.txt | 4 +- depthai_sdk/setup.py | 2 +- .../src/depthai_sdk/classes/packets.py | 9 ++++ .../depthai_sdk/components/nn_component.py | 5 ++- .../components/pointcloud_component.py | 29 +++++++------ .../depthai_sdk/components/tof_component.py | 41 +++++++++++++++---- depthai_sdk/src/depthai_sdk/oak_camera.py | 13 ++++-- 7 files changed, 73 insertions(+), 30 deletions(-) diff --git a/depthai_sdk/requirements.txt b/depthai_sdk/requirements.txt index 739860656..0d6cdcd64 100644 --- a/depthai_sdk/requirements.txt +++ b/depthai_sdk/requirements.txt @@ -1,5 +1,5 @@ -numpy>=1.19; python_version < "3.7" -numpy>=1.21; python_version >= "3.7" +numpy>=1.19,<2.0.0; python_version < "3.7" +numpy>=1.21,<2.0.0; python_version >= "3.7" opencv-contrib-python>4 blobconverter>=1.4.1 pytube>=12.1.0 diff --git a/depthai_sdk/setup.py b/depthai_sdk/setup.py index 6df050633..c240f6d7e 100644 --- a/depthai_sdk/setup.py +++ b/depthai_sdk/setup.py @@ -9,7 +9,7 @@ setup( name='depthai-sdk', - version='1.13.1', + version='1.15.0', description='This package provides an abstraction of the DepthAI API library.', long_description=io.open("README.md", encoding="utf-8").read(), long_description_content_type="text/markdown", diff --git a/depthai_sdk/src/depthai_sdk/classes/packets.py b/depthai_sdk/src/depthai_sdk/classes/packets.py index 2932ab234..7bdc5ee21 100644 --- a/depthai_sdk/src/depthai_sdk/classes/packets.py +++ b/depthai_sdk/src/depthai_sdk/classes/packets.py @@ -234,6 +234,15 @@ def get_sequence_num(self) -> int: def get_timestamp(self) -> timedelta: return self.depth_map.getTimestampDevice() + def crop_points(self, bb: BoundingBox) -> np.ndarray: + """ + Crop points to the bounding box + + Returns: Cropped section of the point cloud + """ + x1, y1, x2, y2 = bb.to_tuple(self.points.shape) + return self.points[y1:y2, x1:x2] + class SpatialBbMappingPacket(DisparityDepthPacket): """ diff --git a/depthai_sdk/src/depthai_sdk/components/nn_component.py b/depthai_sdk/src/depthai_sdk/components/nn_component.py index bc8e378e2..2341fd839 100644 --- a/depthai_sdk/src/depthai_sdk/components/nn_component.py +++ b/depthai_sdk/src/depthai_sdk/components/nn_component.py @@ -551,7 +551,10 @@ def config_nn(self, self._change_resize_mode(self._ar_resize_mode) if conf_threshold is not None and self.is_detector(): - self.node.setConfidenceThreshold(conf_threshold) + if 0 <= conf_threshold <= 1: + self.node.setConfidenceThreshold(conf_threshold) + else: + raise ValueError("Confidence threshold must be between 0 and 1!") def config_spatial(self, bb_scale_factor: Optional[float] = None, diff --git a/depthai_sdk/src/depthai_sdk/components/pointcloud_component.py b/depthai_sdk/src/depthai_sdk/components/pointcloud_component.py index 7e806ffc7..0d4e6161e 100644 --- a/depthai_sdk/src/depthai_sdk/components/pointcloud_component.py +++ b/depthai_sdk/src/depthai_sdk/components/pointcloud_component.py @@ -5,6 +5,7 @@ from depthai_sdk.components.camera_component import CameraComponent from depthai_sdk.components.component import Component, ComponentOutput from depthai_sdk.components.stereo_component import StereoComponent +from depthai_sdk.components.tof_component import ToFComponent from depthai_sdk.oak_outputs.xout.xout_base import XoutBase, StreamXout from depthai_sdk.oak_outputs.xout.xout_pointcloud import XoutPointcloud from depthai_sdk.replay import Replay @@ -15,7 +16,7 @@ class PointcloudComponent(Component): def __init__(self, device: dai.Device, pipeline: dai.Pipeline, - stereo: Union[None, StereoComponent, dai.node.StereoDepth, dai.Node.Output] = None, + depth_input: Union[None, StereoComponent, ToFComponent, dai.node.StereoDepth, dai.Node.Output] = None, colorize: Optional[CameraComponent] = None, replay: Optional[Replay] = None, args: Any = None): @@ -28,15 +29,14 @@ def __init__(self, super().__init__() self.out = self.Out(self) - self.stereo_depth_node: dai.node.StereoDepth - self.depth: dai.Node.Output # Depth node output + self.depth: dai.Node.Output # depth output self.colorize_comp: Optional[CameraComponent] = colorize self._replay: Optional[Replay] = replay # Depth aspect - if stereo is None: + if depth_input is None: stereo = StereoComponent(device, pipeline, replay=replay, args=args) stereo.config_stereo(lr_check=True, subpixel=True, subpixel_bits=3, confidence=230) stereo.node.initialConfig.setNumInvalidateEdgePixels(20) @@ -58,15 +58,18 @@ def __init__(self, # Align to colorize node stereo.config_stereo(align=self.colorize_comp) - if isinstance(stereo, StereoComponent): - stereo = stereo.node - - if isinstance(stereo, dai.node.StereoDepth): - self.stereo_depth_node = stereo - self.depth = stereo.depth - elif isinstance(stereo, dai.Node.Output): - self.stereo_depth_node = stereo.getParent() - self.depth = stereo + if isinstance(depth_input, StereoComponent): + depth_input = stereo.node + elif isinstance(depth_input, ToFComponent): + if depth_input._align is not None: + self.depth = depth_input._align.outputAligned + else: + self.depth = depth_input.node.depth + + if isinstance(depth_input, dai.node.StereoDepth): + self.depth = depth_input.depth + elif isinstance(depth_input, dai.Node.Output): + self.depth = depth_input def config_postprocessing(self) -> None: """ diff --git a/depthai_sdk/src/depthai_sdk/components/tof_component.py b/depthai_sdk/src/depthai_sdk/components/tof_component.py index 48c270de8..c014c74a0 100644 --- a/depthai_sdk/src/depthai_sdk/components/tof_component.py +++ b/depthai_sdk/src/depthai_sdk/components/tof_component.py @@ -1,4 +1,4 @@ -from typing import List, Optional, Union +from typing import List, Optional, Union, Tuple import depthai as dai @@ -21,6 +21,7 @@ def __init__( pipeline: dai.Pipeline, source: Union[str, dai.CameraBoardSocket, None] = None, align_to: Optional[CameraComponent] = None, + fps: Optional[int] = None, ): super().__init__() self.out = self.Out(self) @@ -40,17 +41,19 @@ def __init__( self.camera_node.setBoardSocket(source) self.camera_socket = source - self.node = pipeline.create(dai.node.ToF) + if fps is not None: + self.camera_node.setFps(fps) + + self.node: dai.node.ToF = 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) + self._align_to_output: dai.Node.Output = None + 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) - self._align_to_output.link(self._align.inputAlignTo) + self.set_align_to(align_to) def _find_tof(self, device: dai.Device) -> dai.CameraBoardSocket: # Use the first ToF sensor, usually, there will only be one @@ -62,14 +65,34 @@ 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: + def set_align_to(self, align_to: CameraComponent, output_size: Optional[Tuple] = None) -> 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) + self._align_to_output = align_to.node.out else: - align_to.node.isp.link(self._align.inputAlignTo) + self._align_to_output = align_to.node.isp + + self._align_to_output.link(self._align.inputAlignTo) + + if output_size is not None: + self._align.setOutputSize(*output_size) + + + def configure_tof(self, + phaseShuffleTemporalFilter: bool = None, + phaseUnwrappingLevel: int = None, + phaseUnwrapErrorThreshold: int = None, + ) -> None: + tofConfig = self.node.initialConfig.get() + if phaseShuffleTemporalFilter is not None: + tofConfig.enablePhaseShuffleTemporalFilter = phaseShuffleTemporalFilter + if phaseUnwrappingLevel is not None: + tofConfig.phaseUnwrappingLevel = phaseUnwrappingLevel + if phaseUnwrapErrorThreshold is not None: + tofConfig.phaseUnwrapErrorThreshold = phaseUnwrapErrorThreshold + self.node.initialConfig.set(tofConfig) def on_pipeline_started(self, device: dai.Device) -> None: self.control.set_input_queue( diff --git a/depthai_sdk/src/depthai_sdk/oak_camera.py b/depthai_sdk/src/depthai_sdk/oak_camera.py index 8d5090ec7..ec0b58c07 100644 --- a/depthai_sdk/src/depthai_sdk/oak_camera.py +++ b/depthai_sdk/src/depthai_sdk/oak_camera.py @@ -390,14 +390,19 @@ def create_stereo(self, """ return self.stereo(resolution, fps, left, right, encode) - def create_tof(self, source: Union[str, dai.CameraBoardSocket, None] = None, align_to: Optional[CameraComponent] = None) -> ToFComponent: + def create_tof(self, + source: Union[str, dai.CameraBoardSocket, None] = None, + fps: Optional[float] = None, + align_to: Optional[CameraComponent] = None) -> ToFComponent: """ Create ToF component. Args: source (str / dai.CameraBoardSocket): ToF camera source + fps (float): Sensor FPS + align_to (CameraComponent): Align ToF to this camera component """ - comp = ToFComponent(self.device, self.pipeline, source, align_to) + comp = ToFComponent(self.device, self.pipeline, source, align_to, fps) self._components.append(comp) return comp @@ -410,7 +415,7 @@ def create_imu(self) -> IMUComponent: return comp def create_pointcloud(self, - stereo: Union[None, StereoComponent, dai.node.StereoDepth, dai.Node.Output] = None, + depth_input: Union[None, StereoComponent, ToFComponent, dai.node.StereoDepth, dai.Node.Output] = None, colorize: Union[None, CameraComponent, dai.node.MonoCamera, dai.node.ColorCamera, dai.Node.Output, bool] = None, ) -> PointcloudComponent: @@ -427,7 +432,7 @@ def create_pointcloud(self, comp = PointcloudComponent( self.device, self.pipeline, - stereo=stereo, + depth_input=depth_input, colorize=colorize, replay=self.replay, args=self._args, From 7c91da65dbdd53a9c2ec3192c60164d2e4fa37f0 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Thu, 27 Jun 2024 12:59:20 +0200 Subject: [PATCH 2/4] Fix pointcloud component --- .../examples/PointcloudComponent/pointcloud.py | 2 +- depthai_sdk/setup.py | 4 +++- depthai_sdk/src/depthai_sdk/__init__.py | 2 +- .../depthai_sdk/components/pointcloud_component.py | 14 +++++++------- .../visualize/visualizers/viewer_visualizer.py | 8 +++----- 5 files changed, 15 insertions(+), 15 deletions(-) diff --git a/depthai_sdk/examples/PointcloudComponent/pointcloud.py b/depthai_sdk/examples/PointcloudComponent/pointcloud.py index c746a11e3..5e33c1a5a 100644 --- a/depthai_sdk/examples/PointcloudComponent/pointcloud.py +++ b/depthai_sdk/examples/PointcloudComponent/pointcloud.py @@ -4,6 +4,6 @@ color = oak.camera('color') stereo = oak.create_stereo() stereo.config_stereo(align=color) - pcl = oak.create_pointcloud(stereo=stereo, colorize=color) + pcl = oak.create_pointcloud(depth_input=stereo, colorize=color) oak.visualize(pcl, visualizer='depthai-viewer') oak.start(blocking=True) diff --git a/depthai_sdk/setup.py b/depthai_sdk/setup.py index c240f6d7e..b30c904c0 100644 --- a/depthai_sdk/setup.py +++ b/depthai_sdk/setup.py @@ -26,7 +26,9 @@ "visualize": ['PySide2', 'Qt.py>=1.3.0', 'matplotlib==3.5.3; python_version <= "3.7"', - 'matplotlib==3.6.1; python_version > "3.7"'], + 'matplotlib==3.6.1; python_version > "3.7"', + 'depthai-viewer' + ], "replay": ['mcap>=0.0.10', 'mcap-ros1-support==0.0.8', 'rosbags==0.9.11'], diff --git a/depthai_sdk/src/depthai_sdk/__init__.py b/depthai_sdk/src/depthai_sdk/__init__.py index 9206f39db..39cbd2eeb 100644 --- a/depthai_sdk/src/depthai_sdk/__init__.py +++ b/depthai_sdk/src/depthai_sdk/__init__.py @@ -10,7 +10,7 @@ from depthai_sdk.utils import _create_config, get_config_field, _sentry_before_send from depthai_sdk.visualize import * -__version__ = '1.13.1' +__version__ = '1.15' def __import_sentry(sentry_dsn: str) -> None: diff --git a/depthai_sdk/src/depthai_sdk/components/pointcloud_component.py b/depthai_sdk/src/depthai_sdk/components/pointcloud_component.py index 0d4e6161e..f22794006 100644 --- a/depthai_sdk/src/depthai_sdk/components/pointcloud_component.py +++ b/depthai_sdk/src/depthai_sdk/components/pointcloud_component.py @@ -37,11 +37,11 @@ def __init__(self, # Depth aspect if depth_input is None: - stereo = StereoComponent(device, pipeline, replay=replay, args=args) - stereo.config_stereo(lr_check=True, subpixel=True, subpixel_bits=3, confidence=230) - stereo.node.initialConfig.setNumInvalidateEdgePixels(20) + depth_input = StereoComponent(device, pipeline, replay=replay, args=args) + depth_input.config_stereo(lr_check=True, subpixel=True, subpixel_bits=3, confidence=230) + depth_input.node.initialConfig.setNumInvalidateEdgePixels(20) - config = stereo.node.initialConfig.get() + config = depth_input.node.initialConfig.get() config.postProcessing.speckleFilter.enable = True config.postProcessing.speckleFilter.speckleRange = 50 config.postProcessing.temporalFilter.enable = True @@ -52,14 +52,14 @@ def __init__(self, config.postProcessing.thresholdFilter.maxRange = 20000 # 20m config.postProcessing.decimationFilter.decimationFactor = 2 config.postProcessing.decimationFilter.decimationMode = dai.RawStereoDepthConfig.PostProcessing.DecimationFilter.DecimationMode.NON_ZERO_MEDIAN - stereo.node.initialConfig.set(config) + depth_input.node.initialConfig.set(config) if self.colorize_comp is not None: # Align to colorize node - stereo.config_stereo(align=self.colorize_comp) + depth_input.config_stereo(align=self.colorize_comp) if isinstance(depth_input, StereoComponent): - depth_input = stereo.node + depth_input = depth_input.node elif isinstance(depth_input, ToFComponent): if depth_input._align is not None: self.depth = depth_input._align.outputAligned diff --git a/depthai_sdk/src/depthai_sdk/visualize/visualizers/viewer_visualizer.py b/depthai_sdk/src/depthai_sdk/visualize/visualizers/viewer_visualizer.py index 2d6ae3d96..8ae013822 100644 --- a/depthai_sdk/src/depthai_sdk/visualize/visualizers/viewer_visualizer.py +++ b/depthai_sdk/src/depthai_sdk/visualize/visualizers/viewer_visualizer.py @@ -29,7 +29,7 @@ def __init__(self, scale, fps): try: # timeout is optional, but it might be good to prevent the script from hanging if the module is large. - process = subprocess.Popen([sys.executable, "-m", "depthai_viewer"], stdout=subprocess.PIPE, + process = subprocess.Popen([sys.executable, "-m", "depthai_viewer", "--viewer-mode"], stdout=subprocess.PIPE, stderr=subprocess.PIPE) stdout, stderr = process.communicate(timeout=3) @@ -65,10 +65,8 @@ def show(self, packet) -> None: viewer.log_imu(*packet.get_imu_vals()) elif type(packet) == PointcloudPacket: if packet.colorize_frame is not None: - bgr_frame = packet.colorize_frame - rgb_frame = bgr_frame[..., ::-1] - frame = np.dstack((rgb_frame, np.full(bgr_frame.shape[:2], 255, dtype=np.uint8))) - viewer.log_image(f'color', frame) + rgb_frame = packet.colorize_frame[..., ::-1] + viewer.log_image(f'color', rgb_frame) viewer.log_points(packet.name, packet.points.reshape(-1, 3) / 1000, colors=rgb_frame.reshape(-1, 3)) else: viewer.log_points(packet.name, packet.points.reshape(-1, 3) / 1000) From d05fd583d78cd40e0ce091a3d867e26b4dfd98c5 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Thu, 27 Jun 2024 13:10:25 +0200 Subject: [PATCH 3/4] Added box estimator to SDK --- .../src/depthai_sdk/classes/__init__.py | 1 + .../src/depthai_sdk/classes/box_estimator.py | 300 ++++++++++++++++++ 2 files changed, 301 insertions(+) create mode 100644 depthai_sdk/src/depthai_sdk/classes/box_estimator.py diff --git a/depthai_sdk/src/depthai_sdk/classes/__init__.py b/depthai_sdk/src/depthai_sdk/classes/__init__.py index 45524ca1e..5824a5771 100644 --- a/depthai_sdk/src/depthai_sdk/classes/__init__.py +++ b/depthai_sdk/src/depthai_sdk/classes/__init__.py @@ -7,3 +7,4 @@ TwoStagePacket, IMUPacket ) +from .box_estimator import BoxEstimator diff --git a/depthai_sdk/src/depthai_sdk/classes/box_estimator.py b/depthai_sdk/src/depthai_sdk/classes/box_estimator.py new file mode 100644 index 000000000..efffc43ca --- /dev/null +++ b/depthai_sdk/src/depthai_sdk/classes/box_estimator.py @@ -0,0 +1,300 @@ +import numpy as np +import cv2 +import random +from typing import Tuple +import open3d as o3d +import json +from depthai_sdk.logger import LOGGER + +N_POINTS_SAMPLED_PLANE = 3 +MAX_ITER_PLANE = 300 + +class BoxEstimator: + def __init__(self, median_window=3, calib_json_path: str = None, threshold=50): + """ + Box estimator helper class. Currently it's applicable for scanning only one box at a time. + + Args: + threshold (int, optional): Distance threshold for plane fitting. Defaults to 50 mm. + """ + self.top_side_pcl = None + + self.ground_plane_eq = None + self.threshold = threshold + + self.height = None + self.width = None + self.length = None + + self.bounding_box = None + self.rotation_matrix = None + self.translate_vector = None + + # Median filter + self.median_window = median_window + self.prev_dimensions = [] + + self.pcd = o3d.geometry.PointCloud() + self.plane_pcd = o3d.geometry.PointCloud() + self.box_pcd = o3d.geometry.PointCloud() + + self.corners = None + + # Try to find plane_eq.json file (where user executed script) + if calib_json_path is None: + calib_json_path = 'plane_eq.json' + + try: + with open(calib_json_path, 'r') as f: + data = json.load(f) + if 'eq' in data: + self.ground_plane_eq = np.array(data['eq'], dtype=np.float64) + else: + raise ValueError("Invalid JSON file") + except FileNotFoundError: + LOGGER.info("No plane_eq.json file found. Please run calibrate() method to calibrate the ground plane.") + + def get_outliers(self, points) -> Tuple[np.ndarray, np.ndarray]: + """ + Get outliers and inliers from a point cloud + """ + A,B,C,D = self.ground_plane_eq + # Calculate the denominator (constant for all points) + denominator = np.sqrt(A**2 + B**2 + C**2) + threshold = 45 # mm + distances = np.abs(A * points[:, 0] + B * points[:, 1] + C * points[:, 2] + D) / denominator + return (points[distances > self.threshold], points[distances <= self.threshold]) + + def is_calibrated(self) -> bool: + return self.ground_plane_eq is not None + + def calibrate(self, points: np.ndarray): + """ + Calibrate the ground plane equation using a point cloud + + Args: + points: Point cloud + + Returns: + None + """ + pcd = o3d.geometry.PointCloud() + pcd.points = o3d.utility.Vector3dVector(points.reshape(-1, 3)) + pcd = pcd.voxel_down_sample(voxel_size=10) + pcd = pcd.remove_statistical_outlier(30, 0.1)[0] + + # Get plane segmentation + plane_eq, plane_inliers = pcd.segment_plane(self.threshold, N_POINTS_SAMPLED_PLANE, MAX_ITER_PLANE) + inlier_points_num = len(plane_inliers) + inline_percentage = inlier_points_num / len(points) + if inline_percentage < 0.8: + LOGGER.error("Not enough inliers found. Please try again.") + return False + + self.ground_plane_eq = plane_eq + with open('plane_eq.json', 'w') as f: + # Convert np.array to list + json.dump({'eq': self.ground_plane_eq.tolist()}, f) + return True + + + def get_plane_mesh(self, size=10, divisions=10) -> Tuple: + """ + Create a mesh representation of a plane given its equation Ax + By + Cz + D = 0. + + Args: + size: Size of the plane (default: 10) + divisions: Number of divisions in each dimension (default: 10) + + Returns: + positions: List of 3D points + indices: List of indices for triangles + normals: List of normal vectors for each vertex + """ + # Normalize the normal vector + A,B,C,D = self.ground_plane_eq + normal = np.array([A, B, C]) + normal = normal / np.linalg.norm(normal) + + # Create a grid of points + x = np.linspace(-size/2, size/2, divisions) + y = np.linspace(-size/2, size/2, divisions) + X, Y = np.meshgrid(x, y) + + # Calculate Z values + if C != 0: + Z = (-A*X - B*Y - D) / C + elif B != 0: + Z = (-A*X - C*Y - D) / B + Y, Z = Z, Y + else: + Z = (-B*Y - C*Z - D) / A + X, Z = Z, X + + # Create positions + positions = np.stack((X, Y, Z), axis=-1).reshape(-1, 3).tolist() + + # Create indices + indices = [] + for i in range(divisions - 1): + for j in range(divisions - 1): + square_start = i * divisions + j + indices.extend([ + square_start, square_start + 1, square_start + divisions, + square_start + 1, square_start + divisions + 1, square_start + divisions + ]) + + # Create normals + normals = [normal.tolist() for _ in range(len(positions))] + + return positions, indices, normals + + def process_points(self, points_roi: np.ndarray) -> Tuple: + self.pcd.points = o3d.utility.Vector3dVector(points_roi) + self.pcd = self.pcd.voxel_down_sample(voxel_size=10) + self.pcd = self.pcd.remove_statistical_outlier(30, 0.1)[0] + + self.box_pcl, self.plane_pcl = self.get_outliers(np.asarray(self.pcd.points)) + + # Remove outliers + self.plane_pcd = self.plane_pcd.remove_statistical_outlier(30, 0.1)[0] + self.box_pcd = self.box_pcd.remove_statistical_outlier(30, 0.1)[0] + + + if len(self.box_pcl) < 100: + return None, None # No box + + self.get_box_top(self.ground_plane_eq) + dimensions = self.get_dimensions() + self.prev_dimensions.append(dimensions) + corners = self.get_3d_corners() + return self._filtered_dimensions(), corners + + @property + def box_pcl(self): + return np.asarray(self.box_pcd.points) + @box_pcl.setter + def box_pcl(self, points): + self.box_pcd.points = o3d.utility.Vector3dVector(points) + @property + def plane_pcl(self): + return np.asarray(self.plane_pcd.points) + @plane_pcl.setter + def plane_pcl(self, points): + self.plane_pcd.points = o3d.utility.Vector3dVector(points) + + def _filtered_dimensions(self): + # If too many dimensions, remove the oldest one + if len(self.prev_dimensions) > self.median_window: + self.prev_dimensions.pop(0) + + # Get median of the dimensions + length = np.median([d[0] for d in self.prev_dimensions]) + width = np.median([d[1] for d in self.prev_dimensions]) + height = np.median([d[2] for d in self.prev_dimensions]) + return length, width, height + + + def create_rotation_matrix(self, vec_in, vec_target): + v = np.cross(vec_in, vec_target) + s = np.linalg.norm(v) + c = np.dot(vec_in, vec_target) + v_mat = np.array([[0, -v[2], v[1]], + [v[2], 0, -v[0]], + [-v[1], v[0], 0]]) + R = np.eye(3) + v_mat + (np.matmul(v_mat, v_mat) * (1 / (1 + c))) + self.rotation_matrix = R + return R + + def get_box_top(self, plane_eq): + rot_matrix = self.create_rotation_matrix(plane_eq[0:3], [0, 0, 1]) + self.plane_pcl = self.rotate_points(self.plane_pcl, rot_matrix) + avg_z = np.average(self.plane_pcl[:, 2]) + + translate_vector = [0, 0, -avg_z] + self.translate_vector = np.array(translate_vector) + + self.plane_pcl = self.translate_points(self.plane_pcl, translate_vector) + self.box_pcl = self.rotate_points(self.box_pcl, rot_matrix) + self.box_pcl = self.translate_points(self.box_pcl, translate_vector) + + top_plane_eq, top_plane_inliers = self.fit_plane_vec_constraint([0, 0, 1], self.box_pcl, 3, 30) + + top_plane = self.box_pcl[top_plane_inliers] + # self.side_planes = self.box_pcl[np.setdiff1d(np.arange(len(self.box_pcl)), top_plane_inliers)] + self.top_side_pcl = top_plane + self.height = abs(top_plane_eq[3]) + return self.height + + def get_dimensions(self): + upper_plane_points = self.top_side_pcl + coordinates = np.c_[upper_plane_points[:, 0], upper_plane_points[:, 1]].astype('float32') + rect = cv2.minAreaRect(coordinates) + self.bounding_box = cv2.boxPoints(rect) + self.width, self.length = rect[1][0], rect[1][1] + + return self.length, self.width, self.height + + def get_3d_lines(self, corners): + lines = [ + [0, 1], [1, 2], [2, 3], [3, 0], # Bottom square + [4, 5], [5, 6], [6, 7], [7, 4], # Top square + [0, 4], [1, 5], [2, 6], [3, 7] # Vertical lines + ] + line_segments = [] + for line in lines: + line_segments.append(corners[line[0]]) + line_segments.append(corners[line[1]]) + return np.array(line_segments) + + def get_3d_corners(self): + bounding_box = self.bounding_box + points_floor = np.c_[bounding_box, np.zeros(4)] + points_top = np.c_[bounding_box, -self.height * np.ones(4)] + box_points = np.concatenate((points_floor, points_top)) + + self.corners = box_points + return box_points + + def inverse_corner_points(self): + # Apply the inverse of the translation and rotation to get back to the original coordinates + inverse_translation = -self.translate_vector + inverse_rot_mat = np.linalg.inv(self.rotation_matrix) + + box_points = self.translate_points(self.corners, inverse_translation) + box_points = self.rotate_points(box_points, inverse_rot_mat) + return box_points + + def fit_plane_vec_constraint(self, norm_vec, pts, thresh=0.05, n_iterations=300): + best_eq = [] + best_inliers = [] + + n_points = pts.shape[0] + for _ in range(n_iterations): + id_sample = random.sample(range(0, n_points), 1) + point = pts[id_sample] + d = -np.sum(np.multiply(norm_vec, point)) + plane_eq = [*norm_vec, d] + pt_id_inliers = self.get_plane_inliers(plane_eq, pts, thresh) + if len(pt_id_inliers) > len(best_inliers): + best_eq = plane_eq + best_inliers = pt_id_inliers + + return best_eq, best_inliers + + def get_plane_inliers(self, plane_eq, pts, thresh=0.05): + dist_pt = self.get_pts_distances_plane(plane_eq, pts) + pt_id_inliers = np.where(np.abs(dist_pt) <= thresh)[0] + return pt_id_inliers + + def get_pts_distances_plane(self, plane_eq, pts): + dist_pt = (plane_eq[0] * pts[:, 0] + plane_eq[1] * pts[:, 1] + + plane_eq[2] * pts[:, 2] + plane_eq[3]) \ + / np.sqrt(plane_eq[0] ** 2 + plane_eq[1] ** 2 + plane_eq[2] ** 2) + return dist_pt + + def rotate_points(self, points, rotation_matrix): + return np.dot(points, rotation_matrix.T) + + def translate_points(self, points, translate_vector): + return points + translate_vector \ No newline at end of file From be87eed0c585aef6b76427c0b42dd61f6f1cd7c9 Mon Sep 17 00:00:00 2001 From: Erol444 Date: Thu, 27 Jun 2024 13:47:53 +0200 Subject: [PATCH 4/4] Added open3d as additional requirement --- depthai_sdk/setup.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/depthai_sdk/setup.py b/depthai_sdk/setup.py index b30c904c0..ce2f809e8 100644 --- a/depthai_sdk/setup.py +++ b/depthai_sdk/setup.py @@ -27,7 +27,8 @@ 'Qt.py>=1.3.0', 'matplotlib==3.5.3; python_version <= "3.7"', 'matplotlib==3.6.1; python_version > "3.7"', - 'depthai-viewer' + 'depthai-viewer', + 'open3d' ], "replay": ['mcap>=0.0.10', 'mcap-ros1-support==0.0.8',