Removing rotation matrix assertion. Using matrix transposition instead of inverse.

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]