aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoScene.py110
1 files changed, 58 insertions, 52 deletions
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