refactor(camera): calibration interface change

- changed the interface for hardware calibration
- added realsense specific calibration code

Author: juelg

extensions/rcs_realsense/src/rcs_realsense/camera.py

@@ -5,6 +5,7 @@
 
 import numpy as np
 import pyrealsense2 as rs
+from rcs.camera.interface import Calibration
 from rcs.camera.hw import HardwareCamera
 from rcs.camera.interface import CameraFrame, DataFrame, Frame, IMUFrame
 
@@ -22,10 +23,12 @@ class RealSenseDevicePipeline:
     pipeline: rs.pipeline
     pipeline_profile: rs.pipeline_profile
     camera: RealSenseDeviceInfo
+    depth_scale: float | None = None
 
 
 class RealSenseCameraSet(HardwareCamera):
     TIMESTAMP_FACTOR = 1e-3
+    DEPTH_SCALE = 10000
 
     def __init__(
         self,
@@ -41,6 +44,7 @@ def __init__(
         self.enable_imu = enable_imu
         self.cameras = cameras
         self._logger = logging.getLogger(__name__)
+        # TODO: safe the last 30 recorded frames
         self._last_frameset: dict[rs.stream, rs.frame] | None = None
         self._frameset_lock = threading.Lock()
         assert (
@@ -168,7 +172,9 @@ def enable_device(self, device_info: RealSenseDeviceInfo, enable_ir_emitter: boo
         if sensor.supports(rs.option.emitter_enabled):
             sensor.set_option(rs.option.emitter_enabled, 1 if enable_ir_emitter else 0)
             sensor.set_option(rs.option.laser_power, self.laser_power)
-        self._enabled_devices[device_info.serial] = RealSenseDevicePipeline(pipeline, pipeline_profile, device_info)
+        self._enabled_devices[device_info.serial] = RealSenseDevicePipeline(
+            pipeline, pipeline_profile, device_info, depth_scale=sensor.get_depth_scale()
+        )
         self._logger.debug("Enabled device %s (%s)", device_info.serial, device_info.product_line)
 
     @staticmethod
@@ -255,7 +261,12 @@ def to_ts(frame: rs.frame) -> float:
             timestamps.append(to_ts(frame))
 
         assert color is not None, "Color frame not found"
-        cf = CameraFrame(color=color, ir=ir, depth=depth)
+        # TODO: check depth type (should be int16)
+        cf = CameraFrame(
+            color=color,
+            ir=ir,
+            depth=(depth.astype(np.float64) * device.depth_scale / self.DEPTH_SCALE).astype(np.int16),
+        )
         imu = IMUFrame(accel=accel, gyro=gyro)
         return Frame(camera=cf, imu=imu, avg_timestamp=float(np.mean(timestamps)) if len(timestamps) > 0 else None)
 
@@ -303,7 +314,7 @@ def get_device_intrinsics(
                 device_intrinsics[device_name][key] = value.get_profile().as_video_stream_profile().get_intrinsics()
         return device_intrinsics
 
-    def get_depth_to_color_extrinsics(self, frames):
+    def get_depth_to_color_extrinsics(self, frames: dict[rs.stream, rs.frame]):
         """
         Get the extrinsics between the depth imager 1 and the color imager using its frame delivered by the realsense device
 
@@ -353,7 +364,25 @@ def enable_emitter(self, enable_ir_emitter=True):
             if enable_ir_emitter:
                 sensor.set_option(rs.option.laser_power, self.laser_power)
 
-
-    def intrinsics(self, camera_name: str) -> np.ndarray[tuple[typing.Literal[3, 4]], np.dtype[np.float64]]:
-        with self._frameset_lock:
-            pass
+    def calibrate(self) -> dict[str, Calibration] | None:
+
+        intrinsics = {}
+        for camera_name, camera_config in self.cameras:
+            serial = camera_config.identifier
+            device = self._enabled_devices[serial]
+            vp = device.pipeline_profile.get_stream(rs.stream.color).as_video_stream_profile()
+            intrinsics[camera_name] = vp.get_intrinsics()
+
+        extrinsics = {}
+        for camera_name, camera_config in self.cameras:
+            serial = camera_config.identifier
+            device = self._enabled_devices[serial]
+            depth_vp = device.pipeline_profile.get_stream(rs.stream.depth).as_video_stream_profile()
+            color_vp = device.pipeline_profile.get_stream(rs.stream.color).as_video_stream_profile()
+            depth_to_color = depth_vp.get_extrinsics_to(color_vp)
+
+            # calibrate strategy: save the last 30 frames and send it
+            # it can
+            # - ask for more new frames
+            # - calibration fail
+            # - calibration successful

python/rcs/camera/hw.py

@@ -35,7 +35,7 @@ def poll_frame(self, camera_name: str) -> Frame:
     def camera_names(self) -> list[str]:
         """Returns the names of the cameras in this set."""
 
-    def intrinsics(self, camera_name: str) -> np.ndarray[tuple[typing.Literal[3, 4]], np.dtype[np.float64]]
+    def calibrate(self) -> dict[str, Calibration] | None:
         """Returns camera intrinsics"""
 
 
@@ -63,6 +63,7 @@ def __init__(
         self._next_ring_index = 0
         self._buffer_len = 0
         self.writer: dict[str, cv2.VideoWriter] = {}
+        self._calibration: dict[str, Calibration] | None = None
 
     @property
     def camera_names(self) -> list[str]:
@@ -181,14 +182,20 @@ def warm_up(self):
                 self.poll_frame(camera_name)
             self.rate_limiter()
 
-    def get_calibration(self, camera_name) -> Calibration:
-        # get intrinsics from the camera and check that extrinsics had been calibrated
-        pass
+    def get_calibration(self) -> dict[str, Calibration] | None:
+        return self._calibration
+
+    def calibrate(self) -> bool:
+        self._calibration = None
+        cal = {}
+        for camera in self.cameras:
+            c = camera.calibrate()
+            if c is None:
+                return False
+            cal.update(cal)
+        self._calibration = cal
+        return True
 
-    def calibrate(self, camera_name) -> bool:
-        # calibrate extrinsics with aruca marker
-        # lets provide a calibration function
-        pass
 
     def polling_thread(self, warm_up: bool = True):
         for camera in self.cameras:

python/rcs/camera/interface.py

@@ -73,11 +73,11 @@ def close(self):
     def config(self, camera_name: str) -> BaseCameraConfig:
         """Returns the configuration object of the cameras."""
 
-    def get_calibration(self, camera_name) -> Calibration:
-        """Returns camera calibration"""
+    def get_calibration(self) -> dict[str, Calibration] | None:
+        """Returns cameras' calibration. None if not calibrated or previous calibration failed."""
 
-    def calibrate(self, camera_name) -> bool:
-        """Calibrates the camera"""
+    def calibrate(self) -> bool:
+        """Calibrates the cameras. Returns calibration success"""
 
     @property
     def camera_names(self) -> list[str]:

python/rcs/camera/sim.py

@@ -108,10 +108,13 @@ def _extrinsics(self, camera_name) -> np.ndarray[tuple[Literal[4, 4]], np.dtype[
 
         return cam.inverse().pose_matrix()
 
-    def get_calibration(self, camera_name) -> Calibration:
-        return Calibration(intrinsics=self._intrinsics(camera_name), extrinsics=self._extrinsics(camera_name))
+    def get_calibration(self) -> dict[str, Calibration]:
+        return {
+            camera_name: Calibration(intrinsics=self._intrinsics(camera_name), extrinsics=self._extrinsics(camera_name))
+            for camera_name in self.camera_names
+        }
 
-    def calibrate(self, camera_name) -> bool:
+    def calibrate(self) -> bool:
         return True
 
     def config(self, camera_name: str) -> SimCameraConfig: