refactor: calibration with list instead of queue

Author: juelg

extensions/rcs_realsense/src/rcs_realsense/calibration.py

@@ -1,11 +1,13 @@
 import logging
+import threading
 import typing
-from queue import Queue
 from time import sleep
 
 import apriltag
 import cv2
 import numpy as np
+from PIL import Image, ImageDraw
+from rcs._core import common
 from rcs.camera.hw import CalibrationStrategy
 from rcs.camera.interface import Frame
 from tqdm import tqdm
@@ -20,39 +22,37 @@ def __init__(self, camera_name: str):
         # base frame to camera, world to base frame
         self._extrinsics: np.ndarray[tuple[typing.Literal[4], typing.Literal[4]], np.dtype[np.float64]] | None = None
         self.camera_name = camera_name
-        self.world_to_tag = np.array(
-            [
-                [0, -1, 0, 0.2],
-                [-1, 0, 0, 0],
-                [0, 0, -1, 0],
-                [0, 0, 0, 1],
-            ]
-        )
+        self.tag_to_world = common.Pose(rpy_vector=[np.pi, 0, -np.pi / 2], translation=[0.2, 0, 0]).pose_matrix()
 
     def calibrate(
         self,
-        samples: Queue[Frame],
+        samples: list[Frame],
         intrinsics: np.ndarray[tuple[typing.Literal[3], typing.Literal[4]], np.dtype[np.float64]],
+        lock: threading.Lock,
     ) -> bool:
         logger.info("Calibrating camera %s. Position it as you wish and press enter.", self.camera_name)
         input()
         tries = 3
-        while samples.qsize() < samples.maxsize - 1 and tries > 0:
+        while len(samples) < 10 and tries > 0:
             logger.info("not enought frames in recorded, waiting 2 seconds...")
             tries = -1
             sleep(2)
         if tries == 0:
             logger.warning("Calibration failed, not enough frames arrived.")
             return False
+
         frames = []
-        for _ in samples.qsize():
-            frames.append(samples.get())
+        with lock:
+            for sample in samples:
+                frames.append(sample.camera.color.data.copy())
+        print(frames)
 
-        _, cam_to_tag, _ = get_average_marker_pose(
-            frames, intrinsics=intrinsics, calib_tag_id=9, show_live_window=False
+        last_frame, tag_to_cam = get_average_marker_pose(
+            frames, intrinsics=intrinsics, calib_tag_id=9, show_live_window=True
         )
 
-        world_to_cam = self.world_to_tag @ np.linalg.inv(cam_to_tag)
+        cam_to_world = self.tag_to_world @ np.linalg.inv(tag_to_cam)
+        world_to_cam = np.linalg.inv(cam_to_world)
         self._extrinsics = world_to_cam
         return True
 
@@ -89,7 +89,7 @@ def get_average_marker_pose(
 
         last_frame = frame.copy()
 
-        camera_matrix = intrinsics
+        camera_matrix = intrinsics[:3, :3]
 
         if show_live_window:
             cv2.drawFrameAxes(frame, camera_matrix, None, pose[:3, :3], pose[:3, 3], 0.1)
@@ -109,9 +109,8 @@ def get_average_marker_pose(
     logger.info(f"Average pose: {avg_pose}")
 
     # paint avg pose on last frame
-    if show_live_window:
-        cv2.drawFrameAxes(last_frame, camera_matrix, None, avg_pose[:3, :3], avg_pose[:3, 3], 0.1)  # type: ignore
-    return last_frame, avg_pose, camera_matrix
+    # cv2.drawFrameAxes(last_frame, camera_matrix, None, avg_pose[:3, :3], avg_pose[:3, 3], 0.1)  # type: ignore
+    return last_frame, avg_pose
 
 
 def get_marker_pose(calib_tag_id, detector, intrinsics, frame):

extensions/rcs_realsense/src/rcs_realsense/camera.py

@@ -1,5 +1,6 @@
+import copy
 import logging
-import queue
+import threading
 import typing
 from dataclasses import dataclass
 
@@ -59,7 +60,8 @@ def __init__(
         self.resolution_width = sample_camera_config.resolution_width
         self.resolution_height = sample_camera_config.resolution_height
         self.frame_rate = sample_camera_config.frame_rate
-        self._frame_buffer: dict[str, queue.Queue] = {}
+        self._frame_buffer_lock: dict[str, threading.Lock] = {}
+        self._frame_buffer: dict[str, list] = {}
 
         self.D400_config = rs.config()
         self.D400_config.enable_stream(
@@ -181,17 +183,21 @@ def enable_device(self, camera_name: str, device_info: RealSenseDeviceInfo, enab
         color_vp = pipeline_profile.get_stream(rs.stream.color).as_video_stream_profile()
 
         rs_color_intrinsics = color_vp.get_intrinsics()
-        color_intrinsics = np.array([
-            [rs_color_intrinsics.fx, 0, (rs_color_intrinsics.width-1)/2, 0],
-            [0, rs_color_intrinsics.fy, (rs_color_intrinsics.height-1)/2, 0],
-            [0, 0, 1, 0],
-        ])
+        color_intrinsics = np.array(
+            [
+                [rs_color_intrinsics.fx, 0, (rs_color_intrinsics.width - 1) / 2, 0],
+                [0, rs_color_intrinsics.fy, (rs_color_intrinsics.height - 1) / 2, 0],
+                [0, 0, 1, 0],
+            ]
+        )
         rs_depth_intrinsics = depth_vp.get_intrinsics()
-        depth_intrinsics = np.array([
-            [rs_depth_intrinsics.fx, 0, (rs_depth_intrinsics.width-1)/2, 0],
-            [0, rs_depth_intrinsics.fy, (rs_depth_intrinsics.height-1)/2, 0],
-            [0, 0, 1, 0],
-        ])
+        depth_intrinsics = np.array(
+            [
+                [rs_depth_intrinsics.fx, 0, (rs_depth_intrinsics.width - 1) / 2, 0],
+                [0, rs_depth_intrinsics.fy, (rs_depth_intrinsics.height - 1) / 2, 0],
+                [0, 0, 1, 0],
+            ]
+        )
 
         depth_to_color = depth_vp.get_extrinsics_to(color_vp)
 
@@ -202,10 +208,13 @@ def enable_device(self, camera_name: str, device_info: RealSenseDeviceInfo, enab
             depth_scale=sensor.get_depth_scale(),
             color_intrinsics=color_intrinsics,
             depth_intrinsics=depth_intrinsics,
-            depth_to_color=common.Pose(translation=depth_to_color.translation, rotation=np.array(depth_to_color.rotation).reshape(3, 3)),
+            depth_to_color=common.Pose(
+                translation=depth_to_color.translation, rotation=np.array(depth_to_color.rotation).reshape(3, 3)
+            ),
         )
 
-        self._frame_buffer[camera_name] = queue.Queue(maxsize=self.CALIBRATION_FRAME_SIZE)
+        self._frame_buffer[camera_name] = []
+        self._frame_buffer_lock[camera_name] = threading.Lock()
         self._logger.debug("Enabled device %s (%s)", device_info.serial, device_info.product_line)
 
     @staticmethod
@@ -257,7 +266,9 @@ def to_ts(frame: rs.frame) -> float:
 
         color_extrinsics = self.calibration_strategy[camera_name].get_extrinsics()
         depth_to_color = device.depth_to_color
-        depth_extrinsics = color_extrinsics @ depth_to_color.inverse().pose_matrix() if color_extrinsics is not None else None
+        depth_extrinsics = (
+            color_extrinsics @ depth_to_color.inverse().pose_matrix() if color_extrinsics is not None else None
+        )
 
         timestamps = []
         for stream in streams:
@@ -275,7 +286,9 @@ def to_ts(frame: rs.frame) -> float:
             elif rs.stream.depth == stream.stream_type():
                 frame = frameset.get_depth_frame()
                 depth = DataFrame(
-                    data=(to_numpy(frame).astype(np.float64) * device.depth_scale / BaseCameraSet.DEPTH_SCALE).astype(np.int16),
+                    data=(to_numpy(frame).astype(np.float64) * device.depth_scale / BaseCameraSet.DEPTH_SCALE).astype(
+                        np.int16
+                    ),
                     timestamp=to_ts(frame),
                     intrinsics=device.depth_intrinsics,
                     extrinsics=depth_extrinsics,
@@ -302,7 +315,10 @@ def to_ts(frame: rs.frame) -> float:
         )
         imu = IMUFrame(accel=accel, gyro=gyro)
         f = Frame(camera=cf, imu=imu, avg_timestamp=float(np.mean(timestamps)) if len(timestamps) > 0 else None)
-        self._frame_buffer[camera_name].put(f)
+        with self._frame_buffer_lock[camera_name]:
+            if len(self._frame_buffer[camera_name]) >= self.CALIBRATION_FRAME_SIZE:
+                self._frame_buffer[camera_name].pop(0)
+            self._frame_buffer[camera_name].append(copy.deepcopy(f))
         return f
 
     def disable_streams(self):
@@ -329,6 +345,12 @@ def enable_emitter(self, enable_ir_emitter=True):
 
     def calibrate(self) -> bool:
         for camera_name in self.cameras:
-            self.calibration_strategy[camera_name].calibrate(
-                intrinsics=self._enabled_devices[camera_name].color_intrinsics, samples=self._frame_buffer
-            )
+            if not self.calibration_strategy[camera_name].calibrate(
+                intrinsics=self._enabled_devices[camera_name].color_intrinsics,
+                samples=self._frame_buffer[camera_name],
+                lock=self._frame_buffer_lock[camera_name],
+            ):
+                self._logger.warning(f"Calibration of camera {camera_name} failed.")
+                return False
+        self._logger.info("Calibration successful.")
+        return True

python/rcs/camera/hw.py

@@ -50,10 +50,20 @@ class CalibrationStrategy(typing.Protocol):
 
     def calibrate(
         self,
-        samples: Queue[Frame],
+        samples: list[Frame],
         intrinsics: np.ndarray[tuple[typing.Literal[3], typing.Literal[4]], np.dtype[np.float64]],
+        lock: threading.Lock,
     ) -> bool:
-        """Implements algorithm to calibrate the camera."""
+        """Implements algorithm to calibrate the camera.
+
+        Args:
+            samples: List of frames to use for calibration.
+            intrinsics: Intrinsic camera parameters, e.g. from a previous calibration.
+            lock: A lock to ensure thread safety during calibration as the samples might refresh in parallel.
+
+        Returns:
+            bool: True if calibration was successful, False otherwise.
+        """
 
     def get_extrinsics(self) -> np.ndarray[tuple[typing.Literal[4], typing.Literal[4]], np.dtype[np.float64]] | None:
         """
@@ -67,8 +77,9 @@ class DummyCalibrationStrategy(CalibrationStrategy):
 
     def calibrate(
         self,
-        samples: Queue[Frame],
+        samples: list[Frame],
         intrinsics: np.ndarray[tuple[typing.Literal[3], typing.Literal[4]], np.dtype[np.float64]],
+        lock: threading.Lock,
     ) -> bool:
         return True
 
@@ -272,5 +283,5 @@ def poll_frame(self, camera_name: str) -> Frame:
     def __enter__(self):
         pass
 
-    def __exit__(self, **kwargs):
+    def __exit__(self, *args, **kwargs):
         self.close()