From efd3e20063f77b581da4ac57d17475eb536f30c6 Mon Sep 17 00:00:00 2001
From: Mathieu De Coster <mathieu.decoster@ugent.be>
Date: Wed, 14 Feb 2024 08:27:25 +0100
Subject: [PATCH] OpenCVVideoCapture implementation based on #81

---
 .../airo_camera_toolkit/cameras/README.md     |  2 +
 .../cameras/generic_opencv/__init__.py        |  0
 .../cameras/generic_opencv/generic_opencv.py  | 80 +++++++++++++++++++
 .../generic_opencv/generic_opencv_camera.md   |  6 ++
 4 files changed, 88 insertions(+)
 create mode 100644 airo-camera-toolkit/airo_camera_toolkit/cameras/generic_opencv/__init__.py
 create mode 100644 airo-camera-toolkit/airo_camera_toolkit/cameras/generic_opencv/generic_opencv.py
 create mode 100644 airo-camera-toolkit/airo_camera_toolkit/cameras/generic_opencv/generic_opencv_camera.md

diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/README.md b/airo-camera-toolkit/airo_camera_toolkit/cameras/README.md
index db373564..8867e288 100644
--- a/airo-camera-toolkit/airo_camera_toolkit/cameras/README.md
+++ b/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`: [generic OpenCV camera](./generic_opencv/)
+
 ## 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:
diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/generic_opencv/__init__.py b/airo-camera-toolkit/airo_camera_toolkit/cameras/generic_opencv/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/generic_opencv/generic_opencv.py b/airo-camera-toolkit/airo_camera_toolkit/cameras/generic_opencv/generic_opencv.py
new file mode 100644
index 00000000..d21ebd26
--- /dev/null
+++ b/airo-camera-toolkit/airo_camera_toolkit/cameras/generic_opencv/generic_opencv.py
@@ -0,0 +1,80 @@
+from __future__ import annotations
+
+import math
+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 VideoCapure so we can test the camera interface without external cameras."""
+
+    def __init__(
+        self, video_capture_args: Tuple[Any] = (0,), intrinsics_matrix: Optional[CameraIntrinsicsMatrixType] = None
+    ) -> None:
+        self.video_capture = cv2.VideoCapture(*video_capture_args)
+        if not self.video_capture.isOpened():
+            raise RuntimeError("Cannot open camera")
+
+        self.fps = self.video_capture.get(cv2.CAP_PROP_FPS)
+        self._intrinsics_matrix = intrinsics_matrix
+
+        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):
+        ret, image = self.video_capture.read()
+        if not ret:
+            raise RuntimeError("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__":
+    camera = OpenCVVideoCapture()
+
+    while True:
+        image = camera.get_rgb_image()
+        print(image.shape)
+        image = ImageConverter.from_numpy_format(image).image_in_opencv_format
+
+        cv2.imshow("VideoCapture", image)
+        key = cv2.waitKey(10)
+        if key == ord("q"):
+            break
diff --git a/airo-camera-toolkit/airo_camera_toolkit/cameras/generic_opencv/generic_opencv_camera.md b/airo-camera-toolkit/airo_camera_toolkit/cameras/generic_opencv/generic_opencv_camera.md
new file mode 100644
index 00000000..1f7e773d
--- /dev/null
+++ b/airo-camera-toolkit/airo_camera_toolkit/cameras/generic_opencv/generic_opencv_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).