Merge pull request #276 from zivid/MISC-2024-06-25-marker-detection-and-handeye

Add marker detection and hand-eye API wrappers

Author: johningve

modules/zivid/_calibration/detector.py

@@ -4,14 +4,16 @@
 the zivid.calibration module.
 """
 
+import numpy
+
 import _zivid
 from zivid.camera import Camera
 from zivid.frame import Frame
 from zivid._calibration.pose import Pose
 
 
 class DetectionResult:
-    """Class representing detected feature points."""
+    """Class representing detected feature points from a calibration board."""
 
     def __init__(self, impl):
         """Initialize DetectionResult wrapper.
@@ -69,6 +71,203 @@ def __str__(self):
         return str(self.__impl)
 
 
+class MarkerShape:
+    """Holds physical (3D) and image (2D) properties of a detected fiducial marker."""
+
+    def __init__(self, impl):
+        """Initialize MarkerShape wrapper.
+
+        This constructor is only used internally, and should not be called by the end-user.
+
+        Args:
+            impl:   Reference to internal/back-end instance.
+
+        Raises:
+            TypeError: If argument does not match the expected internal class.
+        """
+        if not isinstance(impl, _zivid.calibration.MarkerShape):
+            raise TypeError(
+                "Unsupported type for argument impl. Got {}, expected {}".format(
+                    type(impl), _zivid.calibration.MarkerShape
+                )
+            )
+
+        self.__impl = impl
+
+    @property
+    def corners_in_pixel_coordinates(self):
+        """Get 2D image coordinates of the corners of the detected marker.
+
+        Returns:
+            Four 2D corner coordinates as a 4x2 numpy array
+        """
+        return numpy.array(self.__impl.corners_in_pixel_coordinates())
+
+    @property
+    def corners_in_camera_coordinates(self):
+        """Get 3D spatial coordinates of the corners of the detected marker.
+
+        Returns:
+            Four 3D corner coordinates as a 4x3 numpy array
+        """
+        return numpy.array(self.__impl.corners_in_camera_coordinates())
+
+    @property
+    def identifier(self):
+        """Get the id of the detected marker.
+
+        Returns:
+            Id as int
+        """
+        return self.__impl.id_()
+
+    @property
+    def pose(self):
+        """Get 3D pose of the marker.
+
+        Returns:
+            The Pose of the marker center (4x4 transformation matrix)
+        """
+        return Pose(self.__impl.pose().to_matrix())
+
+
+class MarkerDictionary:
+    """Holds information about fiducial markers such as ArUco or AprilTag for detection.
+
+    This class's properties describe the different dictionaries available, for example
+    aruco4x4_50 describes the ArUco dictionary with 50 markers of size 4x4.
+
+    For more information on ArUco markers see the OpenCV documentation on ArUco markers:
+    https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html,
+
+    To get more information about fiducial markers in general, refer to the wikipedia page:
+    https://en.wikipedia.org/wiki/Fiducial_marker
+    """
+
+    aruco4x4_50 = "aruco4x4_50"
+    aruco4x4_100 = "aruco4x4_100"
+    aruco4x4_250 = "aruco4x4_250"
+    aruco4x4_1000 = "aruco4x4_1000"
+    aruco5x5_50 = "aruco5x5_50"
+    aruco5x5_100 = "aruco5x5_100"
+    aruco5x5_250 = "aruco5x5_250"
+    aruco5x5_1000 = "aruco5x5_1000"
+    aruco6x6_50 = "aruco6x6_50"
+    aruco6x6_100 = "aruco6x6_100"
+    aruco6x6_250 = "aruco6x6_250"
+    aruco6x6_1000 = "aruco6x6_1000"
+    aruco7x7_50 = "aruco7x7_50"
+    aruco7x7_100 = "aruco7x7_100"
+    aruco7x7_250 = "aruco7x7_250"
+    aruco7x7_1000 = "aruco7x7_1000"
+
+    _valid_values = {
+        "aruco4x4_50": _zivid.calibration.MarkerDictionary.aruco4x4_50,
+        "aruco4x4_100": _zivid.calibration.MarkerDictionary.aruco4x4_100,
+        "aruco4x4_250": _zivid.calibration.MarkerDictionary.aruco4x4_250,
+        "aruco4x4_1000": _zivid.calibration.MarkerDictionary.aruco4x4_1000,
+        "aruco5x5_50": _zivid.calibration.MarkerDictionary.aruco5x5_50,
+        "aruco5x5_100": _zivid.calibration.MarkerDictionary.aruco5x5_100,
+        "aruco5x5_250": _zivid.calibration.MarkerDictionary.aruco5x5_250,
+        "aruco5x5_1000": _zivid.calibration.MarkerDictionary.aruco5x5_1000,
+        "aruco6x6_50": _zivid.calibration.MarkerDictionary.aruco6x6_50,
+        "aruco6x6_100": _zivid.calibration.MarkerDictionary.aruco6x6_100,
+        "aruco6x6_250": _zivid.calibration.MarkerDictionary.aruco6x6_250,
+        "aruco6x6_1000": _zivid.calibration.MarkerDictionary.aruco6x6_1000,
+        "aruco7x7_50": _zivid.calibration.MarkerDictionary.aruco7x7_50,
+        "aruco7x7_100": _zivid.calibration.MarkerDictionary.aruco7x7_100,
+        "aruco7x7_250": _zivid.calibration.MarkerDictionary.aruco7x7_250,
+        "aruco7x7_1000": _zivid.calibration.MarkerDictionary.aruco7x7_1000,
+    }
+
+    @classmethod
+    def valid_values(cls):
+        """Get valid values for MarkerDictionary.
+
+        Returns:
+            A list of strings representing valid values for MarkerDictionary.
+        """
+        return list(cls._valid_values.keys())
+
+    @classmethod
+    def marker_count(cls, dictionary_name):
+        """Get the number of markers in a dictionary.
+
+        Args:
+            dictionary_name: Name of the dictionary, e.g. "aruco4x4_50". Must be one of the values returned by
+                valid_values().
+
+        Returns:
+            Number of markers in the dictionary.
+
+        Raises:
+            ValueError: If the dictionary name is not one of the valid values returned by
+                valid_values().
+        """
+        if dictionary_name not in cls._valid_values:
+            raise ValueError(
+                "Invalid dictionary name '{}'. Valid values are {}".format(
+                    dictionary_name, cls.valid_values()
+                )
+            )
+
+        return cls._valid_values[dictionary_name].marker_count()
+
+
+class DetectionResultFiducialMarkers:
+    """Class representing detected fiducial markers."""
+
+    def __init__(self, impl):
+        """Initialize DetectionResultFiducialMarkers wrapper.
+
+        This constructor is only used internally, and should not be called by the end-user.
+
+        Args:
+            impl:   Reference to internal/back-end instance.
+
+        Raises:
+            TypeError: If argument does not match the expected internal class.
+        """
+        if not isinstance(impl, _zivid.calibration.DetectionResultFiducialMarkers):
+            raise TypeError(
+                "Unsupported type for argument impl. Got {}, expected {}".format(
+                    type(impl), _zivid.calibration.DetectionResultFiducialMarkers
+                )
+            )
+
+        self.__impl = impl
+
+    def valid(self):
+        """Check validity of DetectionResult.
+
+        Returns:
+            True if DetectionResult is valid
+        """
+        return self.__impl.valid()
+
+    def allowed_marker_ids(self):
+        """Get the allowed marker ids this detection result was made with.
+
+        Returns:
+            A list of integers, equal to what was passed to the detection function.
+        """
+        return self.__impl.allowed_marker_ids()
+
+    def detected_markers(self):
+        """Get all detected markers.
+
+        Returns:
+            A list of MarkerShape instances
+        """
+        return [MarkerShape(impl) for impl in self.__impl.detected_markers()]
+
+    def __bool__(self):
+        return bool(self.__impl)
+
+    def __str__(self):
+        return str(self.__impl)
+
+
 def detect_feature_points(point_cloud):
     """Detect feature points from a calibration object in a point cloud.
 
@@ -154,3 +353,46 @@ def capture_calibration_board(camera):
             camera._Camera__impl  # pylint: disable=protected-access
         )
     )
+
+
+def detect_markers(frame, allowed_marker_ids, marker_dictionary):
+    """Detect fiducial markers such as ArUco markers in a frame.
+
+    Only markers with integer IDs are supported. To get more information about fiducial markers, refer to the
+    wikipedia page:  https://en.wikipedia.org/wiki/Fiducial_marker
+
+    For more information on ArUco markers specifically, see the OpenCV documentation on ArUco markers:
+    https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html,
+
+    Frame need not contain all markers listed in allowedMarkerIds for a successful detection.
+
+    Args:
+        frame: A frame containing an image of one or several fiducial markers
+        allowed_marker_ids: List of the IDs of markers to be detected
+        marker_dictionary: The name of the marker dictionary to use. The name must be one of the values returned by
+            MarkerDictionary.valid_values()
+
+    Raises:
+        ValueError: If marker_dictionary is not one of the valid values returned by MarkerDictionary.valid_values()
+
+    Returns:
+        A DetectionResultFiducialMarkers instance
+    """
+
+    if marker_dictionary not in MarkerDictionary.valid_values():
+        raise ValueError(
+            "Invalid marker dictionary '{}'. Valid values are {}".format(
+                marker_dictionary, MarkerDictionary.valid_values()
+            )
+        )
+    dictionary = MarkerDictionary._valid_values.get(  # pylint: disable=protected-access
+        marker_dictionary
+    )
+
+    return DetectionResultFiducialMarkers(
+        _zivid.calibration.detect_markers(
+            frame._Frame__impl,  # pylint: disable=protected-access
+            allowed_marker_ids,
+            dictionary,
+        )
+    )

modules/zivid/_calibration/hand_eye.py

@@ -6,7 +6,7 @@
 
 import _zivid
 from zivid._calibration.pose import Pose
-from zivid._calibration.detector import DetectionResult
+from zivid._calibration.detector import DetectionResult, DetectionResultFiducialMarkers
 
 
 class HandEyeInput:
@@ -17,7 +17,7 @@ def __init__(self, robot_pose, detection_result):
 
         Args:
             robot_pose: The robot Pose at the time of capture
-            detection_result: The DetectionResult captured when in the above pose
+            detection_result: The DetectionResult or DetectionResultFiducialMarkers captured when in the above pose
 
         Raises:
             TypeError: If one of the input arguments are of the wrong type
@@ -28,16 +28,23 @@ def __init__(self, robot_pose, detection_result):
                     type(robot_pose)
                 )
             )
-        if not isinstance(detection_result, DetectionResult):
+        if isinstance(detection_result, DetectionResult):
+            self.__impl = _zivid.calibration.HandEyeInput(
+                robot_pose._Pose__impl,  # pylint: disable=protected-access
+                detection_result._DetectionResult__impl,  # pylint: disable=protected-access
+            )
+        elif isinstance(detection_result, DetectionResultFiducialMarkers):
+            self.__impl = _zivid.calibration.HandEyeInput(
+                robot_pose._Pose__impl,  # pylint: disable=protected-access
+                detection_result._DetectionResultFiducialMarkers__impl,  # pylint: disable=protected-access
+            )
+        else:
             raise TypeError(
-                "Unsupported type for argument detection_result. Expected zivid.calibration.DetectionResult but got {}".format(
-                    type(detection_result)
-                )
+                (
+                    "Unsupported type for argument detection_result."
+                    "Expected zivid.calibration.DetectionResult or zivid.calibration.DetectionResultFiducialMarkers but got {}"
+                ).format(type(detection_result))
             )
-        self.__impl = _zivid.calibration.HandEyeInput(
-            robot_pose._Pose__impl,  # pylint: disable=protected-access
-            detection_result._DetectionResult__impl,  # pylint: disable=protected-access
-        )
 
     def robot_pose(self):
         """Get the contained robot pose.

modules/zivid/calibration.py

@@ -3,9 +3,13 @@
 # pylint: disable=unused-import
 from zivid._calibration.detector import (
     DetectionResult,
+    DetectionResultFiducialMarkers,
+    MarkerShape,
+    MarkerDictionary,
     detect_feature_points,
     detect_calibration_board,
     capture_calibration_board,
+    detect_markers,
 )
 from zivid._calibration.hand_eye import (
     HandEyeInput,

src/Calibration/Calibration.cpp

@@ -30,6 +30,9 @@ namespace ZividPython::Calibration
         ZIVID_PYTHON_WRAP_CLASS(dest, HandEyeOutput);
         ZIVID_PYTHON_WRAP_CLASS(dest, HandEyeInput);
         ZIVID_PYTHON_WRAP_CLASS(dest, DetectionResult);
+        ZIVID_PYTHON_WRAP_CLASS(dest, MarkerShape);
+        ZIVID_PYTHON_WRAP_CLASS(dest, MarkerDictionary);
+        ZIVID_PYTHON_WRAP_CLASS(dest, DetectionResultFiducialMarkers);
         ZIVID_PYTHON_WRAP_CLASS(dest, HandEyeResidual);
 
         ZIVID_PYTHON_WRAP_CLASS(dest, MultiCameraResidual);
@@ -51,6 +54,12 @@ namespace ZividPython::Calibration
                  [](ReleasableCamera &releasableCamera) {
                      return ReleasableFrame{ Zivid::Calibration::captureCalibrationBoard(releasableCamera.impl()) };
                  })
+            .def("detect_markers",
+                 [](const ReleasableFrame &releasableFrame,
+                    const std::vector<int> &allowedMarkerIds,
+                    const MarkerDictionary &markerDictionary) {
+                     return detectMarkers(releasableFrame.impl(), allowedMarkerIds, markerDictionary);
+                 })
             .def("calibrate_eye_in_hand", &Zivid::Calibration::calibrateEyeInHand)
             .def("calibrate_eye_to_hand", &Zivid::Calibration::calibrateEyeToHand)
             .def("calibrate_multi_camera", &Zivid::Calibration::calibrateMultiCamera)