Merge branch 'main' into change-internal-dependency-listing

.github/.pull_request_template.md

@@ -0,0 +1,6 @@
+## Describe your changes
+
+
+## Checklist
+- [ ] Have you modified the changelog?
+- [ ] If you made changes to the hardware interfaces, have you tested them using the manual tests?

airo-camera-toolkit/README.md

@@ -1,26 +1,17 @@
 # airo-camera-toolkit
 This package contains code for working with RGB(D) cameras, images and pointclouds.
+
+
 Overview of the functionality and the structure:
 ```cs
-airo_camera_toolkit
-├── interfaces.py               # Common interfaces for all cameras.
-├── reprojection.py             # Projecting points between the 3D world and images
-│                               # and reprojecting points from image plane to world
-├── utils.py                    # Conversion between image formats: BGR to RGB, int to float, etc.
-│                               # or channel-first vs channel-last.
-├── annotation_tools.py         # Tool for annotating images with keypoints, lines, etc.
-└── cameras                     # Implementation of the interfaces for real cameras
-│   ├── zed2i.py                # Implementation using ZED SDK, run this file to test your ZED Installation
-│   ├── realsense.py            # Implementation using RealSense SDK
-│   └── manual_test_hw.py       # Used for manually testing in the above implementations.
-└── calibration
-│   ├── fiducial_markers.py     # Detecting and localising aruco markers and charuco boards
-│   └── hand_eye_calibration.py # Camera-robot extrinsics calibration, eye-in-hand and eye-to-hand
-└── image_transforms            # Invertible transforms for cropping/scaling images with keypoints
-│   └── ...
-└── multiprocess                # Multiprocessing for
-    └── ...
-
+airo-camera-toolkit/
+├── calibration/                # hand-eye extrinsics calibration
+├── cameras/                    # actual camera drivers
+├── image_transformations/      # reversible geometric 2D transforms
+├── pinhole_operations/         # 2D-3D operations
+├── utils/                      # a.o. annotation tool and converter
+├── interfaces.py
+└── cli.py
 ```
 
 ## Installation
@@ -63,7 +54,10 @@ airo-camera-toolkit hand-eye-calibration --help
 See [calibration/README.md](./airo_camera_toolkit/calibration/README.md) for more details.
 
 
-## Image format conversion
+
+## Utils
+
+### Image format conversion
 Camera by default return images as numpy 32-bit float RGB images with values between 0 to 1 through `get_rgb_image()`.
 This is most convenient for subsequent processing, e.g. with neural networks.
 For higher performance, 8-bit unsigned integer RGB images are also accessible through `get_rgb_image_as_int()`.
@@ -78,13 +72,15 @@ image_bgr = ImageConverter.from_numpy_int_format(image_rgb_int).image_in_opencv_
 ```
 
 
-## Reprojection
+### Annotation tool
 
-See [reprojection.py](./airo_camera_toolkit/reprojection.py) for more details.
+See [annotation_tool.md](./airo_camera_toolkit/annotation_tool.md) for usage instructions.
 
-## Annotation tool
 
-See [annotation_tool.md](./airo_camera_toolkit/annotation_tool.md) for usage instructions.
+## Pinhole Operations
+
+2D - 3D geometric operations using the pinhole model. See [readme](./airo_camera_toolkit/pinhole_operations/Readme.md) for more information.
+
 
 ## Image Transforms
 
@@ -100,6 +96,7 @@ If this is a problem for your application, see [multiprocess/README.md](./airo_c
 
 ## References
 For more background on cameras, in particular on the meaning of intrinsics, extrinics, distortion coefficients, pinhole (and other) camera models, see:
+- Szeliski - Computer vision: Algorithms and Applications, available [here](https://szeliski.org/Book/)
  - https://web.eecs.umich.edu/~justincj/teaching/eecs442/WI2021/schedule.html
  - https://learnopencv.com/geometry-of-image-formation/ (extrinsics & intrinsics)
  - http://www.cs.cmu.edu/~16385/s17/Slides/11.1_Camera_matrix.pdf (idem)

airo-camera-toolkit/airo_camera_toolkit/calibration/collect_calibration_data.py

@@ -9,7 +9,7 @@
 from airo_camera_toolkit.calibration.fiducial_markers import detect_and_visualize_charuco_pose
 from airo_camera_toolkit.cameras.camera_discovery import click_camera_options, discover_camera
 from airo_camera_toolkit.interfaces import RGBCamera
-from airo_camera_toolkit.utils import ImageConverter
+from airo_camera_toolkit.utils.image_converter import ImageConverter
 from airo_dataset_tools.data_parsers.camera_intrinsics import CameraIntrinsics
 from airo_dataset_tools.data_parsers.pose import Pose
 from airo_robots.manipulators.hardware.ur_rtde import URrtde

airo-camera-toolkit/airo_camera_toolkit/calibration/fiducial_markers.py

@@ -9,7 +9,6 @@
 
 import cv2
 import numpy as np
-from airo_camera_toolkit.cameras.camera_discovery import click_camera_options, discover_camera
 from airo_spatial_algebra import SE3Container
 from airo_typing import CameraIntrinsicsMatrixType, HomogeneousMatrixType, OpenCVIntImageType
 from cv2 import aruco
@@ -249,7 +248,8 @@ def detect_and_visualize_charuco_pose(
     Defaults to the AIRO_DEFAULT_CHARUCO_BOARD.
     """
     import click
-    from airo_camera_toolkit.utils import ImageConverter
+    from airo_camera_toolkit.cameras.camera_discovery import click_camera_options, discover_camera
+    from airo_camera_toolkit.utils.image_converter import ImageConverter
 
     @click.command()
     @click.option("--aruco_marker_size", default=0.031, help="Size of the aruco marker in meters")

airo-camera-toolkit/airo_camera_toolkit/calibration/hand_eye_calibration.py

@@ -18,7 +18,7 @@
     detect_and_visualize_charuco_pose,
 )
 from airo_camera_toolkit.interfaces import RGBCamera
-from airo_camera_toolkit.utils import ImageConverter
+from airo_camera_toolkit.utils.image_converter import ImageConverter
 from airo_dataset_tools.data_parsers.camera_intrinsics import CameraIntrinsics
 from airo_robots.manipulators.hardware.ur_rtde import URrtde
 from airo_robots.manipulators.position_manipulator import PositionManipulator

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

@@ -2,13 +2,20 @@
 # Cameras
 This subpackage contains implementations of the camera interface for the cameras we have at AIRO.
 
+- ZED 2i
+- Realsense D400 series
+
+It also contains code to enable multiprocessed use of the camera streams: [multiprocessed camera](./multiprocess/)
+
 ## 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:
-* [ZED Installation](zed_installation.md)
+* [ZED Installation](zed/installation.md)
+* [RealSense Installation](realsense/realsense_installation.md)
+
 
 ## 2. Testing your hardware installation
-Furthermore, there is code for testing the hardware implementations.
+Furthermore, there is code for testing the hardware implementations: `manual_test_hw.py`
 But since this requires attaching a physical camera, these are 'user tests' which should be done manually by developers/users.
 Each camera implementation can be run as a script and will execute the relevant tests, providing instructions on what to look out for.
 

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

@@ -0,0 +1,2 @@
+from airo_camera_toolkit.cameras.realsense.realsense import Realsense  # noqa: F401
+from airo_camera_toolkit.cameras.zed.zed2i import Zed2i  # noqa: F401

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

@@ -44,11 +44,11 @@ def discover_camera(brand: Optional[str], serial_number: Optional[str] = None, *
     brand_enum = CameraBrand(brand)  # Attempt to convert to enum
 
     if brand_enum == CameraBrand.ZED:
-        from airo_camera_toolkit.cameras.zed2i import Zed2i
+        from airo_camera_toolkit.cameras.zed.zed2i import Zed2i
 
         camera = Zed2i(serial_number=serial_number, **kwargs)
     elif brand_enum == CameraBrand.REALSENSE:
-        from airo_camera_toolkit.cameras.realsense import Realsense
+        from airo_camera_toolkit.cameras.realsense.realsense import Realsense
 
         camera = Realsense(serial_number=serial_number, **kwargs)  # type: ignore
     else:
@@ -88,7 +88,7 @@ def my_command(camera_brand: Optional[str] = None,
 if __name__ == "__main__":
     import click
     import cv2
-    from airo_camera_toolkit.utils import ImageConverter
+    from airo_camera_toolkit.utils.image_converter import ImageConverter
 
     @click.command()
     @click_camera_options

airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_rerun_logger.py

@@ -7,7 +7,7 @@
 from airo_camera_toolkit.cameras.multiprocess.multiprocess_rgb_camera import MultiprocessRGBReceiver
 from airo_camera_toolkit.cameras.multiprocess.multiprocess_rgbd_camera import MultiprocessRGBDReceiver
 from airo_camera_toolkit.image_transforms.image_transform import ImageTransform
-from airo_camera_toolkit.utils import ImageConverter
+from airo_camera_toolkit.utils.image_converter import ImageConverter
 
 logger = loguru.logger
 

airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_rgb_camera.py

@@ -7,7 +7,7 @@
 import cv2
 import numpy as np
 from airo_camera_toolkit.interfaces import RGBCamera
-from airo_camera_toolkit.utils import ImageConverter
+from airo_camera_toolkit.utils.image_converter import ImageConverter
 from airo_typing import CameraIntrinsicsMatrixType, CameraResolutionType, NumpyFloatImageType, NumpyIntImageType
 
 _RGB_SHM_NAME = "rgb"
@@ -285,7 +285,7 @@ def __del__(self) -> None:
     """example of how to use the MultiprocessRGBPublisher and MultiprocessRGBReceiver.
     You can also use the MultiprocessRGBReceiver in a different process (e.g. in a different python script)
     """
-    from airo_camera_toolkit.cameras.zed2i import Zed2i
+    from airo_camera_toolkit.cameras.zed.zed2i import Zed2i
 
     namespace = "camera"
 

airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_rgbd_camera.py

@@ -171,7 +171,7 @@ def __del__(self) -> None:
     You can also use the MultiprocessRGBDReceiver in a different process (e.g. in a different python script)
     """
 
-    from airo_camera_toolkit.cameras.zed2i import Zed2i
+    from airo_camera_toolkit.cameras.zed.zed2i import Zed2i
 
     resolution = Zed2i.RESOLUTION_720
 

airo-camera-toolkit/airo_camera_toolkit/cameras/multiprocess/multiprocess_video_recorder.py

@@ -7,7 +7,7 @@
 
 from airo_camera_toolkit.cameras.multiprocess.multiprocess_rgb_camera import MultiprocessRGBReceiver
 from airo_camera_toolkit.image_transforms.image_transform import ImageTransform
-from airo_camera_toolkit.utils import ImageConverter
+from airo_camera_toolkit.utils.image_converter import ImageConverter
 
 
 class MultiprocessVideoRecorder(Process):

airo-camera-toolkit/airo_camera_toolkit/cameras/realsense.py → airo-camera-toolkit/airo_camera_toolkit/cameras/realsense/realsense.py

@@ -5,7 +5,7 @@
 import numpy as np
 import pyrealsense2 as rs  # type: ignore
 from airo_camera_toolkit.interfaces import RGBDCamera
-from airo_camera_toolkit.utils import ImageConverter
+from airo_camera_toolkit.utils.image_converter import ImageConverter
 from airo_typing import (
     CameraIntrinsicsMatrixType,
     CameraResolutionType,
@@ -138,7 +138,7 @@ def _retrieve_depth_image(self) -> NumpyIntImageType:
 
 
 if __name__ == "__main__":
-    import airo_camera_toolkit.cameras.test_hw as test
+    import airo_camera_toolkit.cameras.manual_test_hw as test
     import cv2
 
     camera = Realsense(fps=30, resolution=Realsense.RESOLUTION_1080, enable_hole_filling=True)

airo-camera-toolkit/airo_camera_toolkit/cameras/zed2i.py → airo-camera-toolkit/airo_camera_toolkit/cameras/zed/zed2i.py

@@ -21,9 +21,8 @@
 import time
 
 import numpy as np
-from airo_camera_toolkit.cameras.test_hw import manual_test_stereo_rgbd_camera
 from airo_camera_toolkit.interfaces import StereoRGBDCamera
-from airo_camera_toolkit.utils import ImageConverter
+from airo_camera_toolkit.utils.image_converter import ImageConverter
 from airo_typing import (
     CameraIntrinsicsMatrixType,
     CameraResolutionType,
@@ -287,6 +286,7 @@ def __exit__(self, exc_type: Any, exc_value: Any, traceback: Any) -> None:
 
 if __name__ == "__main__":
     """this script serves as a 'test' for the zed implementation."""
+    from airo_camera_toolkit.cameras.manual_test_hw import manual_test_stereo_rgbd_camera, profile_rgb_throughput
 
     # zed specific tests:
     # - list all serial numbers of the cameras
@@ -301,7 +301,6 @@ def __exit__(self, exc_type: Any, exc_value: Any, traceback: Any) -> None:
         manual_test_stereo_rgbd_camera(zed)
 
     # profile rgb throughput, should be at 60FPS, i.e. 0.017s
-    from airo_camera_toolkit.cameras.test_hw import profile_rgb_throughput
 
     zed = Zed2i(Zed2i.RESOLUTION_720, fps=60)
     profile_rgb_throughput(zed)

airo-camera-toolkit/airo_camera_toolkit/pinhole_operations/Readme.md

@@ -0,0 +1,16 @@
+# Pinhole Operations
+
+This subpackage contains code to perform various geometric operations to convert between 3D positions and 2D image coordinates using the pinhole camera model.
+
+Most notably these include:
+
+- projection to go from 3D points to 2D image coordinates
+- triangulation to go from a set of corresponding image coordinates to a 3D point
+- unprojection to go from a 2D coordinate to a 3D point by intersecting the ray with depth information
+
+
+For mathematical background, see:
+
+- IRM course
+- Sleziski, [Computer Vision and Algorithms](https://szeliski.org/Book/)
+- Opencv page on [cameras](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#)

airo-camera-toolkit/airo_camera_toolkit/pinhole_operations/__init__.py

@@ -0,0 +1,7 @@
+from .projection import project_points_to_image_plane  # noqa F401
+from .triangulation import calculate_triangulation_errors, multiview_triangulation_midpoint  # noqa F401
+from .unprojection import (  # noqa F401
+    extract_depth_from_depthmap_heuristic,
+    unproject_onto_depth_values,
+    unproject_using_depthmap,
+)

airo-camera-toolkit/airo_camera_toolkit/pinhole_operations/projection.py

@@ -0,0 +1,28 @@
+from typing import Union
+
+from airo_spatial_algebra.operations import _HomogeneousPoints
+from airo_typing import CameraIntrinsicsMatrixType, Vector2DArrayType, Vector3DArrayType, Vector3DType
+
+
+def project_points_to_image_plane(
+    positions_in_camera_frame: Union[Vector3DArrayType, Vector3DType],
+    camera_intrinsics: CameraIntrinsicsMatrixType,
+) -> Vector2DArrayType:
+    """Projects an array of points from the 3D camera frame to their 2D pixel coordinates on the image plane.
+
+    Make sure to transform them to the camera frame first if they are in the world frame.
+
+    Args:
+        positions_in_camera_frame: numpy array of shape (N, 3) containing the 3D positions of the points in the camera frame
+        camera_intrinsics: camera intrinsics matrix as a numpy array of shape (3, 3)
+
+    Returns:
+        numpy array of shape (N, 2) containing the 2D pixel coordinates of the points on the image plane
+    """
+
+    homogeneous_positions_in_camera_frame = _HomogeneousPoints(positions_in_camera_frame).homogeneous_points.T
+    homogeneous_positions_on_image_plane = camera_intrinsics @ homogeneous_positions_in_camera_frame[:3, ...]
+    positions_on_image_plane = (
+        homogeneous_positions_on_image_plane[:2, ...] / homogeneous_positions_on_image_plane[2, ...]
+    )
+    return positions_on_image_plane.T

airo-camera-toolkit/airo_camera_toolkit/pinhole_operations/triangulation.py

@@ -0,0 +1,80 @@
+from typing import List
+
+import numpy as np
+from airo_typing import CameraExtrinsicMatrixType, CameraIntrinsicsMatrixType, Vector2DArrayType, Vector3DType
+
+
+def multiview_triangulation_midpoint(
+    extrinsics_matrices: List[CameraExtrinsicMatrixType],
+    intrinsics_matrices: List[CameraIntrinsicsMatrixType],
+    image_coordinates: Vector2DArrayType,
+) -> Vector3DType:
+    """triangulates a point from multiple views using the midpoint method.
+    This method minimizes the L2 distance in the camera space between the estimated point and all rays
+    cf. https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=8967077
+
+    Args:
+        extrinsics_matrices: list of extrinsics matrices for each viewpoint
+        intrinsics_matrices: list of intrinsics matrices for each viewpoint
+        image_points: list of image coordinates of the 3D point for each viewpoint
+
+    Returns:
+        estimated 3D position in the world frame as a numpy array of shape (3,).
+    """
+
+    # determine the rays for each camera in the world frame
+    rays = []
+    for extrinsics_matrix, intrinsics_matrix, image_point in zip(
+        extrinsics_matrices, intrinsics_matrices, image_coordinates
+    ):
+        ray = (
+            extrinsics_matrix[:3, :3]
+            @ np.linalg.inv(intrinsics_matrix)
+            @ np.array([image_point[0], image_point[1], 1])
+        )
+        ray = ray / np.linalg.norm(ray)
+        rays.append(ray)
+
+    lhs = 0
+    rhs = 0
+    for i, ray in enumerate(rays):
+        rhs += (np.eye(3) - ray[:, np.newaxis] @ ray[np.newaxis, :]) @ extrinsics_matrices[i][:3, 3]
+        lhs += np.eye(3) - ray[:, np.newaxis] @ ray[np.newaxis, :]
+
+    lhs_inv = np.linalg.inv(lhs)
+    midpoint = lhs_inv @ rhs
+    return midpoint
+
+
+def calculate_triangulation_errors(
+    extrinsics_matrices: List[CameraExtrinsicMatrixType],
+    intrinsics_matrices: List[CameraIntrinsicsMatrixType],
+    image_coordinates: Vector2DArrayType,
+    point: Vector3DType,
+) -> List[float]:
+    """calculates the Euclidean distances in the camera space between the estimated point and all rays, as a measure of triangulation error
+
+    Args:
+        extrinsics_matrices: list of extrinsics matrices for each viewpoint
+        intrinsics_matrices: list of intrinsics matrices for each viewpoint
+        image_points: list of image coordinates of the 3D point for each viewpoint
+        point: 3D point in the world frame
+
+    Returns:
+        list of Euclidean distances between the point and the rays for each viewpoint
+    """
+    errors = []
+    for extrinsics_matrix, intrinsics_matrix, image_point in zip(
+        extrinsics_matrices, intrinsics_matrices, image_coordinates
+    ):
+        ray = (
+            extrinsics_matrix[:3, :3]
+            @ np.linalg.inv(intrinsics_matrix)
+            @ np.array([image_point[0], image_point[1], 1])
+        )
+        ray = ray / np.linalg.norm(ray)
+        error = np.linalg.norm(
+            (np.eye(3) - ray[:, np.newaxis] @ ray[np.newaxis, :]) @ ((extrinsics_matrix[:3, 3]) - point)
+        )
+        errors.append(error.item())
+    return errors