Smoothing marker corners if required to stabilize pose estimation.

1 files changed, 35 insertions, 9 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoDetector.py b/src/argaze/ArUcoMarkers/ArUcoDetector.py
index 3ef7fa6..9e40561 100644
--- a/src/argaze/ArUcoMarkers/ArUcoDetector.py
+++ b/src/argaze/ArUcoMarkers/ArUcoDetector.py
@@ -131,24 +131,27 @@ class DetectorParameters():
 
 @dataclass
 class ArUcoDetector():
-    """ArUco markers detector."""
+    """ArUco markers detector.
+
+    Parameters:
+        dictionary: ArUco markers dictionary to detect.
+        marker_size: Size of ArUco markers to detect in centimeter.
+        optic_parameters: Optic parameters to use for ArUco detection into image.
+        parameters: ArUco detector parameters.
+        smooth_marker_corners: Enable marker corners smoothing to stabilize pose estimation.
+    """
 
     dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = field(default_factory=ArUcoMarkersDictionary.ArUcoMarkersDictionary)
-    """ArUco markers dictionary to detect."""
-
     marker_size: float = field(default=0.)
-    """Size of ArUco markers to detect in centimeter."""
-
     optic_parameters: ArUcoOpticCalibrator.OpticParameters = field(default_factory=ArUcoOpticCalibrator.OpticParameters)
-    """Optic parameters to use for ArUco detection into image."""
-
     parameters: DetectorParameters = field(default_factory=DetectorParameters)
-    """ArUco detector parameters."""
+    smooth_marker_corners: bool = field(default=False)
 
     def __post_init__(self):
 
         # Init detected markers data
         self.__detected_markers = {}
+        self.__last_detected_markers = {}
 
         # Init detected board data
         self.__board = None
@@ -264,6 +267,9 @@ class ArUcoDetector():
             detection time: marker detection time in ms.
         """
 
+        # Copy last detected markers
+        self.__last_detected_markers = self.__detected_markers
+
         # Reset detected markers data
         self.__detected_markers, detected_markers_corners, detected_markers_ids = {}, [], []
 
@@ -289,7 +295,27 @@ class ArUcoDetector():
 
                 marker = ArUcoMarker.ArUcoMarker(self.dictionary, marker_id, self.marker_size)
 
-                marker.corners = detected_markers_corners[i]
+                # Smooth marker corners if required
+                if self.smooth_marker_corners:
+
+                    # Try to smooth corners with last detected markers corners
+                    try:
+
+                        # Smooth corners positions if the distance between new marker and last marker is lower than half marker size
+                        half_marker_size_px = numpy.linalg.norm(detected_markers_corners[i][0][1] - detected_markers_corners[i][0][0]) / 2
+                        distance_to_last = numpy.linalg.norm(detected_markers_corners[i] - self.__last_detected_markers[marker_id].corners)
+                        smooth_factor = 0. if distance_to_last > half_marker_size_px else (half_marker_size_px - distance_to_last) / half_marker_size_px
+
+                        marker.corners = numpy.rint(self.__last_detected_markers[marker_id].corners * smooth_factor + detected_markers_corners[i] * (1 - smooth_factor))
+
+                    # Avoid smoothing if the marker was not part of last detection
+                    except KeyError:
+
+                        marker.corners = detected_markers_corners[i]
+
+                else:
+
+                    marker.corners = detected_markers_corners[i]
 
                 # No pose estimation: call estimate_markers_pose to get one
                 marker.translation = numpy.empty([0])