reachy2_sdk.media.camera

  1"""Reachy Camera module.
  2
  3Define the RGB Camera of Reachy's head (Teleop) and the RGBD Camera of its torso (Depth).
  4Provide access to the frames (color, depth, disparity) and the camera parameters.
  5"""
  6
  7import logging
  8from enum import Enum
  9from typing import Optional, Tuple
 10
 11import cv2
 12import numpy as np
 13import numpy.typing as npt
 14from reachy2_sdk_api.video_pb2 import CameraFeatures, View, ViewRequest
 15from reachy2_sdk_api.video_pb2_grpc import VideoServiceStub
 16
 17from ..utils.utils import invert_affine_transformation_matrix
 18
 19
 20class CameraView(Enum):
 21    """Enumeration for different camera views.
 22
 23    The `CameraView` enum provides options for specifying the view from which
 24    to capture images or video frames. For monocular cameras, LEFT is used as default.
 25    """
 26
 27    LEFT = View.LEFT
 28    RIGHT = View.RIGHT
 29    DEPTH = View.DEPTH
 30
 31
 32class CameraType(Enum):
 33    """Camera names defined in pollen-vision."""
 34
 35    TELEOP = "teleop_head"
 36    DEPTH = "depth_camera"
 37
 38
 39class Camera:
 40    """Camera class represents an RGB camera on the robot.
 41
 42    The Camera class is primarily used for teleoperation cameras but can also be
 43    utilized for the RGB component of the RGB-D torso camera. It provides access
 44    to camera frames and parameters such as intrinsic, extrinsic matrices, and
 45    distortion coefficients. Additionally, it allows for converting pixel coordinates
 46    to world coordinates.
 47    """
 48
 49    def __init__(self, cam_info: CameraFeatures, video_stub: VideoServiceStub) -> None:
 50        """Initialize a Camera instance.
 51
 52        This constructor sets up a camera instance by storing the camera's
 53        information and gRPC video stub for accessing camera-related services.
 54
 55        Args:
 56            cam_info: An instance of `CameraFeatures` containing the camera's
 57                details, such as its name, capabilities, and settings.
 58            video_stub: A `VideoServiceStub` for making gRPC calls to the video
 59                service, enabling access to camera frames, parameters, and other
 60                camera-related functionality.
 61        """
 62        self._logger = logging.getLogger(__name__)
 63        self._cam_info = cam_info
 64        self._video_stub = video_stub
 65
 66    def get_frame(self, view: CameraView = CameraView.LEFT) -> Optional[Tuple[npt.NDArray[np.uint8], int]]:
 67        """Retrieve an RGB frame from the camera.
 68
 69        Args:
 70            view: The camera view to retrieve the frame from. Default is CameraView.LEFT.
 71
 72        Returns:
 73            A tuple containing the frame as a NumPy array in OpenCV format and the timestamp in nanoseconds.
 74            Returns None if no frame is retrieved.
 75        """
 76        frame = self._video_stub.GetFrame(request=ViewRequest(camera_feat=self._cam_info, view=view.value))
 77        if frame.data == b"":
 78            self._logger.warning("No frame retrieved")
 79            return None
 80        np_data = np.frombuffer(frame.data, np.uint8)
 81        img = cv2.imdecode(np_data, cv2.IMREAD_COLOR).astype(np.uint8)
 82        return img, frame.timestamp.ToNanoseconds()
 83
 84    def get_compressed_frame(self, view: CameraView = CameraView.LEFT) -> Optional[Tuple[bytes, int]]:
 85        """Retrieve an RGB frame in a JPEG format from the camera.
 86
 87        Args:
 88            view: The camera view to retrieve the frame from. Default is CameraView.LEFT.
 89
 90        Returns:
 91            A bytes array containing the jpeg frame and the timestamp in nanoseconds.
 92            Returns None if no frame is retrieved.
 93        """
 94        frame = self._video_stub.GetFrame(request=ViewRequest(camera_feat=self._cam_info, view=view.value))
 95        if frame.data == b"":
 96            self._logger.warning("No frame retrieved")
 97            return None
 98        return frame.data, frame.timestamp.ToNanoseconds()
 99
100    def get_parameters(
101        self, view: CameraView = CameraView.LEFT
102    ) -> Optional[
103        Tuple[int, int, str, npt.NDArray[np.float64], npt.NDArray[np.float64], npt.NDArray[np.float64], npt.NDArray[np.float64]]
104    ]:
105        """Retrieve camera parameters including intrinsic matrix.
106
107        Args:
108            view: The camera view for which parameters should be retrieved. Default is CameraView.LEFT.
109
110        Returns:
111            A tuple containing height, width, distortion model, distortion coefficients, intrinsic matrix,
112            rotation matrix, and projection matrix. Returns None if no parameters are retrieved.
113        """
114        params = self._video_stub.GetParameters(request=ViewRequest(camera_feat=self._cam_info, view=view.value))
115        if params.K == []:
116            self._logger.warning("No parameter retrieved")
117            return None
118
119        D = np.array(params.D)
120        K = np.array(params.K).reshape((3, 3))
121        R = np.array(params.R).reshape((3, 3))
122        P = np.array(params.P).reshape((3, 4))
123
124        return params.height, params.width, params.distortion_model, D, K, R, P
125
126    def get_extrinsics(self, view: CameraView = CameraView.LEFT) -> Optional[npt.NDArray[np.float64]]:
127        """Retrieve the 4x4 extrinsic matrix of the camera.
128
129        Args:
130            view: The camera view for which the extrinsic matrix should be retrieved. Default is CameraView.LEFT.
131
132        Returns:
133            The extrinsic matrix as a NumPy array. Returns None if no matrix is retrieved.
134        """
135        res = self._video_stub.GetExtrinsics(request=ViewRequest(camera_feat=self._cam_info, view=view.value))
136        if res.extrinsics is None:
137            self._logger.warning("No extrinsic matrix retrieved")
138            return None
139        return np.array(res.extrinsics.data).reshape((4, 4))
140
141    def pixel_to_world(
142        self, u: int, v: int, z_c: float = 1.0, view: CameraView = CameraView.LEFT
143    ) -> Optional[npt.NDArray[np.float64]]:
144        """Convert pixel coordinates to XYZ coordinate in Reachy coordinate system.
145
146        Args:
147            u: The x-coordinate (pixel) in the camera view (horizontal axis, left-to-right).
148            v: The y-coordinate (pixel) in the camera view (vertical axis, top-to-bottom).
149            z_c: The depth value in meters at the given pixel. Default is 1.0.
150            view: The camera view to use for the conversion. Default is CameraView.LEFT.
151
152        Returns:
153            A NumPy array containing the [X, Y, Z] world coordinates in meters. Returns None if the conversion fails.
154        """
155        params = self.get_parameters(view)
156
157        if params is None:
158            return None
159
160        height, width, distortion_model, D, K, R, P = params
161
162        if u < 0 or u > width:
163            self._logger.warning(f"u value should be between 0 and {width}")
164            return None
165        if v < 0 or v > height:
166            self._logger.warning(f"v value should be between 0 and {height}")
167            return None
168
169        T_cam_world = self.get_extrinsics(view)
170        if T_cam_world is None:
171            return None
172
173        T_world_cam = invert_affine_transformation_matrix(T_cam_world)
174
175        uv_homogeneous = np.array([u, v, 1])
176        camera_coords = np.linalg.inv(K) @ uv_homogeneous
177        camera_coords_homogeneous = np.array([camera_coords[0] * z_c, camera_coords[1] * z_c, z_c, 1])
178
179        world_coords_homogeneous = T_world_cam @ camera_coords_homogeneous
180
181        return np.array(world_coords_homogeneous[:3])
182
183    def __repr__(self) -> str:
184        """Clean representation of a RGB camera."""
185        if self._cam_info.name == CameraType.TELEOP.value:
186            name = "teleop"
187        elif self._cam_info.name == CameraType.DEPTH.value:
188            name = "depth"
189        else:
190            name = self._cam_info.name
191        return f"""<Camera name="{name}" stereo={self._cam_info.stereo}> """
192
193
194class DepthCamera(Camera):
195    """DepthCamera class represents the depth component of the RGB-D torso camera.
196
197    It provides access to depth frames and extends the functionality
198    of the Camera class, allowing users to retrieve depth information.
199    """
200
201    def get_depth_frame(self, view: CameraView = CameraView.DEPTH) -> Optional[Tuple[npt.NDArray[np.uint16], int]]:
202        """Retrieve a depth frame from the camera.
203
204        Args:
205            view: The camera view to retrieve the depth frame from. Default is CameraView.DEPTH.
206
207        Returns:
208            A tuple containing the depth frame as a NumPy array in 16-bit format and the timestamp in nanoseconds.
209            Returns None if no frame is retrieved.
210        """
211        frame = self._video_stub.GetDepth(request=ViewRequest(camera_feat=self._cam_info, view=view.value))
212        if frame.data == b"":
213            self._logger.error("No frame retrieved")
214            return None
215
216        if frame.encoding != "16UC1":
217            self._logger.error("Depth is not encoded in 16bit")
218        np_data = np.frombuffer(frame.data, np.uint16)
219        np_data = np_data.reshape((frame.height, frame.width))
220
221        return np_data, frame.timestamp.ToNanoseconds()