aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
authorThéo de la Hogue2022-11-30 14:52:11 +0100
committerThéo de la Hogue2022-11-30 14:52:11 +0100
commite26dc74f080891b9a521b19a1150bf4a0d10e425 (patch)
treebb7cccef337671b41734928151cbc60e3a29b84b /src
parentb614be479e8e4174f9fb706fa6539ffbdad63ecc (diff)
downloadargaze-e26dc74f080891b9a521b19a1150bf4a0d10e425.zip
argaze-e26dc74f080891b9a521b19a1150bf4a0d10e425.tar.gz
argaze-e26dc74f080891b9a521b19a1150bf4a0d10e425.tar.bz2
argaze-e26dc74f080891b9a521b19a1150bf4a0d10e425.tar.xz
Fixing .OBJ makers loading. Calculating rotation from normal vector.
Diffstat (limited to 'src')
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoScene.py97
1 files 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))