diff --git a/lerobot/common/robot_devices/cameras/intelrealsense.py b/lerobot/common/robot_devices/cameras/intelrealsense.py index 29e99ea3..d9c57aa2 100644 --- a/lerobot/common/robot_devices/cameras/intelrealsense.py +++ b/lerobot/common/robot_devices/cameras/intelrealsense.py @@ -114,7 +114,7 @@ def save_images_from_cameras( camera = IntelRealSenseCamera(config) camera.connect() print( - f"IntelRealSenseCamera({camera.serial_number}, fps={camera.fps}, width={camera.width}, height={camera.height}, color_mode={camera.color_mode})" + f"IntelRealSenseCamera({camera.serial_number}, fps={camera.fps}, width={camera.capture_width}, height={camera.capture_height}, color_mode={camera.color_mode})" ) cameras.append(camera) @@ -224,9 +224,20 @@ class IntelRealSenseCamera: self.serial_number = self.find_serial_number_from_name(config.name) else: self.serial_number = config.serial_number + + # Store the raw (capture) resolution from the config. + self.capture_width = config.width + self.capture_height = config.height + + # If rotated by ±90, swap width and height. + if config.rotation in [-90, 90]: + self.width = config.height + self.height = config.width + else: + self.width = config.width + self.height = config.height + self.fps = config.fps - self.width = config.width - self.height = config.height self.channels = config.channels self.color_mode = config.color_mode self.use_depth = config.use_depth @@ -246,7 +257,6 @@ class IntelRealSenseCamera: else: import cv2 - # TODO(alibets): Do we keep original width/height or do we define them after rotation? self.rotation = None if config.rotation == -90: self.rotation = cv2.ROTATE_90_COUNTERCLOCKWISE @@ -284,15 +294,19 @@ class IntelRealSenseCamera: config = rs.config() config.enable_device(str(self.serial_number)) - if self.fps and self.width and self.height: + if self.fps and self.capture_width and self.capture_height: # TODO(rcadene): can we set rgb8 directly? - config.enable_stream(rs.stream.color, self.width, self.height, rs.format.rgb8, self.fps) + config.enable_stream( + rs.stream.color, self.capture_width, self.capture_height, rs.format.rgb8, self.fps + ) else: config.enable_stream(rs.stream.color) if self.use_depth: - if self.fps and self.width and self.height: - config.enable_stream(rs.stream.depth, self.width, self.height, rs.format.z16, self.fps) + if self.fps and self.capture_width and self.capture_height: + config.enable_stream( + rs.stream.depth, self.capture_width, self.capture_height, rs.format.z16, self.fps + ) else: config.enable_stream(rs.stream.depth) @@ -330,18 +344,18 @@ class IntelRealSenseCamera: raise OSError( f"Can't set {self.fps=} for IntelRealSenseCamera({self.serial_number}). Actual value is {actual_fps}." ) - if self.width is not None and self.width != actual_width: + if self.capture_width is not None and self.capture_width != actual_width: raise OSError( - f"Can't set {self.width=} for IntelRealSenseCamera({self.serial_number}). Actual value is {actual_width}." + f"Can't set {self.capture_width=} for IntelRealSenseCamera({self.serial_number}). Actual value is {actual_width}." ) - if self.height is not None and self.height != actual_height: + if self.capture_height is not None and self.capture_height != actual_height: raise OSError( - f"Can't set {self.height=} for IntelRealSenseCamera({self.serial_number}). Actual value is {actual_height}." + f"Can't set {self.capture_height=} for IntelRealSenseCamera({self.serial_number}). Actual value is {actual_height}." ) self.fps = round(actual_fps) - self.width = round(actual_width) - self.height = round(actual_height) + self.capture_width = round(actual_width) + self.capture_height = round(actual_height) self.is_connected = True @@ -387,7 +401,7 @@ class IntelRealSenseCamera: color_image = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR) h, w, _ = color_image.shape - if h != self.height or w != self.width: + if h != self.capture_height or w != self.capture_width: raise OSError( f"Can't capture color image with expected height and width ({self.height} x {self.width}). ({h} x {w}) returned instead." ) @@ -409,7 +423,7 @@ class IntelRealSenseCamera: depth_map = np.asanyarray(depth_frame.get_data()) h, w = depth_map.shape - if h != self.height or w != self.width: + if h != self.capture_height or w != self.capture_width: raise OSError( f"Can't capture depth map with expected height and width ({self.height} x {self.width}). ({h} x {w}) returned instead." ) diff --git a/lerobot/common/robot_devices/cameras/opencv.py b/lerobot/common/robot_devices/cameras/opencv.py index e6133a84..173f3f1a 100644 --- a/lerobot/common/robot_devices/cameras/opencv.py +++ b/lerobot/common/robot_devices/cameras/opencv.py @@ -144,8 +144,8 @@ def save_images_from_cameras( camera = OpenCVCamera(config) camera.connect() print( - f"OpenCVCamera({camera.camera_index}, fps={camera.fps}, width={camera.width}, " - f"height={camera.height}, color_mode={camera.color_mode})" + f"OpenCVCamera({camera.camera_index}, fps={camera.fps}, width={camera.capture_width}, " + f"height={camera.capture_height}, color_mode={camera.color_mode})" ) cameras.append(camera) @@ -244,9 +244,19 @@ class OpenCVCamera: else: raise ValueError(f"Please check the provided camera_index: {self.camera_index}") + # Store the raw (capture) resolution from the config. + self.capture_width = config.width + self.capture_height = config.height + + # If rotated by ±90, swap width and height. + if config.rotation in [-90, 90]: + self.width = config.height + self.height = config.width + else: + self.width = config.width + self.height = config.height + self.fps = config.fps - self.width = config.width - self.height = config.height self.channels = config.channels self.color_mode = config.color_mode self.mock = config.mock @@ -263,7 +273,6 @@ class OpenCVCamera: else: import cv2 - # TODO(aliberts): Do we keep original width/height or do we define them after rotation? self.rotation = None if config.rotation == -90: self.rotation = cv2.ROTATE_90_COUNTERCLOCKWISE @@ -325,10 +334,10 @@ class OpenCVCamera: if self.fps is not None: self.camera.set(cv2.CAP_PROP_FPS, self.fps) - if self.width is not None: - self.camera.set(cv2.CAP_PROP_FRAME_WIDTH, self.width) - if self.height is not None: - self.camera.set(cv2.CAP_PROP_FRAME_HEIGHT, self.height) + if self.capture_width is not None: + self.camera.set(cv2.CAP_PROP_FRAME_WIDTH, self.capture_width) + if self.capture_height is not None: + self.camera.set(cv2.CAP_PROP_FRAME_HEIGHT, self.capture_height) actual_fps = self.camera.get(cv2.CAP_PROP_FPS) actual_width = self.camera.get(cv2.CAP_PROP_FRAME_WIDTH) @@ -340,19 +349,22 @@ class OpenCVCamera: raise OSError( f"Can't set {self.fps=} for OpenCVCamera({self.camera_index}). Actual value is {actual_fps}." ) - if self.width is not None and not math.isclose(self.width, actual_width, rel_tol=1e-3): + if self.capture_width is not None and not math.isclose( + self.capture_width, actual_width, rel_tol=1e-3 + ): raise OSError( - f"Can't set {self.width=} for OpenCVCamera({self.camera_index}). Actual value is {actual_width}." + f"Can't set {self.capture_width=} for OpenCVCamera({self.camera_index}). Actual value is {actual_width}." ) - if self.height is not None and not math.isclose(self.height, actual_height, rel_tol=1e-3): + if self.capture_height is not None and not math.isclose( + self.capture_height, actual_height, rel_tol=1e-3 + ): raise OSError( - f"Can't set {self.height=} for OpenCVCamera({self.camera_index}). Actual value is {actual_height}." + f"Can't set {self.capture_height=} for OpenCVCamera({self.camera_index}). Actual value is {actual_height}." ) self.fps = round(actual_fps) - self.width = round(actual_width) - self.height = round(actual_height) - + self.capture_width = round(actual_width) + self.capture_height = round(actual_height) self.is_connected = True def read(self, temporary_color_mode: str | None = None) -> np.ndarray: @@ -393,7 +405,7 @@ class OpenCVCamera: color_image = cv2.cvtColor(color_image, cv2.COLOR_BGR2RGB) h, w, _ = color_image.shape - if h != self.height or w != self.width: + if h != self.capture_height or w != self.capture_width: raise OSError( f"Can't capture color image with expected height and width ({self.height} x {self.width}). ({h} x {w}) returned instead." ) diff --git a/tests/test_cameras.py b/tests/test_cameras.py index 3ab74516..ded0a3d5 100644 --- a/tests/test_cameras.py +++ b/tests/test_cameras.py @@ -85,8 +85,8 @@ def test_camera(request, camera_type, mock): camera.connect() assert camera.is_connected assert camera.fps is not None - assert camera.width is not None - assert camera.height is not None + assert camera.capture_width is not None + assert camera.capture_height is not None # Test connecting twice raises an error with pytest.raises(RobotDeviceAlreadyConnectedError): @@ -204,3 +204,49 @@ def test_save_images_from_cameras(tmp_path, request, camera_type, mock): # Small `record_time_s` to speedup unit tests save_images_from_cameras(tmp_path, record_time_s=0.02, mock=mock) + + +@pytest.mark.parametrize("camera_type, mock", TEST_CAMERA_TYPES) +@require_camera +def test_camera_rotation(request, camera_type, mock): + config_kwargs = {"camera_type": camera_type, "mock": mock, "width": 640, "height": 480, "fps": 30} + + # No rotation. + camera = make_camera(**config_kwargs, rotation=None) + camera.connect() + assert camera.capture_width == 640 + assert camera.capture_height == 480 + assert camera.width == 640 + assert camera.height == 480 + no_rot_img = camera.read() + h, w, c = no_rot_img.shape + assert h == 480 and w == 640 and c == 3 + camera.disconnect() + + # Rotation = 90 (clockwise). + camera = make_camera(**config_kwargs, rotation=90) + camera.connect() + # With a 90° rotation, we expect the metadata dimensions to be swapped. + assert camera.capture_width == 640 + assert camera.capture_height == 480 + assert camera.width == 480 + assert camera.height == 640 + import cv2 + + assert camera.rotation == cv2.ROTATE_90_CLOCKWISE + rot_img = camera.read() + h, w, c = rot_img.shape + assert h == 640 and w == 480 and c == 3 + camera.disconnect() + + # Rotation = 180. + camera = make_camera(**config_kwargs, rotation=None) + camera.connect() + assert camera.capture_width == 640 + assert camera.capture_height == 480 + assert camera.width == 640 + assert camera.height == 480 + no_rot_img = camera.read() + h, w, c = no_rot_img.shape + assert h == 480 and w == 640 and c == 3 + camera.disconnect()