From 5444605b02bf0a27127b0f8dfb5054949389e6a2 Mon Sep 17 00:00:00 2001 From: tajgr Date: Wed, 13 Oct 2021 21:57:03 +0200 Subject: [PATCH 01/10] osgar/drivers/realsense.py: rgbd, start implementation --- osgar/drivers/realsense.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/osgar/drivers/realsense.py b/osgar/drivers/realsense.py index 2def64135..c5e970d9b 100644 --- a/osgar/drivers/realsense.py +++ b/osgar/drivers/realsense.py @@ -58,7 +58,6 @@ def __init__(self, config, bus): self.tracking_sensor_yaw_quat = euler_to_quaternion( math.radians(config.get('tracking_sensor_yaw', 0)), 0, 0) elif self.device in ['D400', 'L500']: - bus.register('depth:gz', 'color', 'infra') self.depth_subsample = config.get("depth_subsample", 3) self.depth_rgb = config.get("depth_rgb", False) self.depth_infra = config.get("depth_infra", False) @@ -70,6 +69,13 @@ def __init__(self, config, bus): if self.depth_rgb or self.depth_infra: import cv2 global cv2 + + self.rgbd = config.get("rgbd", False) + if self.rgbd: + assert self + bus.register('rgbd', 'infra') + else: + bus.register('depth:gz', 'color', 'infra') else: g_logger.warning("Device is not specified in the config!") From be621b3dd6c0467156c3735db63b3576e60c914b Mon Sep 17 00:00:00 2001 From: tajgr Date: Thu, 14 Oct 2021 17:41:26 +0200 Subject: [PATCH 02/10] osgar/drivers/realsense.py: rgbd image with help of realsense align, enable png format for images --- osgar/drivers/realsense.py | 36 ++++++++++++++++++++++++++---------- 1 file changed, 26 insertions(+), 10 deletions(-) diff --git a/osgar/drivers/realsense.py b/osgar/drivers/realsense.py index c5e970d9b..17d579eea 100644 --- a/osgar/drivers/realsense.py +++ b/osgar/drivers/realsense.py @@ -26,6 +26,7 @@ from osgar.node import Node from osgar.lib import quaternion +from osgar.lib.serialize import serialize # https://github.com/IntelRealSense/librealsense/blob/master/doc/t265.md#sensor-origin-and-coordinate-system @@ -65,6 +66,8 @@ def __init__(self, config, bus): self.default_rgb_resolution = config.get("rgb_resolution", [640, 360]) self.depth_fps = config.get("depth_fps", 30) self.disable_emitor = config.get("disable_emitor", False) + self.camera_pose = config.get("camera_pose", [[0, 0, 0], [0, 0, 0, 1]]) + self.img_format = config.get("img_format", "*.jpeg") if self.depth_rgb or self.depth_infra: import cv2 @@ -72,8 +75,8 @@ def __init__(self, config, bus): self.rgbd = config.get("rgbd", False) if self.rgbd: - assert self - bus.register('rgbd', 'infra') + assert self.depth_rgb, self.depth_rgb + bus.register('rgbd_raw:gz', 'infra') else: bus.register('depth:gz', 'color', 'infra') else: @@ -122,7 +125,12 @@ def pose_callback(self, frame): def depth_callback(self, frameset): try: assert frameset.is_frameset() - depth_frame = frameset.as_frameset().get_depth_frame() + if self.rgbd: + frameset_as = self.align.process(frameset.as_frameset()) + else: + frameset_as = frameset.as_frameset() + depth_frame = frameset_as.get_depth_frame() + n = depth_frame.get_frame_number() if n % self.depth_subsample != 0: return @@ -130,25 +138,28 @@ def depth_callback(self, frameset): depth_image = np.asanyarray(depth_frame.as_depth_frame().get_data()) if self.depth_rgb: - color_frame = frameset.as_frameset().get_color_frame() + color_frame = frameset_as.get_color_frame() assert color_frame.is_video_frame() color_image = np.asanyarray(color_frame.as_video_frame().get_data()) - __, data = cv2.imencode('*.jpeg', color_image) + __, data = cv2.imencode(self.img_format, color_image) if self.depth_infra: - infra_frame = frameset.as_frameset().get_infrared_frame() + infra_frame = frameset_as.get_infrared_frame() assert infra_frame.is_video_frame() infra_image = np.asanyarray(infra_frame.as_video_frame().get_data()) - __, infra_data = cv2.imencode('*.jpeg', infra_image) + __, infra_data = cv2.imencode(self.img_format, infra_image) except Exception as e: print(e) self.finished.set() return - self.publish('depth', depth_image) - if self.depth_rgb: - self.publish('color', data.tobytes()) + if self.rgbd: + self.publish('rgbd_raw', [None, self.camera_pose, data.tobytes(), serialize(depth_image)]) # None for robot pose + else: + self.publish('depth', depth_image) + if self.depth_rgb: + self.publish('color', data.tobytes()) if self.depth_infra: self.publish('infra', infra_data.tobytes()) @@ -160,6 +171,8 @@ def start(self): info_msg = "Enabling streams: depth" if self.depth_rgb: info_msg += ", depth_rgb" + if self.rgbd: + info_msg += " (rgbd allowed)" if self.depth_infra: info_msg += ", depth_infra" g_logger.info(info_msg) @@ -173,6 +186,9 @@ def start(self): if self.depth_rgb: w, h = self.default_rgb_resolution depth_cfg.enable_stream(rs.stream.color, w, h, rs.format.bgr8, self.depth_fps) + if self.rgbd: + align_to = rs.stream.color + self.align = rs.align(align_to) if self.depth_infra: w, h = self.default_depth_resolution depth_cfg.enable_stream(rs.stream.infrared, w, h, rs.format.y8, self.depth_fps) From 8c3cc5fbf04cc0e2a5b6dc6d73efd49b59550d86 Mon Sep 17 00:00:00 2001 From: tajgr Date: Thu, 14 Oct 2021 19:55:32 +0200 Subject: [PATCH 03/10] osgar/drivers/realsense.py: extend functionality to multicam, rgbd, image format, image resolutions --- osgar/drivers/realsense.py | 51 +++++++++++++++++++++++++++----------- 1 file changed, 36 insertions(+), 15 deletions(-) diff --git a/osgar/drivers/realsense.py b/osgar/drivers/realsense.py index 17d579eea..e09d49c2c 100644 --- a/osgar/drivers/realsense.py +++ b/osgar/drivers/realsense.py @@ -252,12 +252,15 @@ def __init__(self, config, bus): self.depth_subsample = config.get('depth_subsample', 3) self.depth_rgb = config.get('depth_rgb', False) self.depth_infra = config.get('depth_infra', False) - self.default_depth_resolution = config.get('depth_resolution', [640, 360]) - self.default_rgb_resolution = config.get('rgb_resolution', [640, 360]) self.depth_fps = config.get('depth_fps', 30) self.pose_subsample = config.get('pose_subsample', 20) self.tracking_sensor_pitch_quat_inv = {} self.tracking_sensor_yaw_quat = {} + self.camera_pose = config.get("camera_pose", [[0, 0, 0], [0, 0, 0, 1]]) + self.img_format = config.get("img_format", "*.jpeg") + self.rgbd = config.get("rgbd", False) + if self.rgbd: + assert self.depth_rgb, self.depth_rgb streams = [] for device in self.devices: @@ -276,7 +279,11 @@ def __init__(self, config, bus): self.tracking_sensor_yaw_quat[name] = euler_to_quaternion( math.radians(device.get('yaw', 0)), 0, 0) elif device_type in ['D400', 'L500']: - for stream in ['depth:gz', 'color', 'infra']: + if self.rgbd: + streams_to_register = ['rgbd_raw:gz', 'infra'] + else: + streams_to_register = ['depth:gz', 'color', 'infra'] + for stream in streams_to_register: streams.append(f'{name}_{stream}') if self.depth_rgb or self.depth_infra: @@ -337,8 +344,13 @@ def pose_callback(self, frame): def depth_callback(self, frameset): try: assert frameset.is_frameset() - depth_frame = frameset.as_frameset().get_depth_frame() - profile = depth_frame.get_profile() + if self.rgbd: + frameset_as = self.align.process(frameset.as_frameset()) + else: + frameset_as = frameset.as_frameset() + depth_frame = frameset_as.get_depth_frame() + + profile = frameset.get_profile() with self.origins_lock: try: name = self.origins[profile.unique_id()] @@ -352,25 +364,29 @@ def depth_callback(self, frameset): depth_image = np.asanyarray(depth_frame.as_depth_frame().get_data()) if self.depth_rgb: - color_frame = frameset.as_frameset().get_color_frame() + color_frame = frameset_as.get_color_frame() assert color_frame.is_video_frame() color_image = np.asanyarray(color_frame.as_video_frame().get_data()) - __, data = cv2.imencode('*.jpeg', color_image) + __, data = cv2.imencode(self.img_format, color_image) if self.depth_infra: - infra_frame = frameset.as_frameset().get_infrared_frame() + infra_frame = frameset_as.get_infrared_frame() assert infra_frame.is_video_frame() infra_image = np.asanyarray(infra_frame.as_video_frame().get_data()) - __, infra_data = cv2.imencode('*.jpeg', infra_image) + __, infra_data = cv2.imencode(self.img_format, infra_image) except Exception as e: print(e) self.finished.set() return - self.publish(f'{name}_depth', depth_image) - if self.depth_rgb: - self.publish(f'{name}_color', data.tobytes()) + if self.rgbd: + self.publish(f'{name}_rgbd_raw', + [None, self.camera_pose, data.tobytes(), serialize(depth_image)]) # None for robot pose + else: + self.publish(f'{name}_depth', depth_image) + if self.depth_rgb: + self.publish(f'{name}_color', data.tobytes()) if self.depth_infra: self.publish(f'{name}_infra', infra_data.tobytes()) @@ -390,6 +406,8 @@ def start(self): info_msg = f'Enabling streams: {name}_depth' if self.depth_rgb: info_msg += f', {name}_color' + if self.rgbd: + info_msg += " (rgbd allowed)" if self.depth_infra: info_msg += f', {name}_infra' g_logger.info(info_msg) @@ -397,13 +415,16 @@ def start(self): depth_pipeline = rs.pipeline(ctx) depth_cfg = rs.config() depth_cfg.enable_device(serial_number) - w, h = self.default_depth_resolution + w, h = device.get('depth_resolution', [640, 360]) depth_cfg.enable_stream(rs.stream.depth, w, h, rs.format.z16, self.depth_fps) if self.depth_rgb: - w, h = self.default_rgb_resolution + w, h = device.get('rgb_resolution', [640, 360]) depth_cfg.enable_stream(rs.stream.color, w, h, rs.format.bgr8, self.depth_fps) + if self.rgbd: + align_to = rs.stream.color + self.align = rs.align(align_to) if self.depth_infra: - w, h = self.default_depth_resolution + w, h = device.get('depth_resolution', [640, 360]) depth_cfg.enable_stream(rs.stream.infrared, w, h, rs.format.y8, self.depth_fps) if depth_cfg.can_resolve(depth_pipeline): self.pipelines.append(depth_pipeline) From 3c2190fba66026e92d962401cabeb84e13f8e074 Mon Sep 17 00:00:00 2001 From: tajgr Date: Thu, 14 Oct 2021 20:12:38 +0200 Subject: [PATCH 04/10] config/test-rs-multicam.json: initial commit --- config/test-rs-multicam.json | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) create mode 100644 config/test-rs-multicam.json diff --git a/config/test-rs-multicam.json b/config/test-rs-multicam.json new file mode 100644 index 000000000..c92af57c7 --- /dev/null +++ b/config/test-rs-multicam.json @@ -0,0 +1,23 @@ +{ + "version": 2, + "robot": { + "modules": { + "app": { + "driver": "realsense_multicam", + "init": { + "devices": [{"type": "D400", "serial_number": "920312071702", "name": "D435", + "depth_resolution": [1280, 720], "rgb_resolution": [1280, 720]}, + {"type": "D400", "serial_number": "934222071502", "name": "D435_B"} + ], + "depth_subsample": 10, + "depth_rgb": true, + "depth_infra": true, + "depth_fps": 30, + "rgbd": true, + "img_format": "*.png" + } + } + }, + "links": [] + } +} From 849655d5b1d87e6f58e806cece6812f5b72f5874 Mon Sep 17 00:00:00 2001 From: tajgr Date: Wed, 5 Jan 2022 23:36:22 +0100 Subject: [PATCH 05/10] osgar/tools/lidarview.py: Depth data from realsense cameras are in millimeters. --- osgar/tools/lidarview.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/osgar/tools/lidarview.py b/osgar/tools/lidarview.py index 681210889..84ed3894c 100644 --- a/osgar/tools/lidarview.py +++ b/osgar/tools/lidarview.py @@ -223,7 +223,7 @@ def get_image(data): elif isinstance(data, tuple): img_data, depth_data = data image = pygame.image.load(io.BytesIO(img_data), 'JPG').convert() - g_depth = decompress_depth(depth_data) + g_depth = decompress_depth(depth_data)/1000 elif isinstance(data, list): # image stereo artefact localization # expects localized pair of images [camera_name, [robot_pose, camera_pose, image], [robot_pose, camera_pose, image]] From 6f41d716ede1b28bb30d063bb7b199b36366ec6e Mon Sep 17 00:00:00 2001 From: tajgr Date: Thu, 6 Jan 2022 14:43:54 +0100 Subject: [PATCH 06/10] config/test-realsense-D400.json: add rgbd parameter --- config/test-realsense-D400.json | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/config/test-realsense-D400.json b/config/test-realsense-D400.json index 867d9e2b6..c793b7b88 100644 --- a/config/test-realsense-D400.json +++ b/config/test-realsense-D400.json @@ -10,7 +10,9 @@ "depth_rgb": true, "depth_resolution": [640, 360], "rgb_resolution": [640, 360], - "depth_fps": 30 + "depth_fps": 30, + "rgbd": true, + "img_format": "*.jpeg" } } }, From d6a44b22ea3fcb9dc2c9294eb47dced2148b0b9a Mon Sep 17 00:00:00 2001 From: tajgr Date: Thu, 6 Jan 2022 15:02:06 +0100 Subject: [PATCH 07/10] config/test-rs-multicam.json: enable individual depth_fps for cameras --- config/test-rs-multicam.json | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/config/test-rs-multicam.json b/config/test-rs-multicam.json index c92af57c7..7016bfb83 100644 --- a/config/test-rs-multicam.json +++ b/config/test-rs-multicam.json @@ -6,14 +6,15 @@ "driver": "realsense_multicam", "init": { "devices": [{"type": "D400", "serial_number": "920312071702", "name": "D435", - "depth_resolution": [1280, 720], "rgb_resolution": [1280, 720]}, - {"type": "D400", "serial_number": "934222071502", "name": "D435_B"} + "depth_resolution": [640, 360], "rgb_resolution": [640, 360], "depth_fps": 30}, + + {"type": "D400", "serial_number": "934222071502", "name": "D435_B", + "depth_resolution": [1280, 720], "rgb_resolution": [1280, 720], "depth_fps": 6} ], - "depth_subsample": 10, + "depth_subsample": 3, "depth_rgb": true, "depth_infra": true, - "depth_fps": 30, - "rgbd": true, + "rgbd": false, "img_format": "*.png" } } From 92725405e1f46b3b3f3e639dccd998ca7c07e63a Mon Sep 17 00:00:00 2001 From: tajgr Date: Thu, 6 Jan 2022 15:03:35 +0100 Subject: [PATCH 08/10] osgar/drivers/realsense.py: enable individual depth_fps for cameras --- osgar/drivers/realsense.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/osgar/drivers/realsense.py b/osgar/drivers/realsense.py index e09d49c2c..489aa3e56 100644 --- a/osgar/drivers/realsense.py +++ b/osgar/drivers/realsense.py @@ -252,7 +252,6 @@ def __init__(self, config, bus): self.depth_subsample = config.get('depth_subsample', 3) self.depth_rgb = config.get('depth_rgb', False) self.depth_infra = config.get('depth_infra', False) - self.depth_fps = config.get('depth_fps', 30) self.pose_subsample = config.get('pose_subsample', 20) self.tracking_sensor_pitch_quat_inv = {} self.tracking_sensor_yaw_quat = {} @@ -416,16 +415,17 @@ def start(self): depth_cfg = rs.config() depth_cfg.enable_device(serial_number) w, h = device.get('depth_resolution', [640, 360]) - depth_cfg.enable_stream(rs.stream.depth, w, h, rs.format.z16, self.depth_fps) + depth_fps = device.get('depth_fps', 30) + depth_cfg.enable_stream(rs.stream.depth, w, h, rs.format.z16, depth_fps) if self.depth_rgb: w, h = device.get('rgb_resolution', [640, 360]) - depth_cfg.enable_stream(rs.stream.color, w, h, rs.format.bgr8, self.depth_fps) + depth_cfg.enable_stream(rs.stream.color, w, h, rs.format.bgr8, depth_fps) if self.rgbd: align_to = rs.stream.color self.align = rs.align(align_to) if self.depth_infra: w, h = device.get('depth_resolution', [640, 360]) - depth_cfg.enable_stream(rs.stream.infrared, w, h, rs.format.y8, self.depth_fps) + depth_cfg.enable_stream(rs.stream.infrared, w, h, rs.format.y8, depth_fps) if depth_cfg.can_resolve(depth_pipeline): self.pipelines.append(depth_pipeline) configs.append(depth_cfg) From 7313fb510e4eff8ef15ed3d5cef6af74066a9e36 Mon Sep 17 00:00:00 2001 From: tajgr Date: Thu, 17 Feb 2022 21:52:07 +0100 Subject: [PATCH 09/10] osgar/drivers/realsense.py: use rgbd bundle as dictionary --- osgar/drivers/realsense.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/osgar/drivers/realsense.py b/osgar/drivers/realsense.py index 489aa3e56..895d2b2bd 100644 --- a/osgar/drivers/realsense.py +++ b/osgar/drivers/realsense.py @@ -66,7 +66,7 @@ def __init__(self, config, bus): self.default_rgb_resolution = config.get("rgb_resolution", [640, 360]) self.depth_fps = config.get("depth_fps", 30) self.disable_emitor = config.get("disable_emitor", False) - self.camera_pose = config.get("camera_pose", [[0, 0, 0], [0, 0, 0, 1]]) + camera_pose = config.get("camera_pose", [[0, 0, 0], [0, 0, 0, 1]]) self.img_format = config.get("img_format", "*.jpeg") if self.depth_rgb or self.depth_infra: @@ -76,9 +76,8 @@ def __init__(self, config, bus): self.rgbd = config.get("rgbd", False) if self.rgbd: assert self.depth_rgb, self.depth_rgb - bus.register('rgbd_raw:gz', 'infra') - else: - bus.register('depth:gz', 'color', 'infra') + self.rgbd_bundle = {"camera_pose": camera_pose} # TODO add meta data + bus.register('depth:gz', 'color', 'infra', 'rgbd_raw:gz') else: g_logger.warning("Device is not specified in the config!") @@ -155,7 +154,8 @@ def depth_callback(self, frameset): return if self.rgbd: - self.publish('rgbd_raw', [None, self.camera_pose, data.tobytes(), serialize(depth_image)]) # None for robot pose + self.rgbd_bundle["data"] = [data.tobytes(), serialize(depth_image)] + self.publish('rgbd_raw', self.rgbd_bundle) else: self.publish('depth', depth_image) if self.depth_rgb: From 2bce7eae289bf42a325324e89f924e2fc2d0e7f9 Mon Sep 17 00:00:00 2001 From: tajgr Date: Thu, 17 Feb 2022 21:54:30 +0100 Subject: [PATCH 10/10] osgar/tools/lidarview.py: add rgbd bundle dictionary --- osgar/tools/lidarview.py | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/osgar/tools/lidarview.py b/osgar/tools/lidarview.py index 84ed3894c..95058b0ac 100644 --- a/osgar/tools/lidarview.py +++ b/osgar/tools/lidarview.py @@ -221,9 +221,9 @@ def get_image(data): image = pygame.image.frombuffer(im_color.tostring(), im_color.shape[1::-1], "RGB") g_depth = data elif isinstance(data, tuple): - img_data, depth_data = data + img_data, depth_data, dist_scale = data image = pygame.image.load(io.BytesIO(img_data), 'JPG').convert() - g_depth = decompress_depth(depth_data)/1000 + g_depth = decompress_depth(depth_data)*dist_scale elif isinstance(data, list): # image stereo artefact localization # expects localized pair of images [camera_name, [robot_pose, camera_pose, image], [robot_pose, camera_pose, image]] @@ -398,8 +398,14 @@ def _step(self, direction): elif stream_id == self.camera2_id: self.image2, _ = get_image(deserialize(data)) elif stream_id == self.rgbd_id: - _, _, img_data, depth_data = deserialize(data) - self.image, self.image2 = get_image((img_data, depth_data)) + rgbd_data = deserialize(data) + if isinstance(rgbd_data, dict): + img_data, depth_data = rgbd_data["data"] + dist_scale = 0.001 # TODO read from bundle stream ? + self.image, self.image2 = get_image((img_data, depth_data, dist_scale)) + else: + _, _, img_data, depth_data = rgbd_data + self.image, self.image2 = get_image((img_data, depth_data, 1)) if self.lidar_id is None: keyframe = self.keyframe self.keyframe = False