aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoDetector.py44
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])