aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoSet.py99
1 files changed, 71 insertions, 28 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoSet.py b/src/argaze/ArUcoMarkers/ArUcoSet.py
index 2eeea32..df0ad65 100644
--- a/src/argaze/ArUcoMarkers/ArUcoSet.py
+++ b/src/argaze/ArUcoMarkers/ArUcoSet.py
@@ -108,7 +108,6 @@ class ArUcoSet():
if numpy.array_equal(A, B):
- print('A.all() == B.all()')
angle = 0.
else:
@@ -151,30 +150,40 @@ class ArUcoSet():
except:
self.__distance_cache[B_name] = {A_name: distance}
- def print_cache(self):
- """Print pre-processed data."""
+ def __str__(self) -> str:
+ """Output pre-processed data as string representation."""
- print('\nIdentifier cache:')
+ output = f'\n\n\tDictionary: {self.dictionary.name}'
+
+ output += '\n\n\tIdentifier cache:'
for i, name in self.__identifier_cache.items():
- print(f'- {i}: {name}')
+ output += f'\n\t\t- {i}: {name}'
- print('\nTranslation cache:')
+ output += '\n\n\tTranslation cache:'
for name, item in self.__translation_cache.items():
- print(f'- {name}: {item}')
+ output += f'\n\t\t- {name}: {item}'
- print('\nRotation cache:')
+ output += '\n\n\tRotation cache:'
for name, item in self.__rotation_cache.items():
- print(f'- {name}:\n{item}')
+ output += f'\n\t\t- {name}:\n{item}'
- print('\nAngle cache:')
+ output += '\n\n\tAngle cache:'
for A_name, A_angle_cache in self.__angle_cache.items():
for B_name, angle in A_angle_cache.items():
- print(f'- {A_name}/{B_name}: {angle:3f}')
+ output += f'\n\t\t- {A_name}/{B_name}: {angle:3f}'
- print('\nDistance cache:')
+ output += '\n\n\tDistance cache:'
for A_name, A_distance_cache in self.__distance_cache.items():
for B_name, distance in A_distance_cache.items():
- print(f'- {A_name}/{B_name}: {distance:3f}')
+ output += f'\n\t\t- {A_name}/{B_name}: {distance:3f}'
+
+ return output
+
+ @property
+ def identifiers(self) -> list:
+ """List all makers identifier."""
+
+ return list(self.__identifier_cache.keys())
def __make_rotation_matrix(self, x, y, z):
@@ -221,7 +230,7 @@ class ArUcoSet():
return rvec, tvec
- def estimate_pose(self, tracked_markers) -> Tuple[numpy.array, numpy.array, bool, int]:
+ def estimate_pose(self, tracked_markers) -> Tuple[numpy.array, numpy.array, bool, int, dict]:
"""Estimate set pose from tracked markers (cf ArUcoTracker.track())
* **Returns:**
@@ -229,6 +238,7 @@ class ArUcoSet():
- rotation vector
- pose estimation success status
- the number of places used to estimate the pose as validity score
+ - dict of non valid distance and angle
"""
# Init pose data
@@ -236,11 +246,12 @@ class ArUcoSet():
self._rotation = numpy.zeros(3)
self._succeded = False
self._validity = 0
+ self._unvalid = {}
# Don't try to estimate pose if there is no tracked markers
if len(tracked_markers) == 0:
- return self._translation, self._rotation, self._succeded, self._validity
+ return self._translation, self._rotation, self._succeded, self._validity, self._unvalid
# Look for places related to tracked markers
tracked_places = {}
@@ -267,8 +278,8 @@ class ArUcoSet():
self._validity = 1
#print('!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')
- #print(f'arset rotation vector: {self._rotation[0][0]:3f} {self._rotation[1][0]:3f} {self._rotation[2][0]:3f}')
- #print(f'arset translation vector: {self._translation[0]:3f} {self._translation[1]:3f} {self._translation[2]:3f}')
+ #print(f'ArUcoSet rotation vector: {self._rotation[0][0]:3f} {self._rotation[1][0]:3f} {self._rotation[2][0]:3f}')
+ #print(f'ArUcoSet translation vector: {self._translation[0]:3f} {self._translation[1]:3f} {self._translation[2]:3f}')
# Pose validity checking processes places two by two
else:
@@ -295,16 +306,10 @@ class ArUcoSet():
angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
expected_angle = self.__angle_cache[A_name][B_name]
- #print('angle:', angle)
- #print('expected angle:', expected_angle)
-
# Calculate distance between A place center and B place center
distance = numpy.linalg.norm(A_place.translation - B_place.translation)
expected_distance = self.__distance_cache[A_name][B_name]
- #print('distance: ', distance)
- #print('expected distance: ', expected_distance)
-
# Check angle and distance according given tolerance then normalise place pose
valid_angle = math.isclose(angle, expected_angle, abs_tol=self.angle_tolerance)
valid_distance = math.isclose(distance, expected_distance, abs_tol=self.distance_tolerance)
@@ -333,25 +338,63 @@ class ArUcoSet():
valid_rvecs.append(rvec)
valid_tvecs.append(tvec)
+ else:
+
+ if not valid_angle:
+ self._unvalid[f'{A_name}/{B_name} angle'] = angle
+
+ if not valid_distance:
+ self._unvalid[f'{A_name}/{B_name} distance'] = distance
+
if len(valid_places) > 1:
- # Consider arset rotation as the mean of all valid translations
+ # Consider ArUcoSet rotation as the mean of all valid translations
# !!! 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(valid_rvecs), axis=0)
- # Consider arset translation as the mean of all valid translations
+ # Consider ArUcoSet translation as the mean of all valid translations
self._translation = numpy.mean(numpy.array(valid_tvecs), axis=0)
#print(':::::::::::::::::::::::::::::::::::::::::::::::::::')
- #print(f'arset rotation vector: {self._rotation[0][0]:3f} {self._rotation[1][0]:3f} {self._rotation[2][0]:3f}')
- #print(f'arset translation vector: {self._translation[0]:3f} {self._translation[1]:3f} {self._translation[2]:3f}')
+ #print(f'ArUcoSet rotation vector: {self._rotation[0][0]:3f} {self._rotation[1][0]:3f} {self._rotation[2][0]:3f}')
+ #print(f'ArUcoSet translation vector: {self._translation[0]:3f} {self._translation[1]:3f} {self._translation[2]:3f}')
self._succeded = True
self._validity = len(valid_places)
+ else:
+
+ unvalid_rvecs = []
+ unvalid_tvecs = []
+
+ # Gather unvalid pose estimations
+ for name, place in tracked_places.items():
+
+ if name not in valid_places:
+
+ R, _ = cv.Rodrigues(place.rotation)
+ rvec, tvec = self.__normalise_place_pose(name, place, R)
+
+ unvalid_rvecs = [rvec]
+ unvalid_tvecs = [tvec]
+
+ # Consider ArUcoSet rotation as the mean of all unvalid translations
+ # !!! 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(unvalid_rvecs), axis=0)
+
+ # Consider ArUcoSet translation as the mean of all unvalid translations
+ self._translation = numpy.mean(numpy.array(unvalid_tvecs), axis=0)
+
+ #print(':::::::::::::::::::::::::::::::::::::::::::::::::::')
+ #print(f'ArUcoSet rotation vector: {self._rotation[0][0]:3f} {self._rotation[1][0]:3f} {self._rotation[2][0]:3f}')
+ #print(f'ArUcoSet translation vector: {self._translation[0]:3f} {self._translation[1]:3f} {self._translation[2]:3f}')
+
+ self._succeded = False
+ self._validity = len(tracked_places)
+
#print('----------------------------------------------------')
- return self._translation, self._rotation, self._succeded, self._validity
+ return self._translation, self._rotation, self._succeded, self._validity, self._unvalid
@property
def translation(self) -> numpy.array: