From 412be2331f548558a16dd2e027178243629a63ad Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Thu, 17 Apr 2025 12:03:01 +0200 Subject: [PATCH 1/2] Fix pyrealsense and convert it to uint16 --- .../dora-pyrealsense/dora_pyrealsense/main.py | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py index 940859f2..d0b44feb 100644 --- a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py +++ b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py @@ -36,7 +36,7 @@ def main(): config = rs.config() config.enable_device(device_serial) config.enable_stream(rs.stream.color, image_width, image_height, rs.format.rgb8, 30) - config.enable_stream(rs.stream.depth, image_width, image_height) + config.enable_stream(rs.stream.depth, image_width, image_height, rs.format.z16, 30) align_to = rs.stream.color align = rs.align(align_to) @@ -46,8 +46,8 @@ def main(): depth_sensor = profile.get_device().first_depth_sensor() depth_scale = depth_sensor.get_depth_scale() # rgb_profile = profile.get_stream(rs.stream.color) - # depth_profile = profile.get_stream(rs.stream.depth) - # rgb_intr = rgb_profile.as_video_stream_profile().get_intrinsics() + depth_profile = profile.get_stream(rs.stream.depth) + depth_intr = depth_profile.as_video_stream_profile().get_intrinsics() # rgb_intr = depth_profile.get_extrinsics_to(rgb_profile) node = Node() start_time = time.time() @@ -74,7 +74,7 @@ def main(): continue depth_image = np.asanyarray(aligned_depth_frame.get_data()) - scaled_depth_image = depth_image * depth_scale + scaled_depth_image = depth_image frame = np.asanyarray(color_frame.get_data()) ## Change rgb to bgr @@ -94,6 +94,7 @@ def main(): frame.shape[1] != image_width or frame.shape[0] != image_height ) ): + pass frame = cv2.resize(frame, (image_width, image_height)) metadata = event["metadata"] @@ -112,12 +113,17 @@ def main(): storage = pa.array(frame.ravel()) - # metadata["resolution"] = [int(rgb_intr.width), int(rgb_intr.height)] - # metadata["focal_length"] = [int(rgb_intr.fx), int(rgb_intr.fy)] + metadata["resolution"] = [int(depth_intr.width), int(depth_intr.height)] + metadata["focal_length"] = [int(depth_intr.fx), int(depth_intr.fy)] # metadata["principal_point"] = [int(rgb_intr.ppx), int(rgb_intr.ppy)] + metadata["timestamp"] = time.time_ns() node.send_output("image", storage, metadata) + metadata["encoding"] = "mono16" + scaled_depth_image[scaled_depth_image > 5000] = 0 node.send_output( - "depth", pa.array(scaled_depth_image.ravel()), metadata, + "depth", + pa.array(scaled_depth_image.ravel()), + metadata, ) elif event_type == "ERROR": From f4f14d344714aad0dce0e5728df34e06f29fe329 Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Fri, 18 Apr 2025 11:04:15 +0200 Subject: [PATCH 2/2] Fix resolution parameter and pyrealsense encoding --- .../dora-pyrealsense/dora_pyrealsense/main.py | 23 ++++--------------- node-hub/dora-pyrealsense/pyproject.toml | 12 +++++----- 2 files changed, 11 insertions(+), 24 deletions(-) diff --git a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py index d0b44feb..f30e2a01 100644 --- a/node-hub/dora-pyrealsense/dora_pyrealsense/main.py +++ b/node-hub/dora-pyrealsense/dora_pyrealsense/main.py @@ -43,12 +43,10 @@ def main(): profile = pipeline.start(config) - depth_sensor = profile.get_device().first_depth_sensor() - depth_scale = depth_sensor.get_depth_scale() - # rgb_profile = profile.get_stream(rs.stream.color) + rgb_profile = profile.get_stream(rs.stream.color) depth_profile = profile.get_stream(rs.stream.depth) - depth_intr = depth_profile.as_video_stream_profile().get_intrinsics() - # rgb_intr = depth_profile.get_extrinsics_to(rgb_profile) + _depth_intr = depth_profile.as_video_stream_profile().get_intrinsics() + rgb_intr = rgb_profile.as_video_stream_profile().get_intrinsics() node = Node() start_time = time.time() @@ -86,17 +84,6 @@ def main(): elif flip == "BOTH": frame = cv2.flip(frame, -1) - # resize the frame - if ( - image_width is not None - and image_height is not None - and ( - frame.shape[1] != image_width or frame.shape[0] != image_height - ) - ): - pass - frame = cv2.resize(frame, (image_width, image_height)) - metadata = event["metadata"] metadata["encoding"] = encoding metadata["width"] = int(frame.shape[1]) @@ -113,8 +100,8 @@ def main(): storage = pa.array(frame.ravel()) - metadata["resolution"] = [int(depth_intr.width), int(depth_intr.height)] - metadata["focal_length"] = [int(depth_intr.fx), int(depth_intr.fy)] + metadata["resolution"] = [int(rgb_intr.ppx), int(rgb_intr.ppy)] + metadata["focal_length"] = [int(rgb_intr.fx), int(rgb_intr.fy)] # metadata["principal_point"] = [int(rgb_intr.ppx), int(rgb_intr.ppy)] metadata["timestamp"] = time.time_ns() node.send_output("image", storage, metadata) diff --git a/node-hub/dora-pyrealsense/pyproject.toml b/node-hub/dora-pyrealsense/pyproject.toml index 6465c4b7..b9a572fe 100644 --- a/node-hub/dora-pyrealsense/pyproject.toml +++ b/node-hub/dora-pyrealsense/pyproject.toml @@ -8,12 +8,12 @@ readme = "README.md" requires-python = ">=3.8" dependencies = [ - "dora-rs >= 0.3.9", - "numpy < 2.0.0", - "opencv-python >= 4.1.1", - "pyrealsense2-macosx >= 2.54.2; sys_platform == 'darwin'", - "pyrealsense2 == 2.54.1.5216; sys_platform == 'linux'", - "pyrealsense2 == 2.54.1.5216; sys_platform == 'windows'", + "dora-rs >= 0.3.9", + "numpy < 2.0.0", + "opencv-python >= 4.1.1", + "pyrealsense2-macosx >= 2.54.2; sys_platform == 'darwin'", + "pyrealsense2 == 2.55.1.6486; sys_platform == 'linux'", + "pyrealsense2 == 2.55.1.6486; sys_platform == 'windows'", ] [dependency-groups] dev = ["pytest >=8.1.1", "ruff >=0.9.1"]