diff options
author | Théo de la Hogue | 2023-04-12 11:01:46 +0200 |
---|---|---|
committer | Théo de la Hogue | 2023-04-12 11:01:46 +0200 |
commit | 98bae44272c0c9c3c6e04196cd5b24ddb2eb0ddd (patch) | |
tree | a57cf160c83c2efa97129f5490b2ed1f8f7e6e12 | |
parent | 37bfbf9369db78fe41f1564509ec66e53a6c5ad8 (diff) | |
download | argaze-98bae44272c0c9c3c6e04196cd5b24ddb2eb0ddd.zip argaze-98bae44272c0c9c3c6e04196cd5b24ddb2eb0ddd.tar.gz argaze-98bae44272c0c9c3c6e04196cd5b24ddb2eb0ddd.tar.bz2 argaze-98bae44272c0c9c3c6e04196cd5b24ddb2eb0ddd.tar.xz |
Externalizing make_rotation_matrix function to use it else where than inside the ArScene code.
-rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoScene.py | 44 |
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) |