diff options
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoScene.py | 68 |
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: |