Returning unvalid dictionary to get info about what fails during pose estimation.

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: