diff options
author | Théo de la Hogue | 2023-10-12 21:26:27 +0200 |
---|---|---|
committer | Théo de la Hogue | 2023-10-12 21:26:27 +0200 |
commit | a1ee2b893cce70ba03fbba1c12d9d0829e3e9632 (patch) | |
tree | f465c0d876fc5dd90cae03ff5d3fec7b98343554 /src | |
parent | 5f902cd2f41aa84267e2e27e53229268d8e4d579 (diff) | |
download | argaze-a1ee2b893cce70ba03fbba1c12d9d0829e3e9632.zip argaze-a1ee2b893cce70ba03fbba1c12d9d0829e3e9632.tar.gz argaze-a1ee2b893cce70ba03fbba1c12d9d0829e3e9632.tar.bz2 argaze-a1ee2b893cce70ba03fbba1c12d9d0829e3e9632.tar.xz |
Adding estimate_markers_pose method to ArUcoDetector.
Diffstat (limited to 'src')
-rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoDetector.py | 36 |
1 files changed, 36 insertions, 0 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoDetector.py b/src/argaze/ArUcoMarkers/ArUcoDetector.py index f178a20..01527a1 100644 --- a/src/argaze/ArUcoMarkers/ArUcoDetector.py +++ b/src/argaze/ArUcoMarkers/ArUcoDetector.py @@ -302,6 +302,42 @@ class ArUcoDetector(): return detection_time + def estimate_markers_pose(self, markers_ids: list = []): + """Estimate pose of current detected markers or of given markers id list.""" + + # Is there detected markers ? + if len(self.__detected_markers) > 0: + + # Select all markers by default + if len(markers_ids) == 0: + + markers_ids = self.__detected_markers.keys() + + # Prepare data for aruco.estimatePoseSingleMarkers function + selected_markers_corners = tuple() + selected_markers_ids = [] + + for marker_id, marker in self.__detected_markers.items(): + + if marker_id in markers_ids: + + selected_markers_corners += (marker.corners,) + selected_markers_ids.append(marker_id) + + # Estimate pose of selected markers + if len(selected_markers_corners) > 0: + + markers_rvecs, markers_tvecs, markers_points = aruco.estimatePoseSingleMarkers(selected_markers_corners, self.marker_size, numpy.array(self.optic_parameters.K), numpy.array(self.optic_parameters.D)) + + for i, marker_id in enumerate(selected_markers_ids): + + marker = self.__detected_markers[marker_id] + + marker.translation = markers_tvecs[i][0] + marker.rotation, _ = cv.Rodrigues(markers_rvecs[i][0]) + + marker.points = markers_points.reshape(4, 3).dot(marker.rotation) + marker.translation + @property def detected_markers(self) -> dict[ArUcoMarkerType]: """Access to detected markers dictionary.""" |