aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/argaze/ArUcoMarkers/ArUcoScene.py44
1 files changed, 22 insertions, 22 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index 06faa41..868e84d 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -22,6 +22,26 @@ R0 = numpy.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
ArUcoSceneType = TypeVar('ArUcoScene', bound="ArUcoScene")
# Type definition for type annotation convenience
+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))
+
@dataclass(frozen=True)
class Place():
"""Define a place as a pose and a marker."""
@@ -58,26 +78,6 @@ class ArUcoScene():
# 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
@@ -85,9 +85,9 @@ class ArUcoScene():
identifier = int(identifier)
- # Convert translation and rotation keys to Place object
+ # Convert translation and rotation values
tvec = numpy.array(place.pop('translation')).astype(numpy.float32)
- rmat = __make_rotation_matrix(*place.pop('rotation')).astype(numpy.float32)
+ rmat = make_rotation_matrix(*place.pop('rotation')).astype(numpy.float32)
new_marker = ArUcoMarker.ArUcoMarker(self.dictionary, identifier, self.marker_size)