Renaming angle_cache and distance_cache into rotation_cache and translation_cache. Calculating AB and BA rotations.

1 files changed, 33 insertions, 35 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index 94451d9..78f95ba 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -327,12 +327,12 @@ class ArUcoScene():
             output += f'\n{place.rotation}'
 
         output += '\n\n\tAngle cache:'
-        for A_identifier, A_angle_cache in self.__angle_cache.items():
+        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.__distance_cache.items():
+        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}'
 
@@ -370,8 +370,8 @@ class ArUcoScene():
     def init_places_consistency(self):
         """Initialize places consistency to speed up further markers consistency checking."""
 
-        # Process axis-angle between place combination to speed up further calculations
-        self.__angle_cache = {}
+        # 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
@@ -379,46 +379,44 @@ class ArUcoScene():
 
             if numpy.array_equal(A, B):
 
-                angle = 0.
+                AB_rvec = [0., 0., 0.]
+                BA_rvec = [0., 0., 0.]
 
             else:
 
-                # Rotation matrix from A place to B place
-                AB = B.dot(A.T)
-
-                # Calculate euler angle representation of AB rotation matrix
-                angle = make_euler_rotation_vector(AB)
+                # 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.__angle_cache[A_identifier][B_identifier] = angle
+                self.__rotation_cache[A_identifier][B_identifier] = AB_rvec
             except:
-                self.__angle_cache[A_identifier] = {B_identifier: angle}
+                self.__rotation_cache[A_identifier] = {B_identifier: AB_rvec}
 
-            # TODO: Are we sure that rotation from A -> B equals B -> A in euler representation ?
             try:
-                self.__angle_cache[B_identifier][A_identifier] = angle
+                self.__rotation_cache[B_identifier][A_identifier] = BA_rvec
             except:
-                self.__angle_cache[B_identifier] = {A_identifier: angle}
+                self.__rotation_cache[B_identifier] = {A_identifier: BA_rvec}
 
-        # Process distance between each place combination to speed up further calculations
-        self.__distance_cache = {}
+        # 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 axis-angle representation of AB rotation matrix
-            distance = numpy.linalg.norm(B - A)
+            # Calculate translation between A and B position
+            AB_tvec = numpy.linalg.norm(B - A)
 
             try:
-                self.__distance_cache[A_identifier][B_identifier] = distance
+                self.__translation_cache[A_identifier][B_identifier] = AB_tvec
             except:
-                self.__distance_cache[A_identifier] = {B_identifier: distance}
+                self.__translation_cache[A_identifier] = {B_identifier: AB_tvec}
 
             try:
-                self.__distance_cache[B_identifier][A_identifier] = distance
+                self.__translation_cache[B_identifier][A_identifier] = AB_tvec
             except:
-                self.__distance_cache[B_identifier] = {A_identifier: distance}
+                self.__translation_cache[B_identifier] = {A_identifier: AB_tvec}
 
     def check_markers_consistency(self, scene_markers: dict, angle_tolerance: float, distance_tolerance: float) -> Tuple[dict, dict]:
         """Evaluate if given markers configuration match related places configuration.
@@ -430,7 +428,7 @@ class ArUcoScene():
         """
 
         consistent_markers = {}
-        unconsistencies = {'angle': {}, 'distance': {}}
+        unconsistencies = {'rotation': {}, 'translation': {}}
 
         for (A_identifier, A_marker), (B_identifier, B_marker) in itertools.combinations(scene_markers.items(), 2):
 
@@ -440,18 +438,18 @@ class ArUcoScene():
                 AB = B_marker.rotation.dot(A_marker.rotation.T)
 
                 # Calculate euler angle representation of AB rotation matrix
-                angle = make_euler_rotation_vector(AB)
-                expected_angle = self.__angle_cache[A_identifier][B_identifier]
+                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
-                distance = numpy.linalg.norm(A_marker.translation - B_marker.translation)
-                expected_distance = self.__distance_cache[A_identifier][B_identifier]
+                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_angle = numpy.allclose(angle, expected_angle, atol=angle_tolerance)
-                consistent_distance = math.isclose(distance, expected_distance, abs_tol=distance_tolerance)
+                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_angle and consistent_distance:
+                if consistent_rotation and consistent_translation:
 
                     if A_identifier not in consistent_markers.keys():
 
@@ -465,11 +463,11 @@ class ArUcoScene():
 
                 else:
 
-                    if not consistent_angle:
-                        unconsistencies['angle'][f'{A_identifier}/{B_identifier}'] = {'current': angle, 'expected': expected_angle}
+                    if not consistent_rotation:
+                        unconsistencies['rotation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_rvec, 'expected': expected_rvec}
                     
-                    if not consistent_distance:
-                        unconsistencies['distance'][f'{A_identifier}/{B_identifier}'] = {'current': distance, 'expected': expected_distance}
+                    if not consistent_translation:
+                        unconsistencies['translation'][f'{A_identifier}/{B_identifier}'] = {'current': AB_tvec, 'expected': expected_tvec}
 
             except KeyError: