checking distance to validate cube pose.

1 files changed, 30 insertions, 9 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoCube.py b/src/argaze/ArUcoMarkers/ArUcoCube.py
index 7012e0a..552d4d1 100644
--- a/src/argaze/ArUcoMarkers/ArUcoCube.py
+++ b/src/argaze/ArUcoMarkers/ArUcoCube.py
@@ -40,15 +40,18 @@ class ArUcoCube():
     faces: dict = field(init=False, default_factory=dict)
     """All named faces of the cube and their ArUco markers."""
 
-    translation: numpy.ndarray = field(init=False)
+    translation: numpy.array = field(init=False)
     """Position of the cube."""
 
-    rotation: numpy.ndarray = field(init=False)
+    rotation: numpy.array = field(init=False)
     """Rotation of the cube."""
 
     angle_tolerance: float = field(init=False)
     """Angle error tolerance allowed to validate face pose in degree."""
 
+    distance_tolerance: float = field(init=False)
+    """Distance error tolerance allowed to validate face pose in centimeter."""
+
     def __init__(self, configuration_filepath):
         """Define cube from a .json  file."""
 
@@ -76,6 +79,9 @@ class ArUcoCube():
             # Load angle tolerance
             self.angle_tolerance = configuration['angle_tolerance']
 
+            # Load distance tolerance
+            self.distance_tolerance = configuration['distance_tolerance']
+
             # Init pose data
             self.translation = numpy.zeros(3)
             self.rotation = numpy.zeros(3)
@@ -142,6 +148,9 @@ class ArUcoCube():
                 except:
                     self.__angle_cache[B_name] = {A_name: angle}
 
+            # Process distance between face combination to speed up further calculations
+            self.__distance_cache = numpy.linalg.norm(numpy.array([0, 0, self.edge_size/2]) - numpy.array([0, self.edge_size/2, 0]))
+
     def print_cache(self):
         """Print pre-processed data."""
 
@@ -161,6 +170,8 @@ class ArUcoCube():
         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}')
+
+        print(f'\nDistance cache: {self.__distance_cache}')
     
     def __is_rotation_matrix(self, R):
         """Checks if a matrix is a valid rotation matrix."""
@@ -215,6 +226,8 @@ class ArUcoCube():
             #print(f'arcube rotation vector: {self.rotation[0][0]:3f} {self.rotation[1][0]:3f} {self.rotation[2][0]:3f}')
             #print(f'arcube translation vector: {self.translation[0]:3f} {self.translation[1]:3f} {self.translation[2]:3f}')
 
+            return False
+
         # Pose validity checking processes faces two by two
         else:
 
@@ -247,8 +260,15 @@ class ArUcoCube():
                 #print('expected angle:')
                 #print(expected_angle)
 
-                # Check angle according given tolerance then normalise face pose
-                if math.isclose(angle, expected_angle, abs_tol=self.angle_tolerance):
+                # Calculate distance between A face center and B face center
+                distance = numpy.linalg.norm(A_face.translation - B_face.translation)
+                expected_distance = self.__distance_cache
+
+                # Check angle and distance according given tolerance then normalise face 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)
+
+                if valid_angle and valid_distance:
 
                     if A_name not in valid_faces:
 
@@ -285,16 +305,20 @@ class ArUcoCube():
                 #print(f'arcube rotation vector: {self.rotation[0][0]:3f} {self.rotation[1][0]:3f} {self.rotation[2][0]:3f}')
                 #print(f'arcube translation vector: {self.translation[0]:3f} {self.translation[1]:3f} {self.translation[2]:3f}')
 
+                return True
+
+        raise ValueError('Cube pose can\'t be estimated.')
+
         #print('----------------------------------------------------')
 
-    def draw(self, frame, K):
+    def draw(self, frame, K, D):
 
         l = self.edge_size / 2
         ll = self.edge_size
 
         # Draw axis
         axisPoints = numpy.float32([[ll, 0, 0], [0, ll, 0], [0, 0, ll], [0, 0, 0]]).reshape(-1, 3)
-        axisPoints, _ = cv.projectPoints(axisPoints, self.rotation, self.translation, K, (0, 0, 0, 0))
+        axisPoints, _ = cv.projectPoints(axisPoints, self.rotation, self.translation, K, D)
         axisPoints = axisPoints.astype(int)
 
         frame = cv.line(frame, tuple(axisPoints[3].ravel()), tuple(axisPoints[0].ravel()), (0,0,255), 5) # X (red)
@@ -332,6 +356,3 @@ class ArUcoCube():
         frame = cv.line(frame, tuple(frontPoints[3].ravel()), tuple(frontPoints[0].ravel()), (255,0,0), 2)
 
         return frame
-
-
-