From 6ce4cbba2429e0aed9865a94a8a53a03793c7b2d Mon Sep 17 00:00:00 2001
From: Théo de la Hogue
Date: Wed, 26 Apr 2023 23:05:21 +0200
Subject: Adding a new method to initialize place consistency. Renaming
 variables.

---
 src/argaze/ArUcoMarkers/ArUcoScene.py | 110 ++++++++++++++++++----------------
 1 file changed, 58 insertions(+), 52 deletions(-)

(limited to 'src')

diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index 788d00a..43a6ae3 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -69,7 +69,7 @@ class ArUcoScene():
     """Expected markers place"""
 
     def __post_init__(self):
-        """Initialize cached data to speed up further processings."""
+        """Init scene pose and places pose."""
 
         # Init pose data
         self._translation = numpy.zeros(3)
@@ -116,54 +116,8 @@ class ArUcoScene():
 
         self.places = new_places
 
-        # Process axis-angle between place combination to speed up further calculations
-        self.__angle_cache = {}
-        for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2):
-
-            A = self.places[A_identifier].rotation
-            B = self.places[B_identifier].rotation
-
-            if numpy.array_equal(A, B):
-
-                angle = 0.
-
-            else:
-
-                # Rotation matrix from A place to B place
-                AB = B.dot(A.T)
-
-                # Calculate axis-angle representation of AB rotation matrix
-                angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
-
-            try:
-                self.__angle_cache[A_identifier][B_identifier] = angle
-            except:
-                self.__angle_cache[A_identifier] = {B_identifier: angle}
-
-            try:
-                self.__angle_cache[B_identifier][A_identifier] = angle
-            except:
-                self.__angle_cache[B_identifier] = {A_identifier: angle}
-
-        # Process distance between each place combination to speed up further calculations
-        self.__distance_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)
-
-            try:
-                self.__distance_cache[A_identifier][B_identifier] = distance
-            except:
-                self.__distance_cache[A_identifier] = {B_identifier: distance}
-
-            try:
-                self.__distance_cache[B_identifier][A_identifier] = distance
-            except:
-                self.__distance_cache[B_identifier] = {A_identifier: distance}
+        # Init place consistency
+        self.init_places_consistency()
 
     @classmethod
     def from_obj(self, obj_filepath: str) -> ArUcoSceneType:
@@ -383,6 +337,58 @@ class ArUcoScene():
 
         return scene_markers, remaining_markers
 
+    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 = {}
+        for (A_identifier, A_place), (B_identifier, B_place) in itertools.combinations(self.places.items(), 2):
+
+            A = self.places[A_identifier].rotation
+            B = self.places[B_identifier].rotation
+
+            if numpy.array_equal(A, B):
+
+                angle = 0.
+
+            else:
+
+                # Rotation matrix from A place to B place
+                AB = B.dot(A.T)
+
+                # Calculate axis-angle representation of AB rotation matrix
+                angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
+
+            try:
+                self.__angle_cache[A_identifier][B_identifier] = angle
+            except:
+                self.__angle_cache[A_identifier] = {B_identifier: angle}
+
+            try:
+                self.__angle_cache[B_identifier][A_identifier] = angle
+            except:
+                self.__angle_cache[B_identifier] = {A_identifier: angle}
+
+        # Process distance between each place combination to speed up further calculations
+        self.__distance_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)
+
+            try:
+                self.__distance_cache[A_identifier][B_identifier] = distance
+            except:
+                self.__distance_cache[A_identifier] = {B_identifier: distance}
+
+            try:
+                self.__distance_cache[B_identifier][A_identifier] = distance
+            except:
+                self.__distance_cache[B_identifier] = {A_identifier: distance}
+
     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.
 
@@ -441,11 +447,11 @@ class ArUcoScene():
         # Gather unconsistent markers
         unconsistent_markers = {}
 
-        for name, marker in scene_markers.items():
+        for identifier, marker in scene_markers.items():
 
-            if name not in consistent_markers:
+            if identifier not in consistent_markers.keys():
 
-                unconsistent_markers[name] = marker
+                unconsistent_markers[identifier] = marker
 
         return consistent_markers, unconsistent_markers, unconsistencies
 
-- 
cgit v1.1