diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoCamera.py | 5 | ||||
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoDetector.py | 64 | ||||
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoMarker.py | 2 | ||||
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py | 327 | ||||
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoScene.py | 31 |
5 files changed, 66 insertions, 363 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoCamera.py b/src/argaze/ArUcoMarkers/ArUcoCamera.py index b850dde..ed6c619 100644 --- a/src/argaze/ArUcoMarkers/ArUcoCamera.py +++ b/src/argaze/ArUcoMarkers/ArUcoCamera.py @@ -189,11 +189,8 @@ class ArUcoCamera(ArFeatures.ArCamera): try: - # Estimate scene markers poses - self.aruco_detector.estimate_markers_pose(scene.aruco_markers_group.identifiers) - # Estimate scene pose from detected scene markers - tvec, rmat, _, _ = scene.estimate_pose(self.aruco_detector.detected_markers) + tvec, rmat, _ = scene.estimate_pose(self.aruco_detector.detected_markers) # Project scene into camera frame according estimated pose for layer_name, layer_projection in scene.project(tvec, rmat, self.visual_hfov, self.visual_vfov): diff --git a/src/argaze/ArUcoMarkers/ArUcoDetector.py b/src/argaze/ArUcoMarkers/ArUcoDetector.py index 9e40561..f178a20 100644 --- a/src/argaze/ArUcoMarkers/ArUcoDetector.py +++ b/src/argaze/ArUcoMarkers/ArUcoDetector.py @@ -138,20 +138,17 @@ class ArUcoDetector(): 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) marker_size: float = field(default=0.) optic_parameters: ArUcoOpticCalibrator.OpticParameters = field(default_factory=ArUcoOpticCalibrator.OpticParameters) parameters: DetectorParameters = field(default_factory=DetectorParameters) - 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 @@ -267,9 +264,6 @@ 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 = {}, [], [] @@ -294,28 +288,7 @@ class ArUcoDetector(): for i, marker_id in enumerate(detected_markers_ids): marker = ArUcoMarker.ArUcoMarker(self.dictionary, marker_id, self.marker_size) - - # 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] + marker.corners = detected_markers_corners[i][0] # No pose estimation: call estimate_markers_pose to get one marker.translation = numpy.empty([0]) @@ -329,41 +302,6 @@ 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) - @property def detected_markers(self) -> dict[ArUcoMarkerType]: """Access to detected markers dictionary.""" diff --git a/src/argaze/ArUcoMarkers/ArUcoMarker.py b/src/argaze/ArUcoMarkers/ArUcoMarker.py index 57bd8bd..f088dae 100644 --- a/src/argaze/ArUcoMarkers/ArUcoMarker.py +++ b/src/argaze/ArUcoMarkers/ArUcoMarker.py @@ -68,7 +68,7 @@ class ArUcoMarker(): # Draw marker if required if color is not None: - aruco.drawDetectedMarkers(image, [self.corners], numpy.array([self.identifier]), color) + aruco.drawDetectedMarkers(image, [numpy.array([list(self.corners)])], numpy.array([self.identifier]), color) # Draw marker axes if pose has been estimated and if required if self.translation.size == 3 and self.rotation.size == 9 and draw_axes is not None: diff --git a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py index 4a43965..df390b4 100644 --- a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py +++ b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py @@ -17,8 +17,7 @@ import re from argaze.ArUcoMarkers import ArUcoMarkersDictionary, ArUcoMarker, ArUcoOpticCalibrator import numpy -import cv2 as cv -import cv2.aruco as aruco +import cv2 T0 = numpy.array([0., 0., 0.]) """Define no translation vector.""" @@ -58,37 +57,16 @@ def is_rotation_matrix(R): return n < 1e-3 -def make_euler_rotation_vector(R): - - assert(is_rotation_matrix(R)) - - sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0]) - - singular = sy < 1e-6 - - if not singular : - x = math.atan2(R[2,1] , R[2,2]) - y = math.atan2(-R[2,0], sy) - z = math.atan2(R[1,0], R[0,0]) - else : - x = math.atan2(-R[1,2], R[1,1]) - y = math.atan2(-R[2,0], sy) - z = 0 - - return numpy.array([numpy.rad2deg(x), numpy.rad2deg(y), numpy.rad2deg(z)]) - @dataclass(frozen=True) class Place(): - """Define a place as a pose and a marker. + """Define a place as list of corners position and a marker. Parameters: - translation: position in group referential. - rotation: rotation in group referential. + corners: 3D corners position in group referential. marker: ArUco marker linked to the place. """ - translation: numpy.array - rotation: numpy.array + corners: numpy.array marker: dict @dataclass @@ -146,12 +124,16 @@ class ArUcoMarkersGroup(): new_marker = ArUcoMarker.ArUcoMarker(self.dictionary, identifier, self.marker_size) - new_places[identifier] = Place(tvec, rmat, new_marker) + # Build marker corners thanks to translation vector and rotation matrix + place_corners = numpy.array([[-self.marker_size/2, self.marker_size/2, 0], [self.marker_size/2, self.marker_size/2, 0], [self.marker_size/2, -self.marker_size/2, 0], [-self.marker_size/2, -self.marker_size/2, 0]]) + place_corners = place_corners.dot(rmat) + tvec + + new_places[identifier] = Place(place_corners, new_marker) - # else places are configured using detected markers + # else places are configured using detected markers estimated points elif isinstance(data, ArUcoMarker.ArUcoMarker): - new_places[identifier] = Place(data.translation, data.rotation, data) + new_places[identifier] = Place(data.points, data) # else places are already at expected format elif (type(identifier) == int) and isinstance(data, Place): @@ -160,9 +142,6 @@ class ArUcoMarkersGroup(): self.places = new_places - # Init place consistency - self.init_places_consistency() - @classmethod def from_obj(self, obj_filepath: str) -> ArUcoMarkersGroupType: """Load ArUco markers group from .obj file. @@ -264,28 +243,16 @@ class ArUcoMarkersGroup(): # Retreive marker vertices thanks to face vertice ids for identifier, face in faces.items(): - # Gather place corners from counter clockwise ordered face vertices - corners = numpy.array([ vertices[i-1] for i in face ]) - - # Edit translation (Tp) allowing to move world axis (W) at place axis (P) - Tp = corners.mean(axis=0) + # Gather place corners in clockwise order + cw_corners = numpy.array([ vertices[i-1] for i in reversed(face) ]) # Edit place axis from corners positions - place_x_axis = corners[1:3].mean(axis=0) - Tp + place_x_axis = cw_corners[1:3].mean(axis=0) place_x_axis_norm = numpy.linalg.norm(place_x_axis) - place_x_axis = place_x_axis / place_x_axis_norm - - place_y_axis = corners[2:4].mean(axis=0) - Tp + + place_y_axis = cw_corners[2:4].mean(axis=0) place_y_axis_norm = numpy.linalg.norm(place_y_axis) - place_y_axis = place_y_axis / place_y_axis_norm - place_z_axis = normals[identifier] - - # Edit rotation (Rp) allowing to transform world axis (W) into place axis (P) - W = numpy.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - P = numpy.array([place_x_axis, place_y_axis, place_z_axis]) - Rp = W.dot(P.T) - # Check axis size: they should be almost equal if math.isclose(place_x_axis_norm, place_y_axis_norm, rel_tol=1e-3): @@ -302,7 +269,7 @@ class ArUcoMarkersGroup(): # Create a new place related to a new marker new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size) - new_places[identifier] = Place(Tp, Rp, new_marker) + new_places[identifier] = Place(cw_corners, new_marker) except IOError: raise IOError(f'File not found: {obj_filepath}') @@ -337,18 +304,7 @@ class ArUcoMarkersGroup(): output += '\n\n\tPlaces:' for identifier, place in self.places.items(): output += f'\n\t\t- {identifier}:' - output += f'\n{place.translation}' - output += f'\n{place.rotation}' - - output += '\n\n\tAngle cache:' - for A_identifier, A_angle_cache in self.__rotation_cache.items(): - for B_identifier, angle in A_angle_cache.items(): - output += f'\n\t\t- {A_identifier}/{B_identifier}: [{angle[0]:3f} {angle[1]:3f} {angle[2]:3f}]' - - output += '\n\n\tDistance cache:' - for A_identifier, A_distance_cache in self.__translation_cache.items(): - for B_identifier, distance in A_distance_cache.items(): - output += f'\n\t\t- {A_identifier}/{B_identifier}: {distance:3f}' + output += f'\n{place.corners}' return output @@ -381,148 +337,22 @@ class ArUcoMarkersGroup(): return group_markers, remaining_markers - def init_places_consistency(self): - """Initialize places consistency to speed up further markers consistency checking.""" - - # Process expected rotation between places combinations to speed up further calculations - self.__rotation_cache = {} - for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2): - - A = self.places[A_identifier].rotation - B = self.places[B_identifier].rotation - - if numpy.array_equal(A, B): - - AB_rvec = [0., 0., 0.] - BA_rvec = [0., 0., 0.] - - else: - - # Calculate euler angle representation of AB and BA rotation matrix - AB_rvec = make_euler_rotation_vector(B.dot(A.T)) - BA_rvec = make_euler_rotation_vector(A.dot(B.T)) - - try: - self.__rotation_cache[A_identifier][B_identifier] = AB_rvec - except: - self.__rotation_cache[A_identifier] = {B_identifier: AB_rvec} - - try: - self.__rotation_cache[B_identifier][A_identifier] = BA_rvec - except: - self.__rotation_cache[B_identifier] = {A_identifier: BA_rvec} - - # Process translation between each places combinations to speed up further calculations - self.__translation_cache = {} - for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2): - - A = self.places[A_identifier].translation - B = self.places[B_identifier].translation - - # Calculate translation between A and B position - AB_tvec = numpy.linalg.norm(B - A) - - try: - self.__translation_cache[A_identifier][B_identifier] = AB_tvec - except: - self.__translation_cache[A_identifier] = {B_identifier: AB_tvec} - - try: - self.__translation_cache[B_identifier][A_identifier] = AB_tvec - except: - self.__translation_cache[B_identifier] = {A_identifier: AB_tvec} + def estimate_pose_from_markers_corners(self, markers: dict, K: numpy.array, D: numpy.array) -> Tuple[bool, numpy.array, numpy.array]: + """Estimate pose from markers corners and places corners. - def check_markers_consistency(self, group_markers: dict, angle_tolerance: float, distance_tolerance: float) -> Tuple[dict, dict, dict]: - """Evaluate if given markers configuration match related places configuration. + Parameters: + markers: detected markers to use for pose estimation. + K: intrinsic camera parameters + D: camera distorsion matrix Returns: - dict of consistent markers - dict of unconsistent markers - dict of identified distance or angle unconsistencies and out-of-bounds values + success: True if the pose estimation succeeded + tvec: scene translation vector + rvec: scene rotation vector """ - consistent_markers = {} - unconsistencies = {'rotation': {}, 'translation': {}} - - for (A_identifier, A_marker), (B_identifier, B_marker) in itertools.combinations(group_markers.items(), 2): - - try: - - # Rotation matrix from A marker to B marker - AB = B_marker.rotation.dot(A_marker.rotation.T) - - # Calculate euler angle representation of AB rotation matrix - AB_rvec = make_euler_rotation_vector(AB) - expected_rvec= self.__rotation_cache[A_identifier][B_identifier] - - # Calculate distance between A marker center and B marker center - AB_tvec = numpy.linalg.norm(A_marker.translation - B_marker.translation) - expected_tvec = self.__translation_cache[A_identifier][B_identifier] - - # Check angle and distance according given tolerance then normalise marker pose - consistent_rotation = numpy.allclose(AB_rvec, expected_rvec, atol=angle_tolerance) - consistent_translation = math.isclose(AB_tvec, expected_tvec, abs_tol=distance_tolerance) - - if consistent_rotation and consistent_translation: - - if A_identifier not in consistent_markers.keys(): - - # Remember this marker is already validated - consistent_markers[A_identifier] = A_marker - - if B_identifier not in consistent_markers.keys(): - - # Remember this marker is already validated - consistent_markers[B_identifier] = B_marker - - else: - - if not consistent_rotation: - unconsistencies['rotation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_rvec, 'expected': expected_rvec} - - if not consistent_translation: - unconsistencies['translation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_tvec, 'expected': expected_tvec} - - except KeyError: - - raise ValueError(f'Marker {A_identifier} or {B_identifier} don\'t belong to the group.') - - # Gather unconsistent markers - unconsistent_markers = {} - - for identifier, marker in group_markers.items(): - - if identifier not in consistent_markers.keys(): - - unconsistent_markers[identifier] = marker - - return consistent_markers, unconsistent_markers, unconsistencies - - def estimate_pose_from_single_marker(self, marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]: - """Calculate rotation and translation that move a marker to its place.""" - - # Get the place related to the given marker - try: - - place = self.places[marker.identifier] - - # Rotation matrix that transform marker to related place - self._rotation = marker.rotation.dot(place.rotation.T) - - # Translation vector that transform marker to related place - self._translation = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T) - - return self._translation, self._rotation - - except KeyError: - - raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the group.') - - def estimate_pose_from_markers(self, markers: dict) -> Tuple[numpy.array, numpy.array]: - """Calculate average rotation and translation that move markers to their related places.""" - - rotations = [] - translations = [] + markers_corners_2d = [] + places_corners_3d = [] for identifier, marker in markers.items(): @@ -530,72 +360,23 @@ class ArUcoMarkersGroup(): place = self.places[identifier] - # Rotation matrix that transform marker to related place - R = marker.rotation.dot(place.rotation.T) - - # Translation vector that transform marker to related place - T = marker.translation - place.translation.dot(place.rotation).dot(marker.rotation.T) + for marker_corner in marker.corners: + markers_corners_2d.append(list(marker_corner)) - rotations.append(R) - translations.append(T) + for place_corner in place.corners: + places_corners_3d.append(list(place_corner)) except KeyError: raise ValueError(f'Marker {marker.identifier} doesn\'t belong to the group.') - # Consider ArUcoMarkersGroup rotation as the mean of all marker rotations - # !!! WARNING !!! This is a bad hack : processing rotations average is a very complex problem that needs to well define the distance calculation method before. - self._rotation = numpy.mean(numpy.array(rotations), axis=0) - - # Consider ArUcoMarkersGroup translation as the mean of all marker translations - self._translation = numpy.mean(numpy.array(translations), axis=0) - - return self._translation, self._rotation - - def estimate_pose_from_axis_markers(self, origin_marker: ArUcoMarker.ArUcoMarker, horizontal_axis_marker: ArUcoMarker.ArUcoMarker, vertical_axis_marker: ArUcoMarker.ArUcoMarker) -> Tuple[numpy.array, numpy.array]: - """Calculate rotation and translation from 3 markers defining an orthogonal axis.""" - - O_marker = origin_marker - A_marker = horizontal_axis_marker - B_marker = vertical_axis_marker - - O_place = self.places[O_marker.identifier] - A_place = self.places[A_marker.identifier] - B_place = self.places[B_marker.identifier] - - # Place axis - OA = A_place.translation - O_place.translation - OA = OA / numpy.linalg.norm(OA) - - OB = B_place.translation - O_place.translation - OB = OB / numpy.linalg.norm(OB) - - # Detect and correct bad place axis orientation - X_sign = numpy.sign(OA)[0] - Y_sign = numpy.sign(OB)[1] - - P = numpy.array([OA*X_sign, OB*Y_sign, numpy.cross(OA*X_sign, OB*Y_sign)]) - - # Marker axis - OA = A_marker.translation - O_marker.translation - OA = OA / numpy.linalg.norm(OA) - - OB = B_marker.translation - O_marker.translation - OB = OB / numpy.linalg.norm(OB) - - # Detect and correct bad place axis orientation - X_sign = numpy.sign(OA)[0] - Y_sign = -numpy.sign(OB)[1] - - M = numpy.array([OA*X_sign, OB*Y_sign, numpy.cross(OA*X_sign, OB*Y_sign)]) - - # Then estimate ArUcoMarkersGroup rotation - self._rotation = P.dot(M.T) + # Solve + success, rvec, tvec = cv2.solvePnP(numpy.array(places_corners_3d), numpy.array(markers_corners_2d), numpy.array(K), numpy.array(D), flags=0) - # Consider ArUcoMarkersGroup translation as the translation of the marker at axis origin - self._translation = O_marker.translation - O_place.translation.dot(O_place.rotation).dot(M.T) + self._translation = tvec.T + self._rotation = rvec.T - return self._translation, self._rotation + return success, self._translation, self._rotation @property def translation(self) -> numpy.array: @@ -624,15 +405,15 @@ class ArUcoMarkersGroup(): try: axisPoints = numpy.float32([[length, 0, 0], [0, length, 0], [0, 0, length], [0, 0, 0]]).reshape(-1, 3) - axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) + axisPoints, _ = cv2.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) axisPoints = axisPoints.astype(int) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0, 255, 0), thickness) # Y (green) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255, 0, 0), thickness) # Z (blue) + cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red) + cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0, 255, 0), thickness) # Y (green) + cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255, 0, 0), thickness) # Z (blue) # Ignore errors due to out of field axis: their coordinate are larger than int32 limitations. - except cv.error: + except cv2.error: pass def draw_places(self, image: numpy.array, K, D, color: tuple = None, border_size: int = 0): @@ -648,16 +429,16 @@ class ArUcoMarkersGroup(): R = self.places[identifier].rotation placePoints = (T + numpy.float32([R.dot([-l, -l, 0]), R.dot([l, -l, 0]), R.dot([l, l, 0]), R.dot([-l, l, 0])])).reshape(-1, 3) - placePoints, _ = cv.projectPoints(placePoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) + placePoints, _ = cv2.projectPoints(placePoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) placePoints = placePoints.astype(int) - cv.line(image, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), color, border_size) - cv.line(image, tuple(placePoints[1].ravel()), tuple(placePoints[2].ravel()), color, border_size) - cv.line(image, tuple(placePoints[2].ravel()), tuple(placePoints[3].ravel()), color, border_size) - cv.line(image, tuple(placePoints[3].ravel()), tuple(placePoints[0].ravel()), color, border_size) + cv2.line(image, tuple(placePoints[0].ravel()), tuple(placePoints[1].ravel()), color, border_size) + cv2.line(image, tuple(placePoints[1].ravel()), tuple(placePoints[2].ravel()), color, border_size) + cv2.line(image, tuple(placePoints[2].ravel()), tuple(placePoints[3].ravel()), color, border_size) + cv2.line(image, tuple(placePoints[3].ravel()), tuple(placePoints[0].ravel()), color, border_size) # Ignore errors due to out of field places: their coordinate are larger than int32 limitations. - except cv.error: + except cv2.error: pass def draw_places_axes(self, image: numpy.array, K, D, thickness: int = 0, length: float = 0): @@ -671,15 +452,15 @@ class ArUcoMarkersGroup(): R = self.places[identifier].rotation axisPoints = (T + numpy.float32([R.dot([length, 0, 0]), R.dot([0, length, 0]), R.dot([0, 0, length]), R.dot([0, 0, 0])])).reshape(-1, 3) - axisPoints, _ = cv.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) + axisPoints, _ = cv2.projectPoints(axisPoints, self._rotation, self._translation, numpy.array(K), numpy.array(D)) axisPoints = axisPoints.astype(int) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0, 255, 0), thickness) # Y (green) - cv.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255, 0, 0), thickness) # Z (blue) + cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0, 0, 255), thickness) # X (red) + cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[1].ravel()), (0, 255, 0), thickness) # Y (green) + cv2.line(image, tuple(axisPoints[3].ravel()), tuple(axisPoints[2].ravel()), (255, 0, 0), thickness) # Z (blue) # Ignore errors due to out of field places: their coordinate are larger than int32 limitations. - except cv.error: + except cv2.error: pass def draw(self, image: numpy.array, K, D, draw_axes: dict = None, draw_places: dict = None, draw_places_axes: dict = None): diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py index f6b303a..b8b9cfd 100644 --- a/src/argaze/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze/ArUcoMarkers/ArUcoScene.py @@ -96,14 +96,13 @@ class ArUcoScene(ArFeatures.ArScene): # Create new aruco scene using temporary ar scene values return ArUcoScene(aruco_markers_group=new_aruco_markers_group, **temp_ar_scene_values) - def estimate_pose(self, detected_markers) -> Tuple[numpy.array, numpy.array, str, dict]: + def estimate_pose(self, detected_markers) -> Tuple[numpy.array, numpy.array, dict]: """Estimate scene pose from detected ArUco markers. Returns: - scene translation vector - scene rotation matrix - pose estimation strategy - dict of markers used to estimate the pose + scene translation vector + scene rotation matrix + dict of markers used to estimate the pose """ # Pose estimation fails when no marker is detected @@ -118,26 +117,14 @@ class ArUcoScene(ArFeatures.ArScene): raise ArFeatures.PoseEstimationFailed('No marker belongs to the scene') - # Estimate scene pose from unique marker transformations - elif len(scene_markers) == 1: + # Estimate pose from a markers corners + success, tvec, rmat = self.aruco_markers_group.estimate_pose_from_markers_corners(scene_markers, self.parent.aruco_detector.optic_parameters.K, self.parent.aruco_detector.optic_parameters.D) - marker_id, marker = scene_markers.popitem() - tvec, rmat = self.aruco_markers_group.estimate_pose_from_single_marker(marker) - - return tvec, rmat, 'estimate_pose_from_single_marker', {marker_id: marker} + if not success: - # Otherwise, check markers consistency - consistent_markers, unconsistent_markers, unconsistencies = self.aruco_markers_group.check_markers_consistency(scene_markers, self.angle_tolerance, self.distance_tolerance) + raise ArFeatures.PoseEstimationFailed('Can\'t estimate pose from markers corners positions') - # Pose estimation fails when no marker passes consistency checking - if len(consistent_markers) == 0: - - raise ArFeatures.PoseEstimationFailed('Unconsistent marker poses', unconsistencies) - - # Otherwise, estimate scene pose from all consistent markers pose - tvec, rmat = self.aruco_markers_group.estimate_pose_from_markers(consistent_markers) - - return tvec, rmat, 'estimate_pose_from_markers', consistent_markers + return tvec, rmat, scene_markers def draw(self, image: numpy.array, draw_aruco_markers_group: dict = None): """ |