diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoSet.py | 99 |
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: |