diff options
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoScene.py | 12 |
1 files changed, 1 insertions, 11 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py index 2ca1542..3ea4b43 100644 --- a/src/argaze/ArUcoMarkers/ArUcoScene.py +++ b/src/argaze/ArUcoMarkers/ArUcoScene.py @@ -106,8 +106,6 @@ class ArUcoScene(): # Rotation matrix from A place to B place AB = B.dot(A.T) - #assert(self.__is_rotation_matrix(AB)) - # Calculate axis-angle representation of AB rotation matrix angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2)) @@ -285,7 +283,7 @@ class ArUcoScene(): # RM = P M = numpy.array([marker_x_axis, marker_y_axis, marker_z_axis]) P = numpy.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]) - R = P.dot(numpy.linalg.inv(M)) + R = P.dot(M.T) marker = markers[name] @@ -294,12 +292,6 @@ class ArUcoScene(): except IOError: raise IOError(f'File not found: {obj_filepath}') - def __is_rotation_matrix(self, R): - """Checks if a matrix is a valid rotation matrix.""" - - I = numpy.identity(3, dtype = R.dtype) - return numpy.linalg.norm(I - numpy.dot(R.T, R)) < 1e-6 - def __normalise_marker_pose(self, place, R, T): # Transform marker rotation into scene rotation matrix @@ -366,8 +358,6 @@ class ArUcoScene(): # Rotation matrix from A marker to B marker AB = B_marker.rotation.dot(A_marker.rotation.T) - assert(self.__is_rotation_matrix(AB)) - # Calculate axis-angles representation of AB rotation matrix angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2)) expected_angle = self.__angle_cache[A_name][B_name] |