Fix quaternion comparison

LICENSE.txt

@@ -0,0 +1 @@
+Satellogic SA Copyright 2017. All our code is GPLv3 licensed.

quaternions/quaternions.py

@@ -10,6 +10,7 @@ class QuaternionError(Exception):
 class Quaternion(object):
     ''' A class that holds quaternions. It actually holds Q^op, as
     this is the way Schaub-Jenkins work with them.
+    Note: Quaternion is equal up to scale, but it's not stored normalized.
     '''
     tolerance = 1e-8
 
@@ -42,7 +43,7 @@ def __mul__(self, p):
                 [self.qj, -self.qk,  self.qr,  self.qi],  # noqa
                 [self.qk,  self.qj, -self.qi,  self.qr]   # noqa
             ])
-            result = mat.dot(np.array(p.coordinates))
+            result = mat.dot(p.normalized().coordinates)
             return Quaternion(*result)
         elif isinstance(p, Iterable):
             return self.matrix.dot(p)
@@ -84,8 +85,8 @@ def is_equal(self, other):
         compares as quaternions up to tolerance.
         Note: tolerance in coords, not in quaternions metrics.
         """
-        q1 = np.asarray(self.normalized().coordinates)
-        q2 = np.asarray(other.normalized().coordinates)
+        q1 = self.normalized().coordinates
+        q2 = other.normalized().coordinates
         dist = min(np.linalg.norm(q1 - q2), np.linalg.norm(q1 - (-q2)))
         return dist < self.tolerance
 
@@ -95,40 +96,58 @@ def __eq__(self, other):
     def norm(self):
         return np.sqrt(self._squarenorm())
 
-    def exp(self):
-        exp_norm = np.exp(self.qr)
+    @classmethod
+    def exp(cls, arr):
+        """
+        exponent quaternion
+        :param arr: list of 4 items or Quaternion
+        :return: Quaternion
+        """
+        if isinstance(arr, Quaternion):
+            real, imag = arr.coordinates[0], arr.coordinates[1:]
+        else:
+            real, imag = arr[0], np.asarray(arr[1:])
+
+        exp_norm = np.exp(real)
 
-        imag = np.array([self.qi, self.qj, self.qk])
         imag_norm = np.linalg.norm(imag)
         if imag_norm == 0:
             return Quaternion(exp_norm, 0, 0, 0)
 
-        imag_renorm = np.sin(imag_norm) * imag / imag_norm
-        q = Quaternion(np.cos(imag_norm), *imag_renorm)
+        j, k, l = np.sin(imag_norm) * imag / imag_norm
+        q = Quaternion(np.cos(imag_norm), j, k, l)
 
         return exp_norm * q
 
     def log(self):
+        """
+        logarithm of quaternion
+        :return: np.array with 4 items
+        """
         norm = self.norm()
         imag = np.array((self.qi, self.qj, self.qk)) / norm
         imag_norm = np.linalg.norm(imag)
         if imag_norm == 0:
             i_part = 0 if self.qr > 0 else np.pi
-            return Quaternion(np.log(norm), i_part, 0, 0)
-        imag = imag / imag_norm * np.arctan2(imag_norm, self.qr / norm)
-        return Quaternion(np.log(norm), *imag)
+            return np.array([np.log(norm), i_part, 0, 0])
+
+        j, k, l = imag / imag_norm * np.arctan2(imag_norm, self.qr / norm)
+        return np.array([np.log(norm), j, k, l])
 
     def distance(self, other):
         '''Returns the distance in radians between two unitary quaternions'''
-        quot = (self * other.conjugate()).positive_representant
-        return 2 * quot.log().norm()
+        quot = (self * other.conjugate()).normalized().positive_representant
+        return np.linalg.norm(np.multiply(2, quot.log()))
 
     def is_unitary(self):
         return abs(self._squarenorm() - 1) < self.tolerance
 
     @property
     def coordinates(self):
-        return self.qr, self.qi, self.qj, self.qk
+        """
+        :return: np.array of length 4
+        """
+        return np.array([self.qr, self.qi, self.qj, self.qk])
 
     @property
     def positive_representant(self):
@@ -186,7 +205,7 @@ def matrix(self):
 
     @property
     def rotation_vector(self):
-        return (2 * self.log()).coordinates[1:]
+        return (2 * self.log())[1:]
 
     @property
     def ra_dec_roll(self):
@@ -242,8 +261,8 @@ def from_rotation_vector(xyz):
         This corresponds to the exponential of the quaternion with
         real part 0 and imaginary part 1/2 * xyz.
         '''
-        xyz_half = .5 * np.array(xyz)
-        return Quaternion(0, *xyz_half).exp()
+        a, b, c = .5 * np.array(xyz)
+        return Quaternion.exp([0, a, b, c ])
 
     @staticmethod
     def _first_eigenvector(matrix):
@@ -331,12 +350,9 @@ def from_ra_dec_roll(ra, dec, roll):
         dec stands for declination, and usually lies in [-90, 90]
         roll stands for rotation/rolling, and usually lies in [0, 360]
         '''
-        raq = Quaternion.exp(Quaternion(0, 0, 0, -np.deg2rad(ra) / 2,
-                                        validate_numeric_stability=False))
-        decq = Quaternion.exp(Quaternion(0, 0, -np.deg2rad(dec) / 2, 0,
-                                         validate_numeric_stability=False))
-        rollq = Quaternion.exp(Quaternion(0, -np.deg2rad(roll) / 2, 0, 0,
-                                          validate_numeric_stability=False))
+        raq = Quaternion.exp([0, 0, 0, -np.deg2rad(ra) / 2])
+        decq = Quaternion.exp([0, 0, -np.deg2rad(dec) / 2, 0])
+        rollq = Quaternion.exp([0, -np.deg2rad(roll) / 2, 0, 0])
         return rollq * decq * raq
 
     @staticmethod

quaternions/version.py

@@ -1,2 +1,2 @@
 # https://www.python.org/dev/peps/pep-0440/
-__version__ = '0.1.3.dev0'
+__version__ = '0.1.3.dev1'

setup.cfg

@@ -0,0 +1,2 @@
+[metadata]
+description-file = README.md

tests/test_quaternions.py

@@ -14,12 +14,12 @@ class QuaternionTest(unittest.TestCase):
     schaub_result = np.array([.961798, -.14565, .202665, .112505])
 
     def test_matrix_respects_product(self):
-        q1 = Quaternion.exp(Quaternion(0, .1, .02, -.3))
-        q2 = Quaternion.exp(Quaternion(0, -.2, .21, .083))
+        q1 = Quaternion.exp([0, .1, .02, -.3])
+        q2 = Quaternion.exp([0, -.2, .21, .083])
         np.testing.assert_allclose((q1 * q2).matrix, q1.matrix.dot(q2.matrix))
 
     def test_quaternion_rotates_vector(self):
-        q1 = Quaternion.exp(Quaternion(0, .1, .02, -.3))
+        q1 = Quaternion.exp([0, .1, .02, -.3])
         vector = QuaternionTest.schaub_example_dcm[:, 1]
         rotated_vector = q1 * vector
         np.testing.assert_allclose(rotated_vector, q1.matrix.dot(vector), atol=1e-5, rtol=0)
@@ -99,7 +99,7 @@ def test_average_easy(self):
         np.testing.assert_allclose(q1.coordinates, avg.coordinates)
 
     def test_average_mild(self):
-        q1 = Quaternion.exp(Quaternion(0, .1, .3, .7))
+        q1 = Quaternion.exp([0, .1, .3, .7])
         quats_l = []
         quats_r = []
         for i in np.arange(-.1, .11, .05):
@@ -133,7 +133,7 @@ def test_average_weights_easy_2(self):
         np.testing.assert_allclose(q1.coordinates, avg.coordinates)
 
     def test_average_weights_mild(self):
-        q1 = Quaternion.exp(Quaternion(0, .1, .3, .7))
+        q1 = Quaternion.exp([0, .1, .3, .7])
         quats_l = []
         quats_r = []
         weights = []
@@ -249,11 +249,11 @@ def test_log_exp(self, qi, qj, qk):
         # ignore numerically unstable quaternions:
         assume(np.linalg.norm([qi, qj, qk]) > Quaternion.tolerance)
         q = Quaternion(0, qi, qj, qk)
-        expq = q.exp()
+        expq = Quaternion.exp(q)
         qback = expq.log()
 
         np.testing.assert_almost_equal(q.coordinates,
-                                       qback.coordinates,
+                                       qback,
                                        decimal=8)
 
     @given(floats(min_value=-5, max_value=5),
@@ -268,12 +268,18 @@ def test_exp_log(self, qr, qi, qj, qk):
             return
 
         logq = q.log()
-        qback = logq.exp()
+        qback = Quaternion.exp(logq)
 
         np.testing.assert_almost_equal(q.coordinates,
                                        qback.coordinates,
                                        decimal=8)
 
+    def test_exp_identity(self):
+        assert Quaternion.Unit() == Quaternion.exp([0, 0, 0, 0])
+
+    def test_log_identity(self):
+        np.testing.assert_almost_equal(Quaternion.Unit().log(), [0, 0, 0, 0])
+
     @given(floats(min_value=-2, max_value=2),
            floats(min_value=-2, max_value=2),
            floats(min_value=-2, max_value=2))
@@ -312,6 +318,8 @@ def test_eq(self, qr, qi, qj, qk):
         assert q * small == q
         assert q * not_small != q
 
+        np.testing.assert_almost_equal(q.distance(q), 0)
+
     @given(floats(min_value=-5, max_value=5),
            floats(min_value=-5, max_value=5),
            floats(min_value=-5, max_value=5),