diff options
| -rw-r--r-- | src/argaze.test/ArFeatures.py | 12 | ||||
| -rw-r--r-- | src/argaze/ArFeatures.py | 145 | ||||
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoScene.py | 271 |
3 files changed, 202 insertions, 226 deletions
diff --git a/src/argaze.test/ArFeatures.py b/src/argaze.test/ArFeatures.py index 8e7ff49..af8cfa8 100644 --- a/src/argaze.test/ArFeatures.py +++ b/src/argaze.test/ArFeatures.py @@ -45,13 +45,13 @@ class TestArEnvironmentClass(unittest.TestCase): # Check Aruco scene self.assertEqual(len(ar_scene.aruco_scene.places), 2) - self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places['A'].translation, [1, 0, 0])) - self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places['A'].rotation, [[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])) - self.assertEqual(ar_scene.aruco_scene.places['A'].marker.identifier, 0) + self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places[0].translation, [1, 0, 0])) + self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places[0].rotation, [[1.,0.,0.],[0.,1.,0.],[0.,0.,1.]])) + self.assertEqual(ar_scene.aruco_scene.places[0].marker.identifier, 0) - self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places['B'].translation, [0, 1, 0])) - self.assertIsNone(numpy.testing.assert_array_almost_equal(ar_scene.aruco_scene.places['B'].rotation, [[0.,0.,1.],[0., 1.,0.],[-1.,0.,0.]])) - self.assertEqual(ar_scene.aruco_scene.places['B'].marker.identifier, 1) + self.assertIsNone(numpy.testing.assert_array_equal(ar_scene.aruco_scene.places[1].translation, [0, 1, 0])) + self.assertIsNone(numpy.testing.assert_array_almost_equal(ar_scene.aruco_scene.places[1].rotation, [[0.,0.,1.],[0., 1.,0.],[-1.,0.,0.]])) + self.assertEqual(ar_scene.aruco_scene.places[1].marker.identifier, 1) # Check AOI scene self.assertEqual(len(ar_scene.aoi_scene.items()), 1) diff --git a/src/argaze/ArFeatures.py b/src/argaze/ArFeatures.py index 4e1f5cb..3706aac 100644 --- a/src/argaze/ArFeatures.py +++ b/src/argaze/ArFeatures.py @@ -24,31 +24,72 @@ class ArEnvironment(): """Define an Augmented Reality environment based ArUco marker detection.""" name: str - """Environement name""" + """Environment name""" - aruco_detector: ArUcoDetector.ArUcoDetector = field(init=False, default_factory=ArUcoDetector.ArUcoDetector) - """ArUco detector""" - - def __init__(self, **kwargs): - - self.name = kwargs.pop('name') + working_directory: str + """Environment working directory""" - self.aruco_detector = ArUcoDetector.ArUcoDetector(**kwargs.pop('aruco_detector')) - - self.__scenes = {} - for name, scene_kwargs in kwargs.items(): + aruco_detector: ArUcoDetector.ArUcoDetector = field(default_factory=ArUcoDetector.ArUcoDetector) + """ArUco detector""" - self.__scenes[name] = ArScene(self, **scene_kwargs) + scenes: dict = field(default_factory=dict) + """All environment scenes""" @classmethod def from_json(self, json_filepath: str) -> ArSceneType: """Load ArEnvironment from .json file.""" + new_name = '' + new_working_directory = '.' + new_aruco_detector = None + new_scenes = {} + with open(json_filepath) as configuration_file: - self.__working_directory = os.path.dirname(json_filepath) + data = json.load(configuration_file) + + new_name = data.pop('name') + + new_working_directory = os.path.dirname(json_filepath) + + new_aruco_detector = ArUcoDetector.ArUcoDetector(**data.pop('aruco_detector')) + + for scene_name, scene_data in data.items(): + + new_aruco_scene = None + new_aoi_scene = None + + # Check aruco_scene value type + aruco_scene_value = scene_data.pop('aruco_scene') - return ArEnvironment(**json.load(configuration_file)) + # str: relative path to .obj file + if type(aruco_scene_value) == str: + + aruco_scene_value = os.path.join(new_working_directory, aruco_scene_value) + new_aruco_scene = ArUcoScene.ArUcoScene.from_obj(aruco_scene_value) + + # dict: + else: + + new_aruco_scene = ArUcoScene.ArUcoScene(new_aruco_detector.marker_size, new_aruco_detector.dictionary, aruco_scene_value) + + # Check aoi_scene value type + aoi_scene_value = scene_data.pop('aoi_scene') + + # str: relative path to .obj file + if type(aoi_scene_value) == str: + + obj_filepath = os.path.join(new_working_directory, aoi_scene_value) + new_aoi_scene = AOI3DScene.AOI3DScene.from_obj(obj_filepath) + + # dict: + else: + + new_aoi_scene = AOI3DScene.AOI3DScene(aoi_scene_value) + + new_scenes[scene_name] = ArScene(self, new_aruco_scene, new_aoi_scene, **scene_data) + + return ArEnvironment(new_name, new_working_directory, new_aruco_detector, new_scenes) def __str__(self) -> str: """String display""" @@ -60,17 +101,6 @@ class ArEnvironment(): return output - @property - def working_directory(self): - """Working directory path.""" - return self.__working_directory - - @property - def scenes(self) -> Tuple[str, ArSceneType]: - """Access to scenes dictionary.""" - - return self.__scenes - class PoseEstimationFailed(Exception): """Exception raised by ArScene project method when the pose can't be estimated due to unconsistencies.""" @@ -91,72 +121,33 @@ class SceneProjectionFailed(Exception): class ArScene(): """Define an Augmented Reality scene with ArUco markers and AOI scenes.""" - aruco_scene: ArUcoScene.ArUcoScene = field(init=False, default_factory=ArUcoScene.ArUcoScene) + ar_environment: ArEnvironment = field(default_factory=ArEnvironment) + """AR Environment to which the scene belongs.""" + + aruco_scene: ArUcoScene.ArUcoScene = field(default_factory=ArUcoScene.ArUcoScene) """ArUco markers 3D scene description used to estimate scene pose from detected markers: see `estimate_pose` function below.""" - aoi_scene: AOI3DScene.AOI3DScene = field(init=False, default_factory=AOI3DScene.AOI3DScene) + aoi_scene: AOI3DScene.AOI3DScene = field(default_factory=AOI3DScene.AOI3DScene) """AOI 3D scene description that will be projected onto estimated scene once its pose will be estimated : see `project` function below.""" - aruco_axis: dict + aruco_axis: dict = field(default_factory=dict) """Optional dictionary to define orthogonal axis where each axis is defined by list of 3 markers identifier (first is origin). \ This pose estimation strategy is used by `estimate_pose` function when at least 3 markers are detected.""" - aruco_aoi: dict + aruco_aoi: dict = field(default_factory=dict) """Optional dictionary of AOI defined by list of markers identifier and markers corners index tuples: see `build_aruco_aoi_scene` function below.""" - angle_tolerance: float + angle_tolerance: float = field(default=0.) """Optional angle error tolerance to validate marker pose in degree used into `estimate_pose` function.""" - distance_tolerance: float + distance_tolerance: float = field(default=0.) """Optional distance error tolerance to validate marker pose in centimeter used into `estimate_pose` function.""" - def __init__(self, ar_environment: ArEnvironment, **kwargs): - - self.__ar_environment = ar_environment - - # Check aruco_scene value type - aruco_scene_value = kwargs.pop('aruco_scene') - - # str: relative path to .obj file - if type(aruco_scene_value) == str: - - aruco_scene_value = os.path.join(self.__ar_environment.working_directory, aruco_scene_value) - - self.aruco_scene = ArUcoScene.ArUcoScene(aruco_scene_value, self.__ar_environment.aruco_detector.dictionary, self.__ar_environment.aruco_detector.marker_size) - - # Check aoi_scene value type - aoi_scene_value = kwargs.pop('aoi_scene') - - # str: relative path to .obj file - if type(aoi_scene_value) == str: - - obj_filepath = os.path.join(self.__ar_environment.working_directory, aoi_scene_value) - self.aoi_scene = AOI3DScene.AOI3DScene.from_obj(obj_filepath) - - # dict: all AOI - else: - self.aoi_scene = AOI3DScene.AOI3DScene(aoi_scene_value) - - # Init aruco axis - self.aruco_axis = {} - - # Init aruco aoi - self.aruco_aoi = {} - - # Update all attributes from arguments - self.__dict__.update(kwargs) + def __post_init__(self): # Preprocess orthogonal projection to speed up further aruco aoi processings self.__orthogonal_projection_cache = self.orthogonal_projection - @classmethod - def from_json(self, json_filepath: str) -> ArSceneType: - """Load ArScene from .json file.""" - - with open(json_filepath) as configuration_file: - - return ArScene(**json.load(configuration_file)) - def __str__(self) -> str: """String display""" @@ -270,7 +261,7 @@ class ArScene(): aoi_scene_copy = self.aoi_scene.copy() - aoi_scene_projection = aoi_scene_copy.project(tvec, rvec, self.__ar_environment.aruco_detector.camera.K) + aoi_scene_projection = aoi_scene_copy.project(tvec, rvec, self.ar_environment.aruco_detector.camera.K) # Warn user when the projected scene is empty if len(aoi_scene_projection) == 0: @@ -320,11 +311,11 @@ class ArScene(): def draw_axis(self, frame): """Draw scene axis into frame.""" - self.aruco_scene.draw_axis(frame, self.__ar_environment.aruco_detector.camera.K, self.__ar_environment.aruco_detector.camera.D) + self.aruco_scene.draw_axis(frame, self.ar_environment.aruco_detector.camera.K, self.ar_environment.aruco_detector.camera.D) def draw_places(self, frame): """Draw scene places into frame.""" - self.aruco_scene.draw_places(frame, self.__ar_environment.aruco_detector.camera.K, self.__ar_environment.aruco_detector.camera.D) + self.aruco_scene.draw_places(frame, self.ar_environment.aruco_detector.camera.K, self.ar_environment.aruco_detector.camera.D) diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py index bc89287..19fcf07 100644 --- a/src/argaze/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze/ArUcoMarkers/ArUcoScene.py @@ -35,31 +35,119 @@ class Place(): marker: dict """ArUco marker linked to the place.""" +@dataclass class ArUcoScene(): """Handle group of ArUco markers as one unique spatial entity and estimate its pose.""" - def __init__(self, places: dict, dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = None, marker_size: float = 0.): - """Define scene attributes.""" + marker_size: float = field(default=0.) + """Expected size of all markers in the scene.""" - # Init expected marker size - self.__marker_size = marker_size + dictionary: ArUcoMarkersDictionary.ArUcoMarkersDictionary = field(default_factory=ArUcoMarkersDictionary.ArUcoMarkersDictionary) + """Expected dictionary of all markers in the scene.""" - # Handle dictionary str or instance - if dictionary != None: + places: dict = field(default_factory=dict) + """Expected markers place""" + + def __post_init__(self): + """Initialize cached data to speed up further processings.""" + + # Init pose data + self._translation = numpy.zeros(3) + self._rotation = numpy.zeros(3) + + # Normalize places data + new_places = {} + + def __make_rotation_matrix(x, y, z): + + # Create rotation matrix around x axis + c = numpy.cos(numpy.deg2rad(x)) + s = numpy.sin(numpy.deg2rad(x)) + Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]]) + + # Create rotation matrix around y axis + c = numpy.cos(numpy.deg2rad(y)) + s = numpy.sin(numpy.deg2rad(y)) + Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) + + # Create rotation matrix around z axis + c = numpy.cos(numpy.deg2rad(z)) + s = numpy.sin(numpy.deg2rad(z)) + Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) + + # Return intrinsic rotation matrix + return Rx.dot(Ry.dot(Rz)) + + for identifier, place in self.places.items(): + + # Convert string identifier to int value + if type(identifier) == str: + + identifier = int(identifier) + + # Convert translation and rotation keys to Place object + tvec = numpy.array(place.pop('translation')).astype(numpy.float32) + rmat = __make_rotation_matrix(*place.pop('rotation')).astype(numpy.float32) + + new_marker = ArUcoMarker.ArUcoMarker(self.dictionary, identifier, self.marker_size) + + new_places[identifier] = Place(tvec, rmat, new_marker) + + # else places are already at expected format + elif (type(identifier) == int) and isinstance(place, Place): + + new_places[identifier] = place + + 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. - if type(dictionary) == str: - self.__dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary(dictionary) - elif isinstance(dictionary, ArUcoMarkersDictionary.ArUcoMarkersDictionary): - self.__dictionary = dictionary else: - raise ValueError(f'dictionary: {dictionary}') - 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} - self.__dictionary = ArUcoMarkersDictionary.ArUcoMarkersDictionary() + 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): - # NEVER USE {} as default function argument - self.places = places + 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} @classmethod def from_obj(self, obj_filepath: str) -> ArUcoSceneType: @@ -76,9 +164,9 @@ class ArUcoScene(): """ - new_places = {} - new_dictionary = None new_marker_size = 0 + new_dictionary = None + new_places = {} # Regex rules for .obj file parsing OBJ_RX_DICT = { @@ -205,138 +293,35 @@ class ArUcoScene(): except IOError: raise IOError(f'File not found: {obj_filepath}') - return ArUcoScene(new_places, new_dictionary, new_marker_size) + return ArUcoScene(new_marker_size, new_dictionary, new_places) @classmethod def from_json(self, json_filepath: str) -> ArUcoSceneType: """Load ArUco scene from .json file.""" - new_places = {} - new_dictionary = None new_marker_size = 0 - - def __make_rotation_matrix(x, y, z): - - # Create rotation matrix around x axis - c = numpy.cos(numpy.deg2rad(x)) - s = numpy.sin(numpy.deg2rad(x)) - Rx = numpy.array([[1, 0, 0], [0, c, -s], [0, s, c]]) - - # Create rotation matrix around y axis - c = numpy.cos(numpy.deg2rad(y)) - s = numpy.sin(numpy.deg2rad(y)) - Ry = numpy.array([[c, 0, s], [0, 1, 0], [-s, 0, c]]) - - # Create rotation matrix around z axis - c = numpy.cos(numpy.deg2rad(z)) - s = numpy.sin(numpy.deg2rad(z)) - Rz = numpy.array([[c, -s, 0], [s, c, 0], [0, 0, 1]]) - - # Return intrinsic rotation matrix - return Rx.dot(Ry.dot(Rz)) + new_dictionary = None + new_places = {} with open(json_filepath) as configuration_file: data = json.load(configuration_file) - new_dictionary = data.pop('dictionary') new_marker_size = data.pop('marker_size') + new_dictionary = data.pop('dictionary') + new_places = data.pop('places') - for identifier, place in data.pop('places').items(): - - # Convert string identifier to int value - identifier = int(identifier) - - tvec = numpy.array(place.pop('translation')).astype(numpy.float32) - rmat = __make_rotation_matrix(*place.pop('rotation')).astype(numpy.float32) - - new_marker = ArUcoMarker.ArUcoMarker(new_dictionary, identifier, new_marker_size) - - new_places[identifier] = Place(tvec, rmat, new_marker) - - return ArUcoScene(new_places, new_dictionary, new_marker_size) - - @property - def marker_size(self) -> float: - """Expected size of all markers in the scene.""" - - return self.__marker_size - - @property - def places(self) -> dict: - """All places of the scene and their related ArUco markers.""" - - return self.__places - - @places.setter - def places(self, places: dict): - """Initialize cached data to speed up further processings.""" - - # Init places - self.__places = places - - # Init pose data - self._translation = numpy.zeros(3) - self._rotation = numpy.zeros(3) - - # 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} + return ArUcoScene(new_marker_size, new_dictionary, new_places) def __str__(self) -> str: """String display""" - output = f'\n\n\tDictionary: {self.__dictionary.name}' + output = f'\n\n\tDictionary: {self.dictionary.name}' - output = f'\n\n\tMarker size: {self.__marker_size} cm' + output = f'\n\n\tMarker size: {self.marker_size} cm' output += '\n\n\tPlaces:' - for identifier, place in self.__places.items(): + for identifier, place in self.places.items(): output += f'\n\t\t- {identifier}:' output += f'\n{place.translation}' output += f'\n{place.rotation}' @@ -357,7 +342,7 @@ class ArUcoScene(): def identifiers(self) -> list: """List place maker identifiers belonging to the scene.""" - return list(self.__places.keys()) + return list(self.places.keys()) def filter_markers(self, detected_markers: dict) -> Tuple[dict, dict]: """Sort markers belonging to the scene from given detected markers dict (cf ArUcoDetector.detect_markers()). @@ -372,7 +357,7 @@ class ArUcoScene(): for (marker_id, marker) in detected_markers.items(): - if marker_id in self.__places.keys(): + if marker_id in self.places.keys(): scene_markers[marker_id] = marker @@ -454,7 +439,7 @@ class ArUcoScene(): # Get the place related to the given marker try: - place = self.__places[marker.identifier] + place = self.places[marker.identifier] # Rotation matrix that transform marker to related place self._rotation = marker.rotation.dot(place.rotation.T) @@ -478,7 +463,7 @@ class ArUcoScene(): try: - place = self.__places[identifier] + place = self.places[identifier] # Rotation matrix that transform marker to related place R = marker.rotation.dot(place.rotation.T) @@ -509,9 +494,9 @@ class ArUcoScene(): A_marker = horizontal_axis_marker B_marker = vertical_axis_marker - O_place = self.__places[O_marker.identifier] - A_place = self.__places[A_marker.identifier] - B_place = self.__places[B_marker.identifier] + O_place = self.places[O_marker.identifier] + A_place = self.places[A_marker.identifier] + B_place = self.places[B_marker.identifier] # Place axis OA = A_place.translation - O_place.translation @@ -571,8 +556,8 @@ class ArUcoScene(): def draw_axis(self, frame, K, D, consistency=2): """Draw scene axis according a consistency score.""" - l = self.__marker_size / 2 - ll = self.__marker_size + l = self.marker_size / 2 + ll = self.marker_size # Select color according consistency score n = 95 * consistency if consistency < 2 else 0 @@ -590,19 +575,19 @@ class ArUcoScene(): def draw_places(self, frame, K, D, consistency=2): """Draw scene places and their axis according a consistency score.""" - l = self.__marker_size / 2 - ll = self.__marker_size + l = self.marker_size / 2 + ll = self.marker_size # Select color according consistency score n = 95 * consistency if consistency < 2 else 0 f = 159 * consistency if consistency < 2 else 255 - for identifier, place in self.__places.items(): + for identifier, place in self.places.items(): try: - T = self.__places[identifier].translation - R = self.__places[identifier].rotation + T = self.places[identifier].translation + R = self.places[identifier].rotation # Draw place axis axisPoints = (T + numpy.float32([R.dot([l/2, 0, 0]), R.dot([0, l/2, 0]), R.dot([0, 0, l/2]), R.dot([0, 0, 0])])).reshape(-1, 3) |