diff options
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoDetector.py | 44 |
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]) |