Fixing unconstiency checking using euler angle representation.

1 files changed, 40 insertions, 10 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoScene.py b/src/argaze/ArUcoMarkers/ArUcoScene.py
index 43a6ae3..2e6ee2f 100644
--- a/src/argaze/ArUcoMarkers/ArUcoScene.py
+++ b/src/argaze/ArUcoMarkers/ArUcoScene.py
@@ -42,6 +42,34 @@ def make_rotation_matrix(x, y, z):
     # Return intrinsic rotation matrix
     return Rx.dot(Ry.dot(Rz))
 
+def is_rotation_matrix(R):
+
+    Rt = numpy.transpose(R)
+    shouldBeIdentity = numpy.dot(Rt, R)
+    I = numpy.identity(3, dtype = R.dtype)
+    n = numpy.linalg.norm(I - shouldBeIdentity)
+
+    return n < 1e-3
+
+def make_euler_rotation_vector(R):
+ 
+    assert(is_rotation_matrix(R))
+ 
+    sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0])
+ 
+    singular = sy < 1e-6
+ 
+    if  not singular :
+        x = math.atan2(R[2,1] , R[2,2])
+        y = math.atan2(-R[2,0], sy)
+        z = math.atan2(R[1,0], R[0,0])
+    else :
+        x = math.atan2(-R[1,2], R[1,1])
+        y = math.atan2(-R[2,0], sy)
+        z = 0
+ 
+    return numpy.array([numpy.rad2deg(x), numpy.rad2deg(y), numpy.rad2deg(z)])
+
 @dataclass(frozen=True)
 class Place():
     """Define a place as a pose and a marker."""
@@ -105,6 +133,8 @@ class ArUcoScene():
 
                     raise ValueError(f'Bad rotation value: {rvalue}')
 
+                assert(is_rotation_matrix(rmat))
+
                 new_marker = ArUcoMarker.ArUcoMarker(self.dictionary, identifier, self.marker_size)
 
                 new_places[identifier] = Place(tvec, rmat, new_marker)
@@ -241,7 +271,7 @@ class ArUcoScene():
                     W = numpy.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
                     P = numpy.array([place_x_axis, place_y_axis, place_z_axis])
                     Rp = W.dot(P.T)
-
+                    
                     # Check axis size: they should be almost equal
                     if math.isclose(place_x_axis_norm, place_y_axis_norm, rel_tol=1e-3):
 
@@ -299,7 +329,7 @@ class ArUcoScene():
         output += '\n\n\tAngle cache:'
         for A_identifier, A_angle_cache in self.__angle_cache.items():
             for B_identifier, angle in A_angle_cache.items():
-                output += f'\n\t\t- {A_identifier}/{B_identifier}: {angle:3f}'
+                output += f'\n\t\t- {A_identifier}/{B_identifier}: [{angle[0]:3f} {angle[1]:3f} {angle[2]:3f}]'
 
         output += '\n\n\tDistance cache:'
         for A_identifier, A_distance_cache in self.__distance_cache.items():
@@ -356,8 +386,8 @@ class ArUcoScene():
                 # Rotation matrix from A place to B place
                 AB = B.dot(A.T)
 
-                # Calculate axis-angle representation of AB rotation matrix
-                angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
+                # Calculate euler angle representation of AB rotation matrix
+                angle = make_euler_rotation_vector(AB)
 
             try:
                 self.__angle_cache[A_identifier][B_identifier] = angle
@@ -399,7 +429,7 @@ class ArUcoScene():
         """
 
         consistent_markers = {}
-        unconsistencies = {}
+        unconsistencies = {'angle': {}, 'distance': {}}
 
         for (A_identifier, A_marker), (B_identifier, B_marker) in itertools.combinations(scene_markers.items(), 2):
 
@@ -408,8 +438,8 @@ class ArUcoScene():
                 # Rotation matrix from A marker to B marker
                 AB = B_marker.rotation.dot(A_marker.rotation.T)
 
-                # Calculate axis-angles representation of AB rotation matrix
-                angle = numpy.rad2deg(numpy.arccos((numpy.trace(AB) - 1) / 2))
+                # Calculate euler angle representation of AB rotation matrix
+                angle = make_euler_rotation_vector(AB)
                 expected_angle = self.__angle_cache[A_identifier][B_identifier]
 
                 # Calculate distance between A marker center and B marker center
@@ -417,7 +447,7 @@ class ArUcoScene():
                 expected_distance = self.__distance_cache[A_identifier][B_identifier]
 
                 # Check angle and distance according given tolerance then normalise marker pose
-                consistent_angle = math.isclose(angle, expected_angle, abs_tol=angle_tolerance)
+                consistent_angle = numpy.allclose(angle, expected_angle, atol=angle_tolerance)
                 consistent_distance = math.isclose(distance, expected_distance, abs_tol=distance_tolerance)
 
                 if consistent_angle and consistent_distance:
@@ -435,10 +465,10 @@ class ArUcoScene():
                 else:
 
                     if not consistent_angle:
-                        unconsistencies[f'{A_identifier}/{B_identifier} angle'] = angle
+                        unconsistencies['angle'][f'{A_identifier}/{B_identifier}'] = {'current': angle, 'expected': expected_angle}
                     
                     if not consistent_distance:
-                        unconsistencies[f'{A_identifier}/{B_identifier} distance'] = distance
+                        unconsistencies['distance'][f'{A_identifier}/{B_identifier}'] = {'current': distance, 'expected': expected_distance}
 
             except KeyError: