diff --git a/quaternions/__init__.py b/quaternions/__init__.py index e1f0da1..fd54ad8 100644 --- a/quaternions/__init__.py +++ b/quaternions/__init__.py @@ -1,2 +1,3 @@ -from quaternions.quaternions import Quaternion, QuaternionError # NOQA +from quaternions.quaternion import Quaternion # NOQA +from quaternions.general_quaternion import GeneralQuaternion, QuaternionError # NOQA from quaternions.version import __version__ # NOQA diff --git a/quaternions/general_quaternion.py b/quaternions/general_quaternion.py new file mode 100644 index 0000000..440054d --- /dev/null +++ b/quaternions/general_quaternion.py @@ -0,0 +1,170 @@ +import numpy as np + + +class QuaternionError(Exception): + pass + + +DEFAULT_TOLERANCE = 1e-8 + + +class GeneralQuaternion(object): + """ + represents a quaternion, considered a member in Lie algebra of quaternions. + for backward compatibility shares the same interface with Quaternion. + unlike quaternion, it's not defined up to scale + """ + def __init__(self, qr, qi, qj, qk): + self.qr = qr + self.qi = qi + self.qj = qj + self.qk = qk + + @property + def real(self): + return self.qr + + @property + def imaginary(self): + return np.array([self.qi, self.qj, self.qk]) + + def __add__(self, p): + validate_is_quaternion(p) + return GeneralQuaternion(self.qr + p.qr, self.qi + p.qi, self.qj + p.qj, self.qk + p.qk) + + def __sub__(self, p): + validate_is_quaternion(p) + return GeneralQuaternion(self.qr - p.qr, self.qi - p.qi, self.qj - p.qj, self.qk - p.qk) + + def __neg__(self): + return self.__class__(-self.qr, -self.qi, -self.qj, -self.qk) + + def __mul__(self, p): + if isinstance(p, GeneralQuaternion): + mat = np.array([ + [self.qr, -self.qi, -self.qj, -self.qk], # noqa + [self.qi, self.qr, self.qk, -self.qj], # noqa + [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)) + return self.__class__(*result) + else: + return self.__class__(self.qr * p, self.qi * p, self.qj * p, self.qk * p) + + def __rmul__(self, p): + return self.__mul__(p) + + def __truediv__(self, p): + return self * (1 / p) + + def __rtruediv__(self, p): + return p * self.inverse() + + def conjugate(self): + return self.__class__(self.qr, -self.qi, -self.qj, -self.qk) + + def inverse(self): + return self.conjugate() * (1 / self._squarenorm()) + + def __invert__(self): + return self.inverse() + + def _squarenorm(self): + return self.qr * self.qr + self.qi * self.qi + self.qj * self.qj + self.qk * self.qk + + def __repr__(self): + return 'GeneralQuaternion{}'.format(tuple(self.coordinates)) + + def __str__(self): + return '({qr:.6g}{qi:+.6g}i{qj:+.6g}j{qk:+.6g}k)'.format(**self.__dict__) + + def is_real(self, tolerance=DEFAULT_TOLERANCE): + """ True if i, j, k components are zero. """ + complex_norm = np.linalg.norm([self.qi, self.qj, self.qk]) + return complex_norm < tolerance + + def is_equal(self, other, tolerance=DEFAULT_TOLERANCE): + """ + compares as quaternions up to tolerance. + Note: tolerance in coords, not in quaternions metrics. + Note: unlike quaternions, equality is not up to scale. + """ + return np.linalg.norm(self.coordinates - other.coordinates) < tolerance + + def __eq__(self, other): + return self.is_equal(other) + + def norm(self): + return np.sqrt(self._squarenorm()) + + def normalized(self): + return self / self.norm() + + def euclidean_distance(self, other): + """ Returns the euclidean distance between two quaternions. Note: differs from unitary quaternions distance. """ + return (self - other).norm() + + def is_unitary(self, tolerance=DEFAULT_TOLERANCE): + return abs(self.norm() - 1) < tolerance + + @property + def coordinates(self): + return np.array([self.qr, self.qi, self.qj, self.qk]) + + @classmethod + def unit(cls): + return cls(1, 0, 0, 0) + + @classmethod + def zero(cls): + return GeneralQuaternion(0, 0, 0, 0) + + def exp(self): + return exp(self) + + def log(self): + return log(self) + + +def validate_is_quaternion(q): + if not isinstance(q, GeneralQuaternion): + raise QuaternionError('expected quaternion, got %s' % q.__class__.__name__) + + +def exp(q): + """ + exponent quaternion + :param q: GeneralQuaternion (or any derived) + :return: same class as q (GeneralQuaternion or any derived) + """ + validate_is_quaternion(q) + cls = type(q) + + exp_norm = np.exp(q.real) + + imag_norm = np.linalg.norm(q.imaginary) + if imag_norm == 0: + i, j, k = 0, 0, 0 + else: + i, j, k = np.sin(imag_norm) * q.imaginary / imag_norm + q_exp = GeneralQuaternion(*(exp_norm * np.array([np.cos(imag_norm), i, j, k]))) + return cls(*q_exp.coordinates) # to enable derived classes + + +def log(q): + """ + logarithm of quaternion + :param q: GeneralQuaternion (or any derived) + :return: GeneralQuaternion + """ + validate_is_quaternion(q) + + norm = q.norm() + imag = np.array((q.qi, q.qj, q.qk)) / norm + imag_norm = np.linalg.norm(imag) + if imag_norm == 0: + i, j, k = 0 if q.qr > 0 else np.pi, 0, 0 + else: + i, j, k = imag / imag_norm * np.arctan2(imag_norm, q.qr / norm) + return GeneralQuaternion(np.log(norm), i, j, k) diff --git a/quaternions/quaternions.py b/quaternions/quaternion.py similarity index 62% rename from quaternions/quaternions.py rename to quaternions/quaternion.py index b4f06f5..65109ff 100644 --- a/quaternions/quaternions.py +++ b/quaternions/quaternion.py @@ -1,176 +1,80 @@ import functools import numpy as np from collections import Iterable -from quaternions.utils import (covariance_matrix_from_angles, sigma_lerner, orthogonal_matrix) +import numbers -class QuaternionError(Exception): - pass +from quaternions.utils import (covariance_matrix_from_angles, sigma_lerner, orthogonal_matrix) +from quaternions.general_quaternion import GeneralQuaternion, QuaternionError, DEFAULT_TOLERANCE, exp -class Quaternion(object): - ''' A class that holds quaternions. It actually holds Q^op, as +class Quaternion(GeneralQuaternion): + ''' A class that holds unit quaternions (norm==1, aka versors). It actually holds Q^op, as this is the way Schaub-Jenkins work with them. + Note: Quaternion is equal up to sign. ''' tolerance = 1e-8 - def __init__(self, qr, qi, qj, qk, validate_numeric_stability=True): - self.qr = qr - self.qi = qi - self.qj = qj - self.qk = qk - - if validate_numeric_stability: - if self._squarenorm() < self.tolerance * self.tolerance: - raise QuaternionError('provided numerically unstable quaternion: %s' % self) + def __init__(self, qr, qi, qj, qk): + q = [qr, qi, qj, qk] + norm = np.linalg.norm(q) + if norm < DEFAULT_TOLERANCE: + raise QuaternionError('provided numerically unstable quaternion: %s' % q) - def __add__(self, p): - assert isinstance(p, Quaternion) - return Quaternion(self.qr + p.qr, self.qi + p.qi, self.qj + p.qj, self.qk + p.qk) - - def __sub__(self, p): - assert isinstance(p, Quaternion) - return Quaternion(self.qr - p.qr, self.qi - p.qi, self.qj - p.qj, self.qk - p.qk) - - def __neg__(self): - return Quaternion(-self.qr, -self.qi, -self.qj, -self.qk) + super().__init__(*q / norm) def __mul__(self, p): - if isinstance(p, Quaternion): - mat = np.array([ - [self.qr, -self.qi, -self.qj, -self.qk], # noqa - [self.qi, self.qr, self.qk, -self.qj], # noqa - [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)) - return Quaternion(*result) - elif isinstance(p, Iterable): + if isinstance(p, Quaternion) or isinstance(p, numbers.Number): + mul = GeneralQuaternion(*self.coordinates) * p + return Quaternion(*mul.coordinates) + elif isinstance(p, GeneralQuaternion): + return GeneralQuaternion(*self.coordinates) * p + elif isinstance(p, Iterable) and len(p) == 3: # applies quaternion rotation on vector return self.matrix.dot(p) else: - return Quaternion(self.qr * p, self.qi * p, self.qj * p, self.qk * p) - - def __rmul__(self, p): - return self.__mul__(p) - - def __truediv__(self, p): - return self * (1 / p) - - def __rtruediv__(self, p): - return p * self.inverse() - - def conjugate(self): - return Quaternion(self.qr, -self.qi, -self.qj, -self.qk) - - def inverse(self): - return self.conjugate() * (1 / self._squarenorm()) - - def __invert__(self): - return self.inverse() - - def normalized(self): - return self / np.sqrt(self._squarenorm()) + raise QuaternionError('cant multiply by %s' % type(p)) - def _squarenorm(self): - return self.qr * self.qr + self.qi * self.qi + self.qj * self.qj + self.qk * self.qk - - def __repr__(self): - return 'Quaternion{}'.format(self.coordinates) - - def __str__(self): - return '({qr:.6g}{qi:+.6g}i{qj:+.6g}j{qk:+.6g}k)'.format(**self.__dict__) - - def is_equal(self, other): + def __call__(self, p): """ - compares as quaternions up to tolerance. - Note: tolerance in coords, not in quaternions metrics. + applies quaternion on vector + :param p: array of 3 elements + :return: ndarray of 3 elements """ - q1 = np.asarray(self.normalized().coordinates) - q2 = np.asarray(other.normalized().coordinates) - dist = min(np.linalg.norm(q1 - q2), np.linalg.norm(q1 - (-q2))) - return dist < self.tolerance + try: + return self * p + except Exception as e: + raise QuaternionError('expected list of 3 elements, got %s, caused error: %s' % (p.__class__.__name__, e)) + + def is_equal(self, other, tolerance=DEFAULT_TOLERANCE): + """ compares as quaternions, i.e. up to sign. """ + return super().is_equal(other, tolerance) or super().is_equal(-other, tolerance) def __eq__(self, other): return self.is_equal(other) - def norm(self): - return np.sqrt(self._squarenorm()) - - def exp(self): - exp_norm = np.exp(self.qr) - - 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) - - return exp_norm * q - - def log(self): - 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) - def distance(self, other): - '''Returns the distance in radians between two unitary quaternions''' - quot = (self * other.conjugate()).positive_representant - return 2 * quot.log().norm() - - def is_unitary(self): - return abs(self._squarenorm() - 1) < self.tolerance - - @property - def coordinates(self): - return self.qr, self.qi, self.qj, self.qk + """ Returns the distance [rad] between two unitary quaternions. """ + diff = self * ~other + return diff.rotation_angle() @property def positive_representant(self): - '''Unitary quaternions q and -q correspond to the same element in SO(3). + """ + Unitary quaternions q and -q correspond to the same element in SO(3). In order to perform some computations (v.g., distance), it is important to fix one of them. Though the following computations can be done for any quaternion, we allow them only for unitary ones. - ''' - - assert self.is_unitary(), 'This method makes sense for unitary quaternions' - + """ for coord in self.coordinates: if coord > 0: return self if coord < 0: return -self - # add a return here if you remove the assert - - @property - def basis(self): - qr, qi, qj, qk = self.coordinates - b0 = np.array([ - qr ** 2 + qi ** 2 - qj ** 2 - qk ** 2, - 2 * qr * qk + 2 * qi * qj, - -2 * qr * qj + 2 * qi * qk - ]) - b1 = np.array([ - -2 * qr * qk + 2 * qi * qj, - qr ** 2 - qi ** 2 + qj ** 2 - qk ** 2, - 2 * qr * qi + 2 * qj * qk - ]) - b2 = np.array([ - 2 * qr * qj + 2 * qi * qk, - -2 * qr * qi + 2 * qj * qk, - qr ** 2 - qi ** 2 - qj ** 2 + qk ** 2 - ]) - return b0, b1, b2 @property def matrix(self): + """ returns 3x3 rotation matrix representing the same rotation. """ qr, qi, qj, qk = self.coordinates return np.array([ [qr * qr + qi * qi - qj * qj - qk * qk, @@ -184,13 +88,55 @@ def matrix(self): qr * qr - qi * qi - qj * qj + qk * qk] ]) + @classmethod + def _first_eigenvector(cls, matrix): + """ matrix must be a 4x4 symmetric matrix. """ + vals, vecs = np.linalg.eigh(matrix) + # q is the eigenvec with heighest eigenvalue (already normalized) + q = vecs[:, -1] + if q[0] < 0: + q = -q + return cls(*q) + + @classmethod + def average(cls, *quaternions, weights=None): + """ + Return the quaternion such that its matrix minimizes the square distance + to the matrices of the quaternions in the argument list. + + See Averaging Quaternions, by Markley, Cheng, Crassidis, Oschman. + """ + b = np.array([q.coordinates for q in quaternions]) + if weights is None: + weights = np.ones(len(quaternions)) + m = b.T.dot(np.diag(weights)).dot(b) + + return cls._first_eigenvector(m) + + @property + def basis(self): + m = self.matrix + return m[0, :], m[1, :], m[2, :] + @property def rotation_vector(self): + """ returns [x, y, z]: direction is rotation axis, norm is angle [rad] """ return (2 * self.log()).coordinates[1:] + def rotation_axis(self): + """ returns unit rotation axis: [x, y, z] """ + v = self.rotation_vector + return v / np.linalg.norm(v) + + def rotation_angle(self): + """ returns rotation angle [rad], in [0, 2 * pi) """ + angle = np.linalg.norm(self.rotation_vector) + angle %= 2 * np.math.pi + return angle + @property def ra_dec_roll(self): - '''Returns ra, dec, roll for quaternion. + '''Returns ra, dec, roll for quaternion [deg]. The Euler angles are those called Tait-Bryan XYZ, as defined in https://en.wikipedia.org/wiki/Euler_angles#Tait-Bryan_angles ''' @@ -202,7 +148,7 @@ def ra_dec_roll(self): @property def astrometry_ra_dec_roll(self): - '''Returns ra, dec, roll as reported by astrometry. + '''Returns ra, dec, roll as reported by astrometry [deg]. Notice that Tetra gives a different roll angle, so this is not a fixed standard. ''' @@ -212,9 +158,7 @@ def astrometry_ra_dec_roll(self): @staticmethod def from_matrix(mat): - ''' - Returns the quaternion corresponding to the unitary matrix mat - ''' + """ Returns the quaternion corresponding to the unitary matrix mat. """ mat = np.array(mat) tr = np.trace(mat) d = 1 + 2 * mat.diagonal() - tr @@ -234,30 +178,21 @@ def from_matrix(mat): @staticmethod def from_rotation_vector(xyz): - ''' + """ Returns the quaternion corresponding to the rotation xyz. Explicitly: rotation occurs along the axis xyz and has angle norm(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() - - @staticmethod - def _first_eigenvector(matrix): - '''matrix must be a 4x4 symmetric matrix''' - vals, vecs = np.linalg.eigh(matrix) - # q is the eigenvec with heighest eigenvalue (already normalized) - q = vecs[:, -1] - if q[0] < 0: - q = -q - return Quaternion(*q) + """ + a, b, c = .5 * np.array(xyz) + q_exp = exp(GeneralQuaternion(0, a, b, c)) + return Quaternion(*q_exp.coordinates) @staticmethod def from_qmethod(source, target, probabilities=None): - ''' + """ Returns the quaternion corresponding to solving with qmethod. See: Closed-form solution of absolute orientation using unit quaternions, @@ -277,7 +212,7 @@ def from_qmethod(source, target, probabilities=None): If s and t are the 3xn matrices of v1,..., vn in frames F1 and F2, then Quaternion.from_qmethod(s, t) is the quaternion corresponding to the change of basis from F1 to F2. - ''' + """ if probabilities is not None: B = source.dot(np.diag(probabilities)).dot(target.T) else: @@ -291,22 +226,6 @@ def from_qmethod(source, target, probabilities=None): K[1:4, 0] = [i, j, k] K[1:4, 1:4] = S - sigma * np.identity(3) return Quaternion._first_eigenvector(K) - - @staticmethod - def average(*quaternions, weights=None): - ''' - Return the quaternion such that its matrix minimizes the square distance - to the matrices of the quaternions in the argument list. - - See Averaging Quaternions, by Markley, Cheng, Crassidis, Oschman. - ''' - B = np.array([q.coordinates for q in quaternions]) - if weights is None: - weights = np.ones(len(quaternions)) - M = B.T.dot(np.diag(weights)).dot(B) - - return Quaternion._first_eigenvector(M) - @staticmethod def average_and_std_naive(*quaternions, weights=None): """ @@ -361,10 +280,6 @@ def average_and_covariance(*quaternions, R=np.eye(3)): return q_average, cov_dev_matrix - @staticmethod - def Unit(): - return Quaternion(1, 0, 0, 0) - @staticmethod def integrate_from_velocity_vectors(vectors): '''vectors must be an iterable of 3-d vectors. @@ -373,7 +288,7 @@ def integrate_from_velocity_vectors(vectors): under all rotations in the iterable. ''' qs = list(map(Quaternion.from_rotation_vector, vectors))[::-1] - prod = functools.reduce(Quaternion.__mul__, qs, Quaternion.Unit()) + prod = functools.reduce(Quaternion.__mul__, qs, Quaternion.unit()) return prod.rotation_vector @staticmethod @@ -385,13 +300,11 @@ 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)) - return rollq * decq * raq + raq = exp(GeneralQuaternion(0, 0, 0, -np.deg2rad(ra) / 2)) + decq = exp(GeneralQuaternion(0, 0, -np.deg2rad(dec) / 2, 0)) + rollq = exp(GeneralQuaternion(0, -np.deg2rad(roll) / 2, 0, 0)) + q = rollq * decq * raq + return Quaternion(*q.coordinates) @staticmethod def OpticalAxisFirst(): diff --git a/quaternions/version.py b/quaternions/version.py index 2b720e5..c17b5e8 100644 --- a/quaternions/version.py +++ b/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' diff --git a/setup.py b/setup.py index 0783ff1..180547d 100644 --- a/setup.py +++ b/setup.py @@ -12,7 +12,7 @@ setup( name='satellogic_quaternions', version=version["__version__"], - author='Matias Graña', + author='Matias Graña, Enrique Toomey, Slava Kerner', author_email='matias@satellogic.com', description='This is a library for dealing with quaternions in Python in a unified way.', long_description=open('README.md').read(), diff --git a/tests/test_general_quaternion.py b/tests/test_general_quaternion.py new file mode 100644 index 0000000..ccbbae5 --- /dev/null +++ b/tests/test_general_quaternion.py @@ -0,0 +1,123 @@ +import unittest + +import numpy as np +from hypothesis import given, assume, strategies +import pytest + +from quaternions.general_quaternion import GeneralQuaternion, QuaternionError, DEFAULT_TOLERANCE, exp, log + + +ANY_QUATERNION = strategies.lists(elements=strategies.floats(min_value=-5, max_value=5), min_size=4, max_size=4) + + +class GeneralQuaternionTest(unittest.TestCase): + + @given(ANY_QUATERNION) + def test_equal(self, arr): + q = GeneralQuaternion(*arr) + assert q == q + assert q == q + GeneralQuaternion(0.9 * DEFAULT_TOLERANCE, 0, 0, 0) + assert q != q + GeneralQuaternion(1.1 * DEFAULT_TOLERANCE, 0, 0, 0) + + @given(ANY_QUATERNION) + def test_real_imaginary(self, arr): + q = GeneralQuaternion(*arr) + i, j, k = q.imaginary + assert (q.coordinates == [q.real, i, j, k]).all() + + @given(ANY_QUATERNION) + def test_raises(self, arr): + q = GeneralQuaternion(*arr) + with pytest.raises(QuaternionError): + q + 3 + + @given(ANY_QUATERNION) + def test_arithmetics(self, arr): + q = GeneralQuaternion(*arr) + assert q + q == 2 * q == q * 2 + assert q - q == GeneralQuaternion.zero() + assert q * GeneralQuaternion.zero() == q * 0 == GeneralQuaternion.zero() + assert q * GeneralQuaternion.unit() == q * 1 == q + assert GeneralQuaternion.unit() * q == 1 * q == q + assert -(-q) == q + + @given(ANY_QUATERNION, ANY_QUATERNION) + def test_sum_commutative(self, arr1, arr2): + q1 = GeneralQuaternion(*arr1) + q2 = GeneralQuaternion(*arr2) + assert q1 + q2 == q2 + q1 + assert q1 - q2 == - (q2 - q1) + + @given(ANY_QUATERNION) + def test_conjugate(self, arr): + q = GeneralQuaternion(*arr) + assert (q + q.conjugate()).is_real() + assert (q * q.conjugate()).is_real() + + @given(ANY_QUATERNION) + def test_inverse(self, arr): + q = GeneralQuaternion(*arr) + assume(q.norm() > DEFAULT_TOLERANCE) # ignore quaternions of norm==0, whose inverse is numerically unstable + assert q * q.inverse() == q.inverse() * q == GeneralQuaternion.unit() + assert q * ~q == ~q * q == GeneralQuaternion.unit() + + @given(ANY_QUATERNION) + def test_distance(self, arr): + q = GeneralQuaternion(*arr) + assert q.euclidean_distance(q) == pytest.approx(0) + assert q.norm() == q.euclidean_distance(GeneralQuaternion.zero()) == q.euclidean_distance(2 * q) + + @given(ANY_QUATERNION) + def test_normalized(self, arr): + q = GeneralQuaternion(*arr) + assume(q.norm() > DEFAULT_TOLERANCE) # ignore quaternions of norm==0, whose inverse is numerically unstable + assert q.normalized().norm() == pytest.approx(1, DEFAULT_TOLERANCE) + + @given(ANY_QUATERNION) + def test_is_unitary(self, arr): + q = GeneralQuaternion(*arr) + assume(q.norm() > DEFAULT_TOLERANCE) # ignore quaternions of norm==0, whose inverse is numerically unstable + assert q.normalized().is_unitary() + assert not (2 * q.normalized()).is_unitary() + + @given(ANY_QUATERNION) + def test_coordinates(self, arr): + q = GeneralQuaternion(*arr) + assert q == GeneralQuaternion(*q.coordinates) + + @given(ANY_QUATERNION) + def test_print(self, arr): + """ make sure all coordinates are printed. """ + q = GeneralQuaternion(*arr) + for elem in q.coordinates: + expected_string = '{elem:.6g}'.format(**{'elem': elem}) + assert expected_string in str(q) + + def test_exp_identity(self): + assert exp(GeneralQuaternion.zero()) == GeneralQuaternion.unit() + + def test_log_identity(self): + assert log(GeneralQuaternion.unit()) == GeneralQuaternion.zero() + + @given(ANY_QUATERNION) + def test_exp_norm(self, arr1): + q1 = GeneralQuaternion(*arr1) + assert exp(q1).norm() == pytest.approx(np.exp(q1.qr)) # |exp(q)| == exp(real(q)| + + @given(ANY_QUATERNION) + def test_exp_log(self, arr): + assume(np.linalg.norm(arr) > DEFAULT_TOLERANCE) + q = GeneralQuaternion(*arr).normalized() + assert exp(log(q)) == q + assert log(exp(q)) == GeneralQuaternion(*q.coordinates) + + @given(ANY_QUATERNION) + def test_exp_identical_both_ways(self, arr): + q = GeneralQuaternion(*arr) + assert exp(q) == q.exp() + + @given(ANY_QUATERNION) + def test_log_identical_both_ways(self, arr): + assume(np.linalg.norm(arr) > DEFAULT_TOLERANCE) + q = GeneralQuaternion(*arr) + assert log(q) == q.log() diff --git a/tests/test_quaternion.py b/tests/test_quaternion.py new file mode 100644 index 0000000..04eef23 --- /dev/null +++ b/tests/test_quaternion.py @@ -0,0 +1,304 @@ +import unittest + +from hypothesis import given, assume, strategies +import numpy as np +import pytest +import os +import json + +from quaternions import Quaternion, QuaternionError, GeneralQuaternion +from quaternions.general_quaternion import DEFAULT_TOLERANCE, exp, log + + +ANY_QUATERNION = strategies.lists(elements=strategies.floats(min_value=-5, max_value=5), min_size=4, max_size=4) +ANY_ROTATION_VECTOR = strategies.lists(elements=strategies.floats(min_value=-5, max_value=5), min_size=3, max_size=3) + + +class QuaternionTest(unittest.TestCase): + + schaub_example_dcm = np.array([[.892539, .157379, -.422618], + [-.275451, .932257, -.23457], + [.357073, .325773, .875426]]) + + schaub_result = np.array([.961798, -.14565, .202665, .112505]) + + @given(ANY_QUATERNION) + def test_constructor(self, arr): + assume(GeneralQuaternion(*arr).norm() > DEFAULT_TOLERANCE) + q = Quaternion(*arr) + assert isinstance(q, Quaternion) + assert q.is_unitary() + + def test_constructor_zero_raises(self): + with pytest.raises(QuaternionError): + Quaternion(0, 0, 0, 0) + + @given(ANY_QUATERNION) + def test_is_equal(self, arr): + assume(GeneralQuaternion(*arr).norm() > DEFAULT_TOLERANCE) + q = Quaternion(*arr) + assert q == q + assert q == -q + assert q != q + Quaternion(1, 2, 3, 4) + assert q == GeneralQuaternion(*q.coordinates) + + @given(ANY_QUATERNION, strategies.floats(min_value=0, max_value=2 * np.math.pi-1e-4)) + def test_distance(self, arr, angle_rad): + assume(GeneralQuaternion(*arr).norm() > DEFAULT_TOLERANCE) + q = Quaternion(*arr) + assert q.distance(-q) == pytest.approx(0) or q.distance(-q) == pytest.approx(2 * np.math.pi) + + for diff in [Quaternion.from_ra_dec_roll(np.degrees(angle_rad), 0, 0), + Quaternion.from_ra_dec_roll(0, np.degrees(angle_rad), 0), + Quaternion.from_ra_dec_roll(0, 0, np.degrees(angle_rad))]: + assert q.distance(q * diff) == pytest.approx(angle_rad) + assert q.distance(diff * q) == pytest.approx(angle_rad) + + @given(ANY_QUATERNION) + def test_positive_representant(self, arr): + assume(GeneralQuaternion(*arr).norm() > DEFAULT_TOLERANCE) + q = Quaternion(*arr) + assert q.positive_representant == q or q.positive_representant == -q + assert q.positive_representant == (-q).positive_representant + + @given(ANY_QUATERNION) + def test_mul(self, arr): + assume(GeneralQuaternion(*arr).norm() > DEFAULT_TOLERANCE) + q = Quaternion(*arr) + + # multiply by scalar: + assert isinstance(2 * q, Quaternion) + assert isinstance(q * 2, Quaternion) + assert q == q * 2 == 2 * q # Note: up to scale; differs from GeneralQuaternion() * 2 + + # multiply by Quaternion: + other = Quaternion(1, 2, 3, 4) + assert isinstance(q * other, Quaternion) + assert (q * other).is_unitary() + + # multiply by GeneralQuaternion: + other = GeneralQuaternion(1, 2, 3, 4) + for mul in [other * q, q * other]: + assert isinstance(mul, GeneralQuaternion) and not isinstance(mul, Quaternion) + assert mul.norm() == pytest.approx(other.norm()) + + @given(ANY_QUATERNION) + def test_rotation_vector(self, arr): + assume(np.linalg.norm(np.asarray(arr[1:])) > DEFAULT_TOLERANCE) + q = Quaternion(*arr) + + complex_part = np.array([q.qi, q.qj, q.qk]) + complex_norm = np.linalg.norm(complex_part) + + # test alternative, direct way to calculate rotation axis: + np.testing.assert_allclose(complex_part / complex_norm, q.rotation_axis()) + + # test alternative, direct way to calculate rotation angle: + angle = 2 * np.math.atan2(complex_norm, q.qr) + assert angle == pytest.approx(q.rotation_angle()) + + # test rotation of q^2 is 2*rotation: + assert q*q == Quaternion.from_rotation_vector(2 * np.asarray(q.rotation_vector)) + + @given(ANY_QUATERNION) + def test_from_rotation_vector(self, arr): + assume(np.linalg.norm(np.asarray(arr[1:])) > DEFAULT_TOLERANCE) + q = Quaternion(*arr) + assert q.from_rotation_vector(q.rotation_vector) == q + + def test_rotate_vector_schaub(self): + q1 = exp(Quaternion(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) + + def test_from_matrix_schaub(self): + q = Quaternion.from_matrix(QuaternionTest.schaub_example_dcm) + np.testing.assert_allclose(QuaternionTest.schaub_result, q.coordinates, atol=1e-5, rtol=0) + + @given(ANY_QUATERNION) + def test_matrix(self, arr): + assume(GeneralQuaternion(*arr).norm() > DEFAULT_TOLERANCE) + q = Quaternion(*arr) + m = q.matrix + np.testing.assert_almost_equal(np.identity(3), m.dot(m.T)) + + @given(ANY_QUATERNION) + def test_from_matrix(self, arr): + assume(GeneralQuaternion(*arr).norm() > DEFAULT_TOLERANCE) + q = Quaternion(*arr) + assert q.from_matrix(q.matrix) == q + + @given(ANY_QUATERNION) + def test_basis(self, arr): + assume(GeneralQuaternion(*arr).norm() > DEFAULT_TOLERANCE) + q = Quaternion(*arr) + b1, b2, b3 = q.basis + assert np.array_equal([b1, b2, b3], q.matrix) + + @given(ANY_ROTATION_VECTOR) + def test_from_ra_dec_roll(self, arr): + xyz = np.deg2rad(arr) + c3, c2, c1 = np.cos(xyz) + s3, s2, s1 = np.sin(xyz) + expected = (np.array([ + [c2 * c3, -c2 * s3, s2], # noqa + [c1 * s3 + c3 * s1 * s2, c1 * c3 - s1 * s2 * s3, -c2 * s1], # noqa + [s1 * s3 - c1 * c3 * s2, c3 * s1 + c1 * s2 * s3, c1 * c2] # noqa + ])) + + assert Quaternion.from_ra_dec_roll(*arr) == Quaternion.from_matrix(expected) + + @given(ANY_QUATERNION) + def test_ra_dec_roll(self, arr): + assume(np.linalg.norm(arr) > 3 * DEFAULT_TOLERANCE) + q = Quaternion(*arr) + ra, dec, roll = q.ra_dec_roll + assume(abs(abs(dec) - 90) > 1e-3) # avoid singularity at dec==+_90 + assert Quaternion.from_ra_dec_roll(ra, dec, roll) == q + + def test_qmethod(self): + v1, v2 = [2 / 3, 2 / 3, 1 / 3], [2 / 3, -1 / 3, -2 / 3] + w1, w2 = [0.8, 0.6, 0], [-0.6, 0.8, 0] + q = Quaternion.from_qmethod(np.array([v1, v2]).T, np.array([w1, w2]).T, np.ones(2)) + + np.testing.assert_allclose(q(v1), w1, atol=1e-10) + np.testing.assert_allclose(q(v2), w2, atol=1e-10) + + @given(ANY_ROTATION_VECTOR) + def test_from_qmethod_with_noise(self, r): + assume(np.linalg.norm(r) > Quaternion.tolerance) + q = Quaternion.from_rotation_vector(r) + + vectors = np.random.normal(scale=1.0, size=(3, 6)) + norms = np.linalg.norm(vectors, axis=0) + vectors /= norms + + noise_sigma = 1e-6 + errors = np.random.normal(scale=noise_sigma, size=(3, 6)) + rotated_vectors = q.matrix.dot(vectors) + errors + + q_calculated = Quaternion.from_qmethod(vectors, rotated_vectors, np.ones(6)) + assert q.is_equal(q_calculated, tolerance=10*noise_sigma) + + @given(ANY_ROTATION_VECTOR) + def test_optical_axis_first(self, v): + oaf = Quaternion.OpticalAxisFirst() + np.testing.assert_allclose(oaf(v), [v[2], -v[0], -v[1]]) + + def test_type(self): + # Unit quaternion can be unitary or general: + assert isinstance(GeneralQuaternion.unit(), GeneralQuaternion) + assert isinstance(Quaternion.unit(), Quaternion) + + # Unit quaternion can not be unitary: + assert isinstance(GeneralQuaternion.zero(), GeneralQuaternion) + assert not isinstance(Quaternion.zero(), Quaternion) + assert isinstance(Quaternion.zero(), GeneralQuaternion) + + assert isinstance(exp(GeneralQuaternion(1, 2, 3, 4)), GeneralQuaternion) + assert isinstance(exp(Quaternion(1, 2, 3, 4)), Quaternion) + + assert isinstance(log(Quaternion(1, 2, 3, 4)), GeneralQuaternion) + assert not isinstance(log(Quaternion(1, 2, 3, 4)), Quaternion) + + def test_exp_identity(self): + assert exp(GeneralQuaternion.zero()) == Quaternion.unit() + + def test_log_identity(self): + assert log(Quaternion.unit()) == GeneralQuaternion.zero() + + @given(ANY_QUATERNION) + def test_exp_log(self, arr): + assume(np.linalg.norm(arr) > DEFAULT_TOLERANCE) + q = GeneralQuaternion(*arr).normalized() + assert exp(log(q)) == q + assert (log(exp(q)).imaginary.tolist() == pytest.approx(q.imaginary.tolist())) # log defined up to real + + NUM_ELEMENTS = 25 + + @given(strategies.lists(elements=strategies.floats(min_value=-5, max_value=5), + min_size=4+4*NUM_ELEMENTS, max_size=4+4*NUM_ELEMENTS)) + def test_average(self, arr): + + q = GeneralQuaternion(*arr[:4]) + assume(q.norm() > DEFAULT_TOLERANCE) # ignore quaternions of norm==0, whose inverse is numerically unstable + + q = q.normalized() + randoms = [GeneralQuaternion(*arr[4*i: 4*i+4]) for i in range(1, self.NUM_ELEMENTS+1)] + q_with_noise = [q + n * (.1 * DEFAULT_TOLERANCE) for n in randoms] + + # test without weights: + average = Quaternion.average(*q_with_noise) + assert average == q or average == -q + + # test with weights: + weights = [1] + (self.NUM_ELEMENTS-1) * [0] # only uses q_with_noise[0] + average = Quaternion.average(*q_with_noise, weights=weights) + assert average == q_with_noise[0] or average == -q_with_noise[0] + + def test_apply(self): + q = Quaternion(1, 2, 3, 4) + + v = [1, 2, 3] + assert (q(v) == q * v).all() + assert isinstance(q(v), np.ndarray) + assert len(q(v)) == 3 + + def test_apply_wrong_type(self): + q = Quaternion(1, 2, 3, 4) + with pytest.raises(QuaternionError): + q([1, 2]) + with pytest.raises(QuaternionError): + q({1, 2, 3}) + +class QuaternionStdDevTests(unittest.TestCase): + # tolerance is this big because average_and_std_naive gives slightly different results than matlab implementation + # this may be due to the way weights are taken into account, as in matlab implementation weights were not being used + tolerance_deg = 1e-3 + basedir = os.path.join(os.path.split(os.path.abspath(__file__))[0], 'data') + matlab_basedir = os.path.join(basedir, 'matlab_results') + + @classmethod + def setUpClass(cls): + with open(os.path.join(cls.matlab_basedir, 'matlab_results.json'), 'r') as fid: + cls.results = json.load(fid) + + cls.quat_diff_matlab_quats = {} + for star_vectors_noise_arcsec in cls.results.keys(): + quat_diff_matlab = np.loadtxt( + os.path.join(cls.matlab_basedir, 'quat_diff_ST1_ST2_{}_arcsec.txt'.format(star_vectors_noise_arcsec))) + cls.quat_diff_matlab_quats[star_vectors_noise_arcsec] = [Quaternion(*q) for q in quat_diff_matlab] + + def test_average_std_naive(self): + + for star_vectors_noise_arcsec in self.results.keys(): + quat_diff_matlab_quats = self.quat_diff_matlab_quats[star_vectors_noise_arcsec] + _, mean_total_rotation_angle_deg = Quaternion.average_and_std_naive(*quat_diff_matlab_quats) + + assert abs(mean_total_rotation_angle_deg - self.results[star_vectors_noise_arcsec]['mean_total_angle'])\ + < self.tolerance_deg + + def test_average_std_sigma_lerner(self): + for star_vectors_noise_arcsec in self.results.keys(): + quat_diff_matlab_quats = self.quat_diff_matlab_quats[star_vectors_noise_arcsec] + _, sigma_lerner_deg = Quaternion.average_and_std_lerner(*quat_diff_matlab_quats) + + assert abs(sigma_lerner_deg - self.results[star_vectors_noise_arcsec]['sigma_lerner']) \ + < self.tolerance_deg + + def test_average_and_covariance(self): + # This tests that the trace of the resultant covariance matrix of the + # averaged test is around the same value than the input covariance matrix + # if an individual quaternion divided np.sqrt(N-1), where N is the number of + # quaternions + for star_vectors_noise_arcsec in self.results.keys(): + quat_diff_matlab_quats = self.quat_diff_matlab_quats[star_vectors_noise_arcsec] + sigma_lerner_in_deg = self.results[star_vectors_noise_arcsec]['sigma_lerner'] + _, covariance_rad = Quaternion.average_and_covariance( + *quat_diff_matlab_quats, R=np.deg2rad(sigma_lerner_in_deg)**2*np.eye(3)) + sigma_lerner_out_deg = np.rad2deg(np.sqrt(np.trace(covariance_rad)/3))\ + * np.sqrt(len(quat_diff_matlab_quats)-1) + + assert abs(sigma_lerner_in_deg - sigma_lerner_out_deg ) \ + < self.tolerance_deg \ No newline at end of file diff --git a/tests/test_quaternions.py b/tests/test_quaternions.py deleted file mode 100644 index 1edfb9f..0000000 --- a/tests/test_quaternions.py +++ /dev/null @@ -1,380 +0,0 @@ -import unittest -from hypothesis import given, assume -from hypothesis.strategies import floats -import numpy as np -import json -import os - -from quaternions import Quaternion, QuaternionError - -basedir = os.path.join(os.path.split(os.path.abspath(__file__))[0], 'data') - -class QuaternionTest(unittest.TestCase): - # Schaub, Chapter 3 - schaub_example_dcm = np.array([[.892539, .157379, -.422618], - [-.275451, .932257, -.23457], - [.357073, .325773, .875426]]) - 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)) - 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)) - 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) - - def test_from_matrix(self): - q = Quaternion.from_matrix(QuaternionTest.schaub_example_dcm) - np.testing.assert_allclose(QuaternionTest.schaub_result, q.coordinates, atol=1e-5, rtol=0) - - def test_from_matrix_twisted(self): - q = Quaternion.from_matrix(QuaternionTest.schaub_example_dcm * [-1, -1, 1]) - e1 = Quaternion(*QuaternionTest.schaub_result) - expected = e1 * Quaternion(0, 0, 0, 1) - np.testing.assert_allclose(expected.coordinates, q.coordinates, atol=1e-5, rtol=0) - - def test_from_rotation_vector_to_matrix(self): - phi = np.array([-.295067, .410571, .227921]) - expected = np.array([ - [.892539, .157379, -.422618], - [-.275451, .932257, -.23457], - [.357073, .325773, .875426]]) - q = Quaternion.from_rotation_vector(phi) - np.testing.assert_allclose(expected, q.matrix, atol=1e-5, rtol=0) - - def test_qmethod(self): - frame_1 = np.array([[2 / 3, 2 / 3, 1 / 3], [2 / 3, -1 / 3, -2 / 3]]) - frame_2 = np.array([[0.8, 0.6, 0], [-0.6, 0.8, 0]]) - q = Quaternion.from_qmethod(frame_1.T, frame_2.T, np.ones(2)) - - for a1 in np.arange(0, 1, .1): - for a2 in np.arange(0, 1, .1): - v1 = a1 * frame_1[0] + a2 * frame_1[1] - v2 = a1 * frame_2[0] + a2 * frame_2[1] - np.testing.assert_allclose(q.matrix.dot(v1), v2, atol=1e-10) - - def test_qmethod_with_probability(self): - frame_1 = np.array([[2 / 3, 2 / 3, 1 / 3], [2 / 3, -1 / 3, -2 / 3]]) - frame_2 = np.array([[0.8, 0.6, 0], [-0.6, 0.8, 0]]) - q = Quaternion.from_qmethod(frame_1.T, frame_2.T, np.ones(2)) - - for a1 in np.arange(0, 1, .1): - for a2 in np.arange(0, 1, .1): - v1 = a1 * frame_1[0] + a2 * frame_1[1] - v2 = a1 * frame_2[0] + a2 * frame_2[1] - np.testing.assert_allclose(q.matrix.dot(v1), v2, atol=1e-10) - - def test_ra_dec_roll(self): - for ra in np.linspace(-170, 180, 8): - for dec in np.linspace(-90, 90, 8): - for roll in np.linspace(10, 360, 8): - - xyz = np.deg2rad(np.array([ra, dec, roll])) - c3, c2, c1 = np.cos(xyz) - s3, s2, s1 = np.sin(xyz) - expected = np.array([ - [c2 * c3, -c2 * s3, s2], # noqa - [c1 * s3 + c3 * s1 * s2, c1 * c3 - s1 * s2 * s3, -c2 * s1], # noqa - [s1 * s3 - c1 * c3 * s2, c3 * s1 + c1 * s2 * s3, c1 * c2] # noqa - ]) - - obtained = Quaternion.from_ra_dec_roll(ra, dec, roll) - - np.testing.assert_allclose(expected, obtained.matrix, atol=1e-15) - - def test_to_rdr(self): - for ra in np.linspace(-170, 170, 8): - for dec in np.linspace(-88, 88, 8): - for roll in np.linspace(-170, 170, 8): - q = Quaternion.from_ra_dec_roll(ra, dec, roll) - - np.testing.assert_allclose([ra, dec, roll], q.ra_dec_roll) - - def test_average_easy(self): - q1 = Quaternion(1, 0, 0, 0) - q2 = Quaternion(-1, 0, 0, 0) - avg = Quaternion.average(q1, q2) - - np.testing.assert_allclose(q1.coordinates, avg.coordinates) - - def test_average_mild(self): - q1 = Quaternion.exp(Quaternion(0, .1, .3, .7)) - quats_l = [] - quats_r = [] - for i in np.arange(-.1, .11, .05): - for j in np.arange(-.1, .11, .05): - for k in np.arange(-.1, .11, .05): - try: - q = Quaternion(0, i, j, k) - except QuaternionError: # wrong quaternion - continue - q = Quaternion.exp(q) - quats_l.append(q1 * q) - quats_r.append(q * q1) - - avg_l = Quaternion.average(*quats_l) - avg_r = Quaternion.average(*quats_r) - np.testing.assert_allclose(q1.coordinates, avg_l.coordinates) - np.testing.assert_allclose(q1.coordinates, avg_r.coordinates) - - def test_average_weights_easy(self): - q1 = Quaternion(1, 0, 0, 0) - q2 = Quaternion(-1, 0, 0, 0) - weights = [1, 1] - avg = Quaternion.average(q1, q2, weights=weights) - np.testing.assert_allclose(q1.coordinates, avg.coordinates) - - def test_average_weights_easy_2(self): - q1 = Quaternion(1, 0, 0, 0) - q2 = Quaternion(0.707, 0, 0.707, 0) - weights = [1, 0] - avg = Quaternion.average(q1, q2, weights=weights) - np.testing.assert_allclose(q1.coordinates, avg.coordinates) - - def test_average_weights_mild(self): - q1 = Quaternion.exp(Quaternion(0, .1, .3, .7)) - quats_l = [] - quats_r = [] - weights = [] - for i in np.arange(-.1, .11, .05): - for j in np.arange(-.1, .11, .05): - for k in np.arange(-.1, .11, .05): - try: - q = Quaternion(0, i, j, k) - except QuaternionError: # wrong quaternion - continue - q = Quaternion.exp(q) - quats_l.append(q1 * q) - quats_r.append(q * q1) - weights.append(1) - - avg_l = Quaternion.average(*quats_l, weights=weights) - avg_r = Quaternion.average(*quats_r, weights=weights) - np.testing.assert_allclose(q1.coordinates, avg_l.coordinates) - np.testing.assert_allclose(q1.coordinates, avg_r.coordinates) - - def test_optical_axis_first(self): - v1 = np.array([.02, .01, .99]) - v2 = np.array([-.01, .02, .99]) - oaf = Quaternion.OpticalAxisFirst() - np.testing.assert_allclose([.99, -.02, -.01], oaf.matrix.dot(v1)) - np.testing.assert_allclose([.99, .01, -.02], oaf.matrix.dot(v2)) - - def test_distance(self): - q = Quaternion.from_rotation_vector([.1, .2, .3]) - - for rot_x in np.linspace(-np.pi, np.pi, 7): - for rot_y in np.linspace(-np.pi / 2, np.pi / 2, 3): - for rot_z in np.linspace(-np.pi / 2, np.pi / 2, 2): - - rotation = [rot_x, rot_y, rot_z] - rot_quat = Quaternion.from_rotation_vector(rotation) - q_rot = q * rot_quat - - expected = np.linalg.norm(rotation) % (2 * np.pi) - if expected > np.pi: - expected = 2 * np.pi - expected - - self.assertAlmostEqual(expected, q.distance(q_rot)) - - -class ParameterizedTests(unittest.TestCase): - - @staticmethod - def ra_dec_to_xyz(ra, dec): - cr, sr = np.cos(np.radians(ra)), np.sin(np.radians(ra)) - cd, sd = np.cos(np.radians(dec)), np.sin(np.radians(dec)) - return np.array([cr * cd, sr * cd, sd]) - - @staticmethod - def angle_to_xy(angle): - return np.cos(np.radians(angle)), np.sin(np.radians(angle)) - - @staticmethod - def from_mrp(xyz): - N = xyz.dot(xyz) - - def inv_proj(x): - return 4 * x / (4 + N) - - qi, qj, qk = map(inv_proj, xyz) - qr = (4 - N) / (4 + N) - return Quaternion(qr, qi, qj, qk) - - @given(floats(min_value=-180, max_value=180), - floats(min_value=-89, max_value=89), # avoid singularities in -90 & 90 degs - floats(min_value=0, max_value=360)) - def test_quat_ra_dec_roll(self, ra, dec, roll): - q = Quaternion.from_ra_dec_roll(ra, dec, roll) - ob_ra, ob_dec, ob_roll = q.ra_dec_roll - np.testing.assert_almost_equal(self.ra_dec_to_xyz(ob_ra, ob_dec), - self.ra_dec_to_xyz(ra, dec)) - np.testing.assert_almost_equal(self.angle_to_xy(ob_roll), - self.angle_to_xy(roll), decimal=2) - - @given(floats(min_value=-2, max_value=2), - floats(min_value=-2, max_value=2), - floats(min_value=-2, max_value=2)) - def test_quat_rotation_vector(self, rx, ry, rz): - # ignore numerically unstable quaternions: - assume(np.linalg.norm([rx, ry, rz]) > Quaternion.tolerance) - rot = np.array([rx, ry, rz]) - q = Quaternion.from_rotation_vector(rot) - distance = np.linalg.norm(rot - q.rotation_vector) - - assert (distance % 2 * np.pi) < 1e-8 - - @given(floats(min_value=-1, max_value=1), - floats(min_value=-1, max_value=1), - floats(min_value=-1, max_value=1)) - def test_matrix(self, ma, mb, mc): - q = self.from_mrp(np.array([ma, mb, mc])) - self.assertTrue(q.is_unitary()) - - m = q.matrix - np.testing.assert_almost_equal(np.identity(3), m.dot(m.T)) - - obtained = Quaternion.from_matrix(m) - self.assertTrue(obtained.is_unitary()) - - np.testing.assert_almost_equal(q.positive_representant.coordinates, - obtained.positive_representant.coordinates, - decimal=8) - - @given(floats(min_value=-1, max_value=1), - floats(min_value=-1, max_value=1), - floats(min_value=-1, max_value=1)) - 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() - qback = expq.log() - - np.testing.assert_almost_equal(q.coordinates, - qback.coordinates, - decimal=8) - - @given(floats(min_value=-5, max_value=5), - floats(min_value=-5, max_value=5), - floats(min_value=-5, max_value=5), - floats(min_value=-5, max_value=5)) - def test_exp_log(self, qr, qi, qj, qk): - # ignore numerically unstable quaternions: - assume(np.linalg.norm([qr, qi, qj, qk]) > Quaternion.tolerance) - q = Quaternion(qr, qi, qj, qk) - if q.norm() == 0: - return - - logq = q.log() - qback = logq.exp() - - np.testing.assert_almost_equal(q.coordinates, - qback.coordinates, - decimal=8) - - @given(floats(min_value=-2, max_value=2), - floats(min_value=-2, max_value=2), - floats(min_value=-2, max_value=2)) - def test_from_qmethod(self, rx, ry, rz): - # ignore numerically unstable quaternions: - assume(np.linalg.norm([rx, ry, rz]) > Quaternion.tolerance) - q = Quaternion.from_rotation_vector(np.array([rx, ry, rz])) - - vectors = np.random.normal(scale=1.0, size=(3, 6)) - norms = np.linalg.norm(vectors, axis=0) - vectors /= norms - - errors = np.random.normal(scale=1e-6, size=(3, 6)) - rotated_vectors = q.matrix.dot(vectors) + errors - - qback = Quaternion.from_qmethod(vectors, rotated_vectors, np.ones(6)) - q_diff = (q / qback).positive_representant - - np.testing.assert_almost_equal(q_diff.coordinates, - Quaternion.Unit().coordinates, - decimal=4) - - @given(floats(min_value=-5, max_value=5), - floats(min_value=-5, max_value=5), - floats(min_value=-5, max_value=5), - floats(min_value=-5, max_value=5)) - def test_eq(self, qr, qi, qj, qk): - # ignore numerically unstable quaternions: - assume(np.linalg.norm([qr, qi, qj, qk]) > Quaternion.tolerance) - q = Quaternion(qr, qi, qj, qk) - - small = Quaternion(1, .5 * Quaternion.tolerance, 0, 0) - not_small = Quaternion(1, 2 * Quaternion.tolerance, 0, 0) - - assert q == q - assert q * small == q - assert q * not_small != q - - @given(floats(min_value=-5, max_value=5), - floats(min_value=-5, max_value=5), - floats(min_value=-5, max_value=5), - floats(min_value=-5, max_value=5)) - def test_invert(self, qr, qi, qj, qk): - """ verify all inverse methods are identical, and indeed they invert. """ - # ignore numerically unstable quaternions: - assume(np.linalg.norm([qr, qi, qj, qk]) > Quaternion.tolerance) - - q = Quaternion(qr, qi, qj, qk) - assert ~q == q.inverse() == q.conjugate() - assert q * ~q == ~q * q == Quaternion.Unit() - - -class QuaternionStdDevTests(unittest.TestCase): - # tolerance is this big because average_and_std_naive gives slightly different results than matlab implementation - # this may be due to the way weights are taken into account, as in matlab implementation weights were not being used - tolerance_deg = 1e-3 - matlab_basedir = os.path.join(basedir, 'matlab_results') - - @classmethod - def setUpClass(cls): - with open(os.path.join(cls.matlab_basedir, 'matlab_results.json'), 'r') as fid: - cls.results = json.load(fid) - - cls.quat_diff_matlab_quats = {} - for star_vectors_noise_arcsec in cls.results.keys(): - quat_diff_matlab = np.loadtxt( - os.path.join(cls.matlab_basedir, 'quat_diff_ST1_ST2_{}_arcsec.txt'.format(star_vectors_noise_arcsec))) - cls.quat_diff_matlab_quats[star_vectors_noise_arcsec] = [Quaternion(*q) for q in quat_diff_matlab] - - def test_average_std_naive(self): - - for star_vectors_noise_arcsec in self.results.keys(): - quat_diff_matlab_quats = self.quat_diff_matlab_quats[star_vectors_noise_arcsec] - _, mean_total_rotation_angle_deg = Quaternion.average_and_std_naive(*quat_diff_matlab_quats) - - assert abs(mean_total_rotation_angle_deg - self.results[star_vectors_noise_arcsec]['mean_total_angle'])\ - < self.tolerance_deg - - def test_average_std_sigma_lerner(self): - for star_vectors_noise_arcsec in self.results.keys(): - quat_diff_matlab_quats = self.quat_diff_matlab_quats[star_vectors_noise_arcsec] - _, sigma_lerner_deg = Quaternion.average_and_std_lerner(*quat_diff_matlab_quats) - - assert abs(sigma_lerner_deg - self.results[star_vectors_noise_arcsec]['sigma_lerner']) \ - < self.tolerance_deg - - def test_average_and_covariance(self): - # This tests that the trace of the resultant covariance matrix of the - # averaged test is around the same value than the input covariance matrix - # if an individual quaternion divided np.sqrt(N-1), where N is the number of - # quaternions - for star_vectors_noise_arcsec in self.results.keys(): - quat_diff_matlab_quats = self.quat_diff_matlab_quats[star_vectors_noise_arcsec] - sigma_lerner_in_deg = self.results[star_vectors_noise_arcsec]['sigma_lerner'] - _, covariance_rad = Quaternion.average_and_covariance( - *quat_diff_matlab_quats, R=np.deg2rad(sigma_lerner_in_deg)**2*np.eye(3)) - sigma_lerner_out_deg = np.rad2deg(np.sqrt(np.trace(covariance_rad)/3))\ - * np.sqrt(len(quat_diff_matlab_quats)-1) - - assert abs(sigma_lerner_in_deg - sigma_lerner_out_deg ) \ - < self.tolerance_deg