diff options
Diffstat (limited to 'src')
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoScene.py | 110 |
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 |