OpenCVVideoCapture implementation

CHANGELOG.md

@@ -21,7 +21,7 @@ This project uses a [CalVer](https://calver.org/) versioning scheme with monthly
 - Functions to convert from our numpy-based dataclass to and from open3d point clouds
 - `BoundingBox3DType`
 - `Zed2i.ULTRA_DEPTH_MODE` to enable the ultra depth setting for the Zed2i cameras
-
+- `OpenCVVideoCapture` implementation of `RGBCamera` for working with arbitrary cameras
 
 
 ### Changed

airo-camera-toolkit/airo_camera_toolkit/cameras/README.md

@@ -7,6 +7,8 @@ This subpackage contains implementations of the camera interface for the cameras
 
 It also contains code to enable multiprocessed use of the camera streams: [multiprocessed camera](./multiprocess/)
 
+There is also an implementation for generic RGB cameras using OpenCV `VideoCapture`: [OpenCV VideoCapture](./opencv_videocapture/)
+
 ## 1. Installation
 Implementations usually require the installation of SDKs, drivers etc. to communicate with the camera.
 This information can be found in `READMEs` for each camera:

airo-camera-toolkit/airo_camera_toolkit/cameras/opencv_videocapture/opencv_videocapture.py

@@ -0,0 +1,115 @@
+from __future__ import annotations
+
+import math
+import os
+from typing import Any, Optional, Tuple
+
+import cv2
+from airo_camera_toolkit.interfaces import RGBCamera
+from airo_camera_toolkit.utils.image_converter import ImageConverter
+from airo_typing import CameraIntrinsicsMatrixType, CameraResolutionType, NumpyFloatImageType, NumpyIntImageType
+
+
+class OpenCVVideoCapture(RGBCamera):
+    """Wrapper around OpenCV's VideoCapture so we can test the camera interface without external cameras."""
+
+    # Some standard resolutions that are likely to be supported by webcams.
+    # 16:9
+    RESOLUTION_1080 = (1920, 1080)
+    RESOLUTION_720 = (1280, 720)
+    # 4:3
+    RESOLUTION_768 = (1024, 768)
+    RESOLUTION_480 = (640, 480)
+
+    def __init__(
+        self,
+        video_capture_args: Tuple[Any] = (0,),
+        intrinsics_matrix: Optional[CameraIntrinsicsMatrixType] = None,
+        resolution: CameraResolutionType = RESOLUTION_480,
+        fps: int = 30,
+    ) -> None:
+        self.video_capture = cv2.VideoCapture(*video_capture_args)
+
+        # If passing a video file, we want to check if it exists. Then, we can throw a more meaningful
+        # error if it does not.
+        if len(video_capture_args) > 0 and isinstance(video_capture_args[0], str):
+            if not os.path.isfile(video_capture_args[0]):
+                raise FileNotFoundError(f"Could not find video file {video_capture_args[0]}")
+        if not self.video_capture.isOpened():
+            raise RuntimeError(f"Cannot open camera {video_capture_args[0]}. Is it connected?")
+
+        # Note that the following will not forcibly set the resolution. If the user's webcam
+        # does not support the desired resolution, OpenCV will silently select a close match.
+        self.video_capture.set(cv2.CAP_PROP_FRAME_WIDTH, resolution[0])
+        self.video_capture.set(cv2.CAP_PROP_FRAME_HEIGHT, resolution[1])
+        self.video_capture.set(cv2.CAP_PROP_FPS, fps)
+
+        self._intrinsics_matrix = intrinsics_matrix
+
+        self.fps = self.video_capture.get(cv2.CAP_PROP_FPS)
+        self._resolution = (
+            math.floor(self.video_capture.get(cv2.CAP_PROP_FRAME_WIDTH)),
+            math.floor(self.video_capture.get(cv2.CAP_PROP_FRAME_HEIGHT)),
+        )
+
+    @property
+    def resolution(self) -> CameraResolutionType:
+        return self._resolution
+
+    def __enter__(self) -> RGBCamera:
+        return self
+
+    def __exit__(self, exc_type: Any, exc_value: Any, traceback: Any) -> None:
+        self.video_capture.release()
+
+    def intrinsics_matrix(self) -> CameraIntrinsicsMatrixType:
+        """Obtain the intrinsics matrix of the camera.
+
+        Raises:
+            RuntimeError: You must explicitly pass an intrinsics object to the constructor.
+
+        Returns:
+            CameraIntrinsicsMatrixType: The intrinsics matrix.
+        """
+        if self._intrinsics_matrix is None:
+            raise RuntimeError(
+                "OpenCVVideoCapture does not have a preset intrinsics matrix. Pass it to the constructor if you know it."
+            )
+        return self._intrinsics_matrix
+
+    def _grab_images(self) -> None:
+        ret, image = self.video_capture.read()
+        if not ret:  # When streaming a video, we will at some point reach the end.
+            raise EOFError("Can't receive frame (stream end?). Exiting...")
+
+        self._frame = image
+
+    def _retrieve_rgb_image(self) -> NumpyFloatImageType:
+        return ImageConverter.from_opencv_format(self._frame).image_in_numpy_format
+
+    def _retrieve_rgb_image_as_int(self) -> NumpyIntImageType:
+        return ImageConverter.from_opencv_format(self._frame).image_in_numpy_int_format
+
+
+if __name__ == "__main__":
+    import airo_camera_toolkit.cameras.manual_test_hw as test
+    import numpy as np
+
+    camera = OpenCVVideoCapture(intrinsics_matrix=np.eye(3))
+
+    # Perform tests
+    test.manual_test_camera(camera)
+    test.manual_test_rgb_camera(camera)
+    test.profile_rgb_throughput(camera)
+
+    # Live viewer
+    cv2.namedWindow("OpenCV Webcam RGB", cv2.WINDOW_NORMAL)
+
+    while True:
+        color_image = camera.get_rgb_image_as_int()
+        color_image = ImageConverter.from_numpy_int_format(color_image).image_in_opencv_format
+
+        cv2.imshow("OpenCV Webcam RGB", color_image)
+        key = cv2.waitKey(1)
+        if key == ord("q"):
+            break

airo-camera-toolkit/airo_camera_toolkit/cameras/opencv_videocapture/opencv_videocapture_camera.md

@@ -0,0 +1,6 @@
+# Generic OpenCV camera
+
+This `RGBCamera` implementation allows testing arbitrary cameras through the OpenCV `VideoCapture` interface.
+
+We currently do not support intrinsics calibration in airo-camera-toolkit. You can find the intrinsics of your camera
+using [these instructions](https://docs.opencv.org/4.x/dc/dbb/tutorial_py_calibration.html).