From ab2b5b04ddb012c50d79ea92d4fc786fdc4f87c7 Mon Sep 17 00:00:00 2001 From: Caroline Pascal Date: Fri, 24 Jul 2026 17:43:17 +0200 Subject: [PATCH] (depth image processing): excluding depth frames from the RGB to BGR image processing (#4135) * (depth image processing): excluding depth frames from the RGB to BGR image processing * test(update): updating tests to include RGB/BGR conversion checks --- .../cameras/reachy2_camera/reachy2_camera.py | 3 +- .../cameras/realsense/camera_realsense.py | 2 +- tests/cameras/test_opencv.py | 24 +++++++- tests/cameras/test_reachy2_camera.py | 59 ++++++++++++++----- tests/cameras/test_realsense.py | 28 ++++++++- 5 files changed, 97 insertions(+), 19 deletions(-) diff --git a/src/lerobot/cameras/reachy2_camera/reachy2_camera.py b/src/lerobot/cameras/reachy2_camera/reachy2_camera.py index 9b7e7a2e0..3ee7f190a 100644 --- a/src/lerobot/cameras/reachy2_camera/reachy2_camera.py +++ b/src/lerobot/cameras/reachy2_camera/reachy2_camera.py @@ -173,7 +173,8 @@ class Reachy2Camera(Camera): raise ValueError( f"Invalid color mode '{self.color_mode}'. Expected {ColorMode.RGB} or {ColorMode.BGR}." ) - if self.color_mode == ColorMode.RGB: + is_depth_frame = self.config.name == "depth" and self.config.image_type == "depth" + if not is_depth_frame and self.color_mode == ColorMode.RGB: frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) self.latest_frame = frame diff --git a/src/lerobot/cameras/realsense/camera_realsense.py b/src/lerobot/cameras/realsense/camera_realsense.py index 29cb1e5e0..f873abbd4 100644 --- a/src/lerobot/cameras/realsense/camera_realsense.py +++ b/src/lerobot/cameras/realsense/camera_realsense.py @@ -453,7 +453,7 @@ class RealSenseCamera(Camera): ) processed_image = image - if self.color_mode == ColorMode.BGR: + if not depth_frame and self.color_mode == ColorMode.BGR: processed_image = cv2.cvtColor(image, cv2.COLOR_RGB2BGR) if self.rotation in [cv2.ROTATE_90_CLOCKWISE, cv2.ROTATE_90_COUNTERCLOCKWISE, cv2.ROTATE_180]: diff --git a/tests/cameras/test_opencv.py b/tests/cameras/test_opencv.py index 720d0c9b3..e2ae5c572 100644 --- a/tests/cameras/test_opencv.py +++ b/tests/cameras/test_opencv.py @@ -26,7 +26,7 @@ import cv2 import numpy as np import pytest -from lerobot.cameras.configs import Cv2Rotation +from lerobot.cameras.configs import ColorMode, Cv2Rotation from lerobot.cameras.opencv import OpenCVCamera, OpenCVCameraConfig from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError @@ -132,6 +132,28 @@ def test_read(index_or_path): assert isinstance(img, np.ndarray) +@pytest.mark.parametrize("index_or_path", TEST_IMAGE_PATHS, ids=TEST_IMAGE_SIZES) +def test_color_mode_conversion(index_or_path): + """RGB and BGR reads of the same frame must differ only by a channel-axis reversal.""" + rgb_config = OpenCVCameraConfig(index_or_path=index_or_path, color_mode=ColorMode.RGB, warmup_s=0) + bgr_config = OpenCVCameraConfig(index_or_path=index_or_path, color_mode=ColorMode.BGR, warmup_s=0) + with OpenCVCamera(rgb_config) as rgb_cam: + rgb = rgb_cam.read() + with OpenCVCamera(bgr_config) as bgr_cam: + bgr = bgr_cam.read() + + assert rgb.shape == bgr.shape + np.testing.assert_array_equal(rgb, bgr[..., ::-1]) + + +def test_postprocess_invalid_color_mode(): + config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH) + camera = OpenCVCamera(config) + camera.color_mode = "invalid" + with pytest.raises(ValueError): + camera._postprocess_image(np.zeros((120, 160, 3), dtype=np.uint8)) + + def test_read_before_connect(): config = OpenCVCameraConfig(index_or_path=DEFAULT_PNG_FILE_PATH) diff --git a/tests/cameras/test_reachy2_camera.py b/tests/cameras/test_reachy2_camera.py index 2aebfdf0a..1d3ef89f8 100644 --- a/tests/cameras/test_reachy2_camera.py +++ b/tests/cameras/test_reachy2_camera.py @@ -22,6 +22,7 @@ import pytest pytest.importorskip("reachy2_sdk") +from lerobot.cameras.configs import ColorMode from lerobot.cameras.reachy2_camera import Reachy2Camera, Reachy2CameraConfig from lerobot.utils.errors import DeviceNotConnectedError @@ -33,28 +34,19 @@ PARAMS = [ ] -def _make_cam_manager_mock(): +def _make_cam_manager_mock(color_frame, depth_frame=None): c = MagicMock(name="CameraManagerMock") teleop = MagicMock(name="TeleopCam") teleop.width = 640 teleop.height = 480 - teleop.get_frame = MagicMock( - side_effect=lambda *_, **__: ( - np.zeros((480, 640, 3), dtype=np.uint8), - time.time(), - ) - ) + teleop.get_frame = MagicMock(side_effect=lambda *_, **__: (color_frame, time.time())) depth = MagicMock(name="DepthCam") depth.width = 640 depth.height = 480 - depth.get_frame = MagicMock( - side_effect=lambda *_, **__: ( - np.zeros((480, 640, 3), dtype=np.uint8), - time.time(), - ) - ) + depth.get_frame = MagicMock(side_effect=lambda *_, **__: (color_frame, time.time())) + depth.get_depth_frame = MagicMock(side_effect=lambda *_, **__: (depth_frame, time.time())) c.is_connected.return_value = True c.teleop = teleop @@ -84,12 +76,14 @@ def _make_cam_manager_mock(): # ids=["teleop-left", "teleop-right", "torso-rgb", "torso-depth"], ids=["teleop-left", "teleop-right", "torso-rgb"], ) -def camera(request): +def camera(request, img_array_factory): name, image_type = request.param + color_frame = img_array_factory(height=480, width=640) + depth_frame = img_array_factory(height=480, width=640, channels=1, dtype=np.uint16)[..., 0] with ( patch( "lerobot.cameras.reachy2_camera.reachy2_camera.CameraManager", - side_effect=lambda *a, **k: _make_cam_manager_mock(), + side_effect=lambda *a, **k: _make_cam_manager_mock(color_frame, depth_frame), ), ): config = Reachy2CameraConfig(name=name, image_type=image_type) @@ -188,6 +182,41 @@ def test_read_latest_too_old(camera): _ = camera.read_latest(max_age_ms=0) # immediately too old +def test_color_mode_conversion(img_array_factory): + """teleop frames are native BGR: RGB reverses the channel axis, BGR is passed through.""" + frame = img_array_factory(height=8, width=8) + + outputs = {} + for color_mode in (ColorMode.RGB, ColorMode.BGR): + with patch( + "lerobot.cameras.reachy2_camera.reachy2_camera.CameraManager", + side_effect=lambda *a, **k: _make_cam_manager_mock(frame), + ): + cam = Reachy2Camera(Reachy2CameraConfig(name="teleop", image_type="left", color_mode=color_mode)) + cam.connect() + outputs[color_mode] = cam.read() + cam.disconnect() + + np.testing.assert_array_equal(outputs[ColorMode.BGR], frame) + np.testing.assert_array_equal(outputs[ColorMode.RGB], frame[..., ::-1]) + + +def test_depth_frame_not_color_converted(img_array_factory): + """A depth/depth frame must be returned as-is, without BGR<->RGB conversion.""" + color_frame = img_array_factory(height=8, width=8) + depth = img_array_factory(height=8, width=8, channels=1, dtype=np.uint16)[..., 0] + with patch( + "lerobot.cameras.reachy2_camera.reachy2_camera.CameraManager", + side_effect=lambda *a, **k: _make_cam_manager_mock(color_frame, depth_frame=depth), + ): + cam = Reachy2Camera(Reachy2CameraConfig(name="depth", image_type="depth")) + cam.connect() + out = cam.read() + cam.disconnect() + + np.testing.assert_array_equal(out, depth) + + def test_wrong_camera_name(): with pytest.raises(ValueError): _ = Reachy2CameraConfig(name="wrong-name", image_type="left") diff --git a/tests/cameras/test_realsense.py b/tests/cameras/test_realsense.py index 1deb73f05..64cc272d5 100644 --- a/tests/cameras/test_realsense.py +++ b/tests/cameras/test_realsense.py @@ -25,7 +25,7 @@ from unittest.mock import patch import numpy as np import pytest -from lerobot.cameras.configs import Cv2Rotation +from lerobot.cameras.configs import ColorMode, Cv2Rotation from lerobot.utils.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError pytest.importorskip("pyrealsense2") @@ -109,6 +109,32 @@ def test_read_depth(): assert isinstance(img, np.ndarray) +# These exercise _postprocess_image directly rather than read(): the bag playback returns +# non-deterministic frames we can't compare against, and the depth read() path is skipped +# (see test_read_depth) with the current pyrealsense2 version. +def test_color_mode_conversion(img_array_factory): + """RGB (native for RealSense) is passed through; BGR reverses the channel axis.""" + color = img_array_factory(height=3, width=4) + + outputs = {} + for color_mode in (ColorMode.RGB, ColorMode.BGR): + camera = RealSenseCamera(RealSenseCameraConfig(serial_number_or_name="042", color_mode=color_mode)) + camera.capture_height, camera.capture_width = color.shape[:2] + outputs[color_mode] = camera._postprocess_image(color) + + np.testing.assert_array_equal(outputs[ColorMode.RGB], color) + np.testing.assert_array_equal(outputs[ColorMode.BGR], color[..., ::-1]) + + +def test_depth_frame_not_color_converted(img_array_factory): + """Depth frames must bypass color conversion, even when a BGR color_mode is set.""" + camera = RealSenseCamera(RealSenseCameraConfig(serial_number_or_name="042", color_mode=ColorMode.BGR)) + depth = img_array_factory(height=3, width=4, channels=1, dtype=np.uint16)[..., 0] + camera.capture_height, camera.capture_width = depth.shape + + np.testing.assert_array_equal(camera._postprocess_image(depth, depth_frame=True), depth) + + def test_read_before_connect(): config = RealSenseCameraConfig(serial_number_or_name="042") camera = RealSenseCamera(config)