aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoCamera.py5
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoDetector.py64
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoMarker.py2
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py327
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoScene.py31
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):
"""