From e26dc74f080891b9a521b19a1150bf4a0d10e425 Mon Sep 17 00:00:00 2001 From: Théo de la Hogue Date: Wed, 30 Nov 2022 14:52:11 +0100 Subject: Fixing .OBJ makers loading. Calculating rotation from normal vector. --- src/argaze/ArUcoMarkers/ArUcoScene.py | 97 ++++++++++++++++++++++++----------- 1 file changed, 66 insertions(+), 31 deletions(-) diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py index b267102..6fe0d0f 100644 --- a/src/argaze/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze/ArUcoMarkers/ArUcoScene.py @@ -94,7 +94,7 @@ class ArUcoScene(): for name, place in self.__places.items(): # Create intrinsic rotation matrix - R = self.__make_rotation_matrix(*place.rotation) + R = self.__euler_vector_to_rotation_matrix(*place.rotation) assert(self.__is_rotation_matrix(R)) @@ -196,20 +196,23 @@ class ArUcoScene(): return list(self.__identifier_cache.keys()) def __load_places_from_obj(self, obj_filepath: str) -> dict: - """Load places from .obj file.""" + """Load places from .obj file. + + .. warning:: 'o' tag string format should be DICTIONARY#IDENTIFIER_NAME + """ self.__places = {} - # regex rules for .obj file parsing + # Regex rules for .obj file parsing OBJ_RX_DICT = { - 'comment': re.compile(r'#(.*)\n'), - 'name': re.compile(r'o (\w+)(.*)\n'), + 'object': re.compile(r'o (.*)#([0-9]+)_(.*)\n'), 'vertice': re.compile(r'v ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'), 'normal': re.compile(r'vn ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+) ([+-]?[0-9]*[.]?[0-9]+)\n'), - 'face': re.compile(r'f ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+)\n') + 'face': re.compile(r'f ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+) ([0-9]+)//([0-9]+)\n'), + 'comment': re.compile(r'#(.*)\n') # keep comment regex after object regex because the # is used in object string too } - # regex .obj line parser + # Regex .obj line parser def __parse_obj_line(line): for key, rx in OBJ_RX_DICT.items(): @@ -217,72 +220,104 @@ class ArUcoScene(): if match: return key, match - # if there are no matches + # If there are no matches return None, None - # start parsing + # Start parsing try: name = None vertices = [] - normals = {} + markers = {} + rotations = {} faces = {} - # open the file and read through it line by line + # Open the file and read through it line by line with open(obj_filepath, 'r') as file: line = file.readline() while line: - # at each line check for a match with a regex + # At each line check for a match with a regex key, match = __parse_obj_line(line) - # extract comment + # Extract comment if key == 'comment': pass - # extract place name - elif key == 'name': + # Extract marker dictionary and identifier + elif key == 'object': + + dictionary = str(match.group(1)) + identifier = int(match.group(2)) + last = str(match.group(3)) + + # Check that marker dictionary is like the scene dictionary + if dictionary == self.__dictionary.name: - name = str(match.group(1)) + name = f'{dictionary}#{identifier}' # ignore last part + markers[name] = ArUcoMarker.ArUcoMarker(self.__dictionary, identifier, self.__marker_size) - # fill vertices array + else: + + raise NameError(f'Marker#{identifier} dictionary is not {self.__dictionary.name}') + + # Fill vertices array elif key == 'vertice': vertices.append(tuple([float(match.group(1)), float(match.group(2)), float(match.group(3))])) - # extract normal + # Extract normal to calculate rotation vectore elif key == 'normal': - normals[name] = numpy.array([float(match.group(1)), float(match.group(2)), float(match.group(3))]) + R = self.__normal_vector_to_rotation_matrix(float(match.group(1)), float(match.group(2)), float(match.group(3))) + rvecs, _ = cv.Rodrigues(R) + rvecs = rvecs.reshape(1, 3)[0] + rvecs = numpy.array([numpy.rad2deg(rvecs[0]), numpy.rad2deg(rvecs[1]), numpy.rad2deg(rvecs[2])]) + + print(name) + print(R) + print(rvecs) + + rotations[name] = rvecs - # extract aoi3D vertice id + # Extract vertice ids elif key == 'face': - faces[name] = [int(i) for i in match.group(1).split()] + faces[name] = [int(match.group(1)), int(match.group(3)), int(match.group(5)), int(match.group(7))] - # go to next line + # Go to next line line = file.readline() file.close() - # retreive all place vertices + # Retreive marker vertices thanks to face vertice ids for name, face in faces.items(): - center = numpy.array([ vertices[i-1] for i in face ]).mean(axis=0) - normal = normals[name] + translation = numpy.array([ vertices[i-1] for i in face ]).mean(axis=0) + rotation = rotations[name] + marker = markers[name] - # WARNING: here we set marker identifier depending on the place position in the file - # TODO: extract identifiers from name - marker = ArUcoMarker.ArUcoMarker(self.__dictionary, len(self.__places), self.__marker_size) - - self.__places[name] = Place(center, normal, marker) + self.__places[name] = Place(translation, rotation, marker) except IOError: raise IOError(f'File not found: {obj_filepath}') - def __make_rotation_matrix(self, x, y, z): + def __normal_vector_to_rotation_matrix(self, x, y, z): + # Applying formula found at https://math.stackexchange.com/questions/1956699/getting-a-transformation-matrix-from-a-normal-vector + + xy_dist = math.sqrt(x**2 + y**2) + + if xy_dist > 0: + + return numpy.array([[y/xy_dist, -x/xy_dist, 0], [x*z/xy_dist, y*z/xy_dist, -xy_dist], [x, y, z]]) + + else: + + return numpy.array([[1, 0, 0], [0, 1, 0], [x, y, z]]) + + def __euler_vector_to_rotation_matrix(self, x, y, z): # Create rotation matrix around x axis c = numpy.cos(numpy.deg2rad(x)) -- cgit v1.1