diff options
| -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) |