diff --git a/Quaternion/Quaternion.py b/Quaternion/Quaternion.py index af09bd9..2e9079a 100644 --- a/Quaternion/Quaternion.py +++ b/Quaternion/Quaternion.py @@ -7,7 +7,9 @@ - class methods to multiply and divide quaternions :Copyright: Smithsonian Astrophysical Observatory (2010) -:Author: Jean Connelly (jconnelly@cfa.harvard.edu) +:Authors: - Tom Aldcroft (aldcroft@cfa.harvard.edu) + - Jean Connelly (jconnelly@cfa.harvard.edu) + - Javier Gonzalez (javier.gonzalez@cfa.harvard.edu) """ # Copyright (c) 2010, Smithsonian Astrophysical Observatory # All rights reserved. @@ -35,7 +37,6 @@ # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. import numpy as np -from math import cos, sin, radians, degrees, atan2, sqrt class Quat(object): @@ -62,7 +63,7 @@ class Quat(object): applying the q2 transform followed by the q1 transform. Another way to express this is:: - q = Quat(numpy.dot(q1.transform, q2.transform)) + q = Quat(q1.transform @ q2.transform) Example usage:: @@ -102,39 +103,116 @@ class Quat(object): * a 3 element array (expects ra,dec,roll in degrees) * a 3x3 transform/rotation matrix + :param q: attitude as a quaternion. + + ``q`` must be an array with shape (4,) or (N, 4), with arbitrary N. + The last axis corresponds to quaternion coordinates x, y, z, w. + For example: (3, 2, 4) corresponds to a (3, 2) array of quaternions. + + :param equatorial: attitude in equatorial coordinates. + + ``q`` must be an array with shape (3,) or (N, 3), with arbitrary N. + The last axis corresponds to equatorial coordinates ra, dec, roll. + For example: (3, 2, 3) corresponds to a (3, 2) array of quaternions. + + :param transform: attitude as a 3x3 transform. + + ``q`` must be an array with shape (3, 3) or (N, 3, 3), with arbitrary N. + The last two axes correspond to (3, 3) transformations. + For example: (3, 2, 3, 3) corresponds to a (3, 2) array of quaternions. """ - def __init__(self, attitude): + def __init__(self, attitude=None, transform=None, q=None, equatorial=None): + npar = (int(attitude is not None) + int(transform is not None) + + int(q is not None) + int(equatorial is not None)) + if npar != 1: + raise ValueError( + f'{npar} arguments passed to constructor that takes only one of' + ' attitude, transform, quaternion, equatorial.' + ) self._q = None self._equatorial = None self._T = None + + # other data members that are set lazily. + self._ra0 = None + self._roll0 = None + # checks to see if we've been passed a Quat if isinstance(attitude, Quat): - self._set_q(attitude.q) - else: - # make it an array and check to see if it is a supported shape - attitude = np.array(attitude) - if len(attitude) == 4: - self._set_q(attitude) + q = attitude.q + elif attitude is not None: + # check to see if it is a supported shape + attitude = np.array(attitude, dtype=float) + if attitude.shape == (4,): + q = attitude elif attitude.shape == (3, 3): - self._set_transform(attitude) + transform = attitude elif attitude.shape == (3,): - self._set_equatorial(attitude) + equatorial = attitude else: + try: + shape = attitude.shape + shape = f' (shape {shape})' + except Exception as e: + shape = '' raise TypeError( - "attitude is not one of possible types (3 or 4 elements, Quat, or 3x3 matrix)") + f"attitude argument{shape} is not one of an allowed type:" + " Quat or array with shape (...,3), (...,4), or (..., 3, 3)") + + # checking correct shapes + if q is not None: + q = np.atleast_1d(q) + self._shape = q.shape[:-1] + if q.shape[-1:] != (4,): + raise TypeError("Creating a Quaternion from quaternion(s) " + "requires shape (..., 4), not {}".format(q.shape)) + self._set_q(q) + elif transform is not None: + transform = np.atleast_2d(transform) + self._shape = transform.shape[:-2] + if transform.shape[-2:] != (3, 3): + raise TypeError("Creating a Quaternion from quaternion(s) " + "requires shape (..., 3, 3), not {}".format(transform.shape)) + self._set_transform(transform) + elif equatorial is not None: + equatorial = np.atleast_1d(equatorial) + self._shape = equatorial.shape[:-1] + if equatorial.shape[-1:] != (3,): + raise TypeError("Creating a Quaternion from ra, dec, roll " + "requires shape (..., 3), not {}".format(equatorial.shape)) + self._set_equatorial(equatorial) + assert self._shape is not None + + @property + def shape(self): + """ + The shape of the multi-quaternion. + + For example, if the Quat is: + - a single quaternion, then its shape is (). + - a multi-quaternion with self.q.shape = (N, 4), then its shape is (N,) + + :returns: self.q.shape[:-1] + :rtype: tuple + """ + return self._shape def _set_q(self, q): """ Set the value of the 4 element quaternion vector + May be 4 element list or array or N x 4 element array, + where N can be an arbitrary shape. E.g.: (3,2,4) is allowed. :param q: list or array of normalized quaternion elements """ - q = np.array(q) - if abs(np.sum(q**2) - 1.0) > 1e-6: + q = np.atleast_2d(np.array(q)) + if np.any((np.sum(q ** 2, axis=-1, keepdims=True) - 1.0) > 1e-6): raise ValueError( - 'Quaternion must be normalized so sum(q**2) == 1; use Quaternion.normalize') - self._q = (q if q[3] > 0 else -q) + 'Quaternions must be normalized so sum(q**2) == 1; use Quaternion.normalize') + self._q = q + flip_q = q[..., 3] < 0 + self._q[flip_q] = -1 * q[flip_q] # Erase internal values of other representations self._equatorial = None self._T = None @@ -142,6 +220,7 @@ def _set_q(self, q): def _get_q(self): """ Retrieve 4-vector of quaternion elements in [x, y, z, w] form + or N x 4-vector if N > 1. :rtype: numpy array @@ -152,15 +231,17 @@ def _get_q(self): self._q = self._equatorial2quat() elif self._T is not None: self._q = self._transform2quat() - return self._q + return self._q.reshape(self.shape+(4,)) # use property to make this get/set automatic q = property(_get_q, _set_q) def __repr__(self): q = self.q - return ('<{} q1={:.8f} q2={:.8f} q3={:.8f} q4={:.8f}>' - .format(self.__class__.__name__, q[0], q[1], q[2], q[3])) + if q.ndim == 1: + return ('<{} q1={:.8f} q2={:.8f} q3={:.8f} q4={:.8f}>' + .format(self.__class__.__name__, q[0], q[1], q[2], q[3])) + return '{}({})'.format(self.__class__.__name__, repr(q)) def _set_equatorial(self, equatorial): """Set the value of the 3 element equatorial coordinate list [RA,Dec,Roll] @@ -170,7 +251,7 @@ def _set_equatorial(self, equatorial): :param equatorial: list or array [ RA, Dec, Roll] in degrees """ - self._equatorial = np.array(equatorial) + self._equatorial = np.atleast_2d(np.array(equatorial)) def _get_equatorial(self): """Retrieve [RA, Dec, Roll] @@ -183,34 +264,34 @@ def _get_equatorial(self): elif self._T is not None: self._q = self._transform2quat() self._equatorial = self._quat2equatorial() - return self._equatorial + return self._equatorial.reshape(self.shape+(3,)) equatorial = property(_get_equatorial, _set_equatorial) def _get_ra(self): """Retrieve RA term from equatorial system in degrees""" - return self.equatorial[0] + return self.equatorial[..., 0].reshape(self.shape) def _get_dec(self): """Retrieve Dec term from equatorial system in degrees""" - return self.equatorial[1] + return self.equatorial[..., 1].reshape(self.shape) def _get_roll(self): """Retrieve Roll term from equatorial system in degrees""" - return self.equatorial[2] + return self.equatorial[..., 2].reshape(self.shape) ra = property(_get_ra) dec = property(_get_dec) roll = property(_get_roll) - def _get_zero(self, attr): + @staticmethod + def _get_zero(val): """ - Return a version of attr that is between -180 <= val < 180 + Return a version of val that is between -180 <= val < 180 """ - if not hasattr(self, '_' + attr): - val = getattr(self, attr) % 360.0 - if val >= 180: - val -= 360 + val = np.atleast_1d(val) + val = val % 360 + val[val >= 180] -= 360 return val @property @@ -218,8 +299,8 @@ def ra0(self): """ Return quaternion RA in the range -180 <= roll < 180. """ - if not hasattr(self, '_ra0'): - self._ra0 = self._get_zero('ra') + if self._ra0 is None: + self._ra0 = self._get_zero(self.ra) return self._ra0 @property @@ -227,8 +308,8 @@ def roll0(self): """ Return quaternion roll in the range -180 <= roll < 180. """ - if not hasattr(self, '_roll0'): - self._roll0 = self._get_zero('roll') + if self._roll0 is None: + self._roll0 = self._get_zero(self.roll) return self._roll0 @property @@ -245,13 +326,15 @@ def yaw(self): """ return self.ra0 - def _set_transform(self, T): + def _set_transform(self, t): """ Set the value of the 3x3 rotation/transform matrix - :param T: 3x3 array/numpy array + :param t: 3x3 array/numpy array """ - transform = np.array(T) + transform = np.array(t) + if transform.ndim == 2: + transform = transform[np.newaxis] self._T = transform def _get_transform(self): @@ -267,116 +350,119 @@ def _get_transform(self): self._T = self._quat2transform() elif self._equatorial is not None: self._T = self._equatorial2transform() - return self._T + return self._T.reshape(self.shape+(3, 3)) transform = property(_get_transform, _set_transform) def _quat2equatorial(self): """ - Determine Right Ascension, Declination, and Roll for the object quaternion + Determine Right Ascension, Declination, and Roll for the quaternion - :returns: RA, Dec, Roll + :returns: N x (RA, Dec, Roll) :rtype: numpy array [ra,dec,roll] """ - q = self.q - q2 = self.q**2 + q = np.atleast_2d(self.q) + q2 = q ** 2 # calculate direction cosine matrix elements from $quaternions - xa = q2[0] - q2[1] - q2[2] + q2[3] - xb = 2 * (q[0] * q[1] + q[2] * q[3]) - xn = 2 * (q[0] * q[2] - q[1] * q[3]) - yn = 2 * (q[1] * q[2] + q[0] * q[3]) - zn = q2[3] + q2[2] - q2[0] - q2[1] + xa = q2[..., 0] - q2[..., 1] - q2[..., 2] + q2[..., 3] + xb = 2 * (q[..., 0] * q[..., 1] + q[..., 2] * q[..., 3]) + xn = 2 * (q[..., 0] * q[..., 2] - q[..., 1] * q[..., 3]) + yn = 2 * (q[..., 1] * q[..., 2] + q[..., 0] * q[..., 3]) + zn = q2[..., 3] + q2[..., 2] - q2[..., 0] - q2[..., 1] # Due to numerical precision this can go negative. Allow *slightly* negative # values but raise an exception otherwise. one_minus_xn2 = 1 - xn**2 - if one_minus_xn2 < 0: - if one_minus_xn2 < -1e-12: + if np.any(one_minus_xn2 < 0): + if np.any(one_minus_xn2 < -1e-12): raise ValueError('Unexpected negative norm: {}'.format(one_minus_xn2)) - one_minus_xn2 = 0 + one_minus_xn2[one_minus_xn2 < 0] = 0 # ; calculate RA, Dec, Roll from cosine matrix elements - ra = degrees(atan2(xb, xa)) - dec = degrees(atan2(xn, sqrt(one_minus_xn2))) - roll = degrees(atan2(yn, zn)) - if (ra < 0): - ra += 360 - if (roll < 0): - roll += 360 - - return np.array([ra, dec, roll]) + ra = np.degrees(np.arctan2(xb, xa)) + dec = np.degrees(np.arctan2(xn, np.sqrt(one_minus_xn2))) + roll = np.degrees(np.arctan2(yn, zn)) + # all negative angles are incremented by 360, + # the output is in the (0,360) interval instead of in (-180, 180) + ra[ra < 0] = ra[ra < 0] + 360 + roll[roll < 0] = roll[roll < 0] + 360 + # moveaxis in the following line is a "transpose" + # from shape (3, N) to (N, 3), where N can be an arbitrary tuple + # e.g. (3, 2, 5) -> (2, 5, 3) (np.transpose would give (2, 3, 5)) + return np.moveaxis(np.array([ra, dec, roll]), 0, -1) # _quat2transform is largely from Enthought's quaternion.rotmat, though this math is # probably from Hamilton. # License included for completeness # -#This software is OSI Certified Open Source Software. -#OSI Certified is a certification mark of the Open Source Initiative. -# -#Copyright (c) 2006, Enthought, Inc. -#All rights reserved. +# This software is OSI Certified Open Source Software. +# OSI Certified is a certification mark of the Open Source Initiative. # -#Redistribution and use in source and binary forms, with or without -#modification, are permitted provided that the following conditions are met: +# Copyright (c) 2006, Enthought, Inc. +# All rights reserved. # -# * Redistributions of source code must retain the above copyright notice, this -# list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright notice, -# this list of conditions and the following disclaimer in the documentation -# and/or other materials provided with the distribution. -# * Neither the name of Enthought, Inc. nor the names of its contributors may -# be used to endorse or promote products derived from this software without -# specific prior written permission. +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: # -#THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -#ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -#WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -#DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR -#ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -#(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -#LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON -#ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -#(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -#SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +# * Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Enthought, Inc. nor the names of its contributors may +# be used to endorse or promote products derived from this software without +# specific prior written permission. # +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +# ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +# ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + def _quat2transform(self): """ - Transform a unit quaternion into its corresponding rotation matrix (to - be applied on the right side). + Transform a unit quaternion into its corresponding rotation/transform matrix. - :returns: transform matrix + :returns: Nx3x3 transform matrix :rtype: numpy array """ - x, y, z, w = self.q - xx2 = 2 * x * x - yy2 = 2 * y * y - zz2 = 2 * z * z - xy2 = 2 * x * y - wz2 = 2 * w * z - zx2 = 2 * z * x - wy2 = 2 * w * y - yz2 = 2 * y * z - wx2 = 2 * w * x - - rmat = np.empty((3, 3), float) - rmat[0, 0] = 1. - yy2 - zz2 - rmat[0, 1] = xy2 - wz2 - rmat[0, 2] = zx2 + wy2 - rmat[1, 0] = xy2 + wz2 - rmat[1, 1] = 1. - xx2 - zz2 - rmat[1, 2] = yz2 - wx2 - rmat[2, 0] = zx2 - wy2 - rmat[2, 1] = yz2 + wx2 - rmat[2, 2] = 1. - xx2 - yy2 - - return rmat + q = np.atleast_2d(self.q) + + x, y, z, w = q[..., 0], q[..., 1], q[..., 2], q[..., 3] + xx2 = x * x * 2. + yy2 = y * y * 2. + zz2 = z * z * 2. + xy2 = x * y * 2. + wz2 = w * z * 2. + zx2 = z * x * 2. + wy2 = w * y * 2. + yz2 = y * z * 2. + wx2 = w * x * 2. + + t = np.empty(tuple(q.shape[:-1] + (3, 3)), float) + t[..., 0, 0] = 1. - yy2 - zz2 + t[..., 0, 1] = xy2 - wz2 + t[..., 0, 2] = zx2 + wy2 + t[..., 1, 0] = xy2 + wz2 + t[..., 1, 1] = 1. - xx2 - zz2 + t[..., 1, 2] = yz2 - wx2 + t[..., 2, 0] = zx2 - wy2 + t[..., 2, 1] = yz2 + wx2 + t[..., 2, 2] = 1. - xx2 - yy2 + + return t def _equatorial2quat(self): - """Dummy method to return return quat. + """Return quaternion. :returns: quaternion :rtype: Quat @@ -388,18 +474,18 @@ def _equatorial2transform(self): """Construct the transform/rotation matrix from RA,Dec,Roll :returns: transform matrix - :rtype: 3x3 numpy array + :rtype: Nx3x3 numpy array """ - ra = radians(self._get_ra()) - dec = radians(self._get_dec()) - roll = radians(self._get_roll()) - ca = cos(ra) - sa = sin(ra) - cd = cos(dec) - sd = sin(dec) - cr = cos(roll) - sr = sin(roll) + ra = np.radians(self._get_ra()) + dec = np.radians(self._get_dec()) + roll = np.radians(self._get_roll()) + ca = np.cos(ra) + sa = np.sin(ra) + cd = np.cos(dec) + sd = np.sin(dec) + cr = np.cos(roll) + sr = np.sin(roll) # This is the transpose of the transformation matrix (related to translation # of original perl code @@ -407,49 +493,62 @@ def _equatorial2transform(self): [-ca * sd * sr - sa * cr, -sa * sd * sr + ca * cr, cd * sr], [-ca * sd * cr + sa * sr, -sa * sd * cr - ca * sr, cd * cr]]) - return rmat.transpose() + return np.moveaxis(np.moveaxis(rmat, 0, -1), 0, -2) def _transform2quat(self): - """Construct quaternion from the transform/rotation matrix + """Construct quaternions from the transform/rotation matrices - :returns: quaternion formed from transform matrix + :returns: quaternions formed from transform matrices :rtype: numpy array """ - # Code was copied from perl PDL code that uses backwards index ordering - T = self.transform.transpose() - den = np.array([1.0 + T[0, 0] - T[1, 1] - T[2, 2], - 1.0 - T[0, 0] + T[1, 1] - T[2, 2], - 1.0 - T[0, 0] - T[1, 1] + T[2, 2], - 1.0 + T[0, 0] + T[1, 1] + T[2, 2]]) - - max_idx = np.flatnonzero(den == max(den))[0] - - q = np.zeros(4) - q[max_idx] = 0.5 * sqrt(max(den)) - denom = 4.0 * q[max_idx] - if (max_idx == 0): - q[1] = (T[1, 0] + T[0, 1]) / denom - q[2] = (T[2, 0] + T[0, 2]) / denom - q[3] = -(T[2, 1] - T[1, 2]) / denom - if (max_idx == 1): - q[0] = (T[1, 0] + T[0, 1]) / denom - q[2] = (T[2, 1] + T[1, 2]) / denom - q[3] = -(T[0, 2] - T[2, 0]) / denom - if (max_idx == 2): - q[0] = (T[2, 0] + T[0, 2]) / denom - q[1] = (T[2, 1] + T[1, 2]) / denom - q[3] = -(T[1, 0] - T[0, 1]) / denom - if (max_idx == 3): - q[0] = -(T[2, 1] - T[1, 2]) / denom - q[1] = -(T[0, 2] - T[2, 0]) / denom - q[2] = -(T[1, 0] - T[0, 1]) / denom + T = self.transform + if T.ndim == 2: + T = T[np.newaxis] + den = np.array([1.0 + T[..., 0, 0] - T[..., 1, 1] - T[..., 2, 2], + 1.0 - T[..., 0, 0] + T[..., 1, 1] - T[..., 2, 2], + 1.0 - T[..., 0, 0] - T[..., 1, 1] + T[..., 2, 2], + 1.0 + T[..., 0, 0] + T[..., 1, 1] + T[..., 2, 2]]) + + half_rt_q_max = 0.5 * np.sqrt(np.max(den, axis=0)) + max_idx = np.argmax(den, axis=0) + poss_quat = np.zeros(tuple((4,) + T.shape[:-2] + (4,))) + denom = 4.0 * half_rt_q_max + poss_quat[0] = np.moveaxis( + np.array( + [half_rt_q_max, + (T[..., 1, 0] + T[..., 0, 1]) / denom, + (T[..., 2, 0] + T[..., 0, 2]) / denom, + (T[..., 2, 1] - T[..., 1, 2]) / denom]), 0, -1) + poss_quat[1] = np.moveaxis( + np.array( + [(T[..., 1, 0] + T[..., 0, 1]) / denom, + half_rt_q_max, + (T[..., 2, 1] + T[..., 1, 2]) / denom, + (T[..., 0, 2] - T[..., 2, 0]) / denom]), 0, -1) + poss_quat[2] = np.moveaxis( + np.array( + [(T[..., 2, 0] + T[..., 0, 2]) / denom, + (T[..., 2, 1] + T[..., 1, 2]) / denom, + half_rt_q_max, + (T[..., 1, 0] - T[..., 0, 1]) / denom]), 0, -1) + poss_quat[3] = np.moveaxis( + np.array( + [(T[..., 2, 1] - T[..., 1, 2]) / denom, + (T[..., 0, 2] - T[..., 2, 0]) / denom, + (T[..., 1, 0] - T[..., 0, 1]) / denom, + half_rt_q_max]), 0, -1) + + q = np.zeros(tuple(T.shape[:-2] + (4,))) + for idx in range(0, 4): + max_match = max_idx == idx + q[max_match] = poss_quat[idx][max_match] return q def __div__(self, quat2): """ - Divide one quaternion by another. + Divide one quaternion by another (or divide N quats by N quats). Example usage:: @@ -471,13 +570,13 @@ def __div__(self, quat2): def __mul__(self, quat2): """ - Multiply quaternion by another. + Multiply quaternion by another (or multiply N quats by N quats). Quaternion composition as a multiplication q = q1 * q2 is equivalent to applying the q2 transform followed by the q1 transform. Another way to express this is:: - q = Quat(numpy.dot(q1.transform, q2.transform)) + q = Quat(q1.transform @ q2.transform) Example usage:: @@ -498,33 +597,36 @@ def __mul__(self, quat2): :rtype: Quat """ - q1 = self.q - q2 = quat2.q - mult = np.zeros(4) - mult[0] = q1[3] * q2[0] - q1[2] * q2[1] + q1[1] * q2[2] + q1[0] * q2[3] - mult[1] = q1[2] * q2[0] + q1[3] * q2[1] - q1[0] * q2[2] + q1[1] * q2[3] - mult[2] = -q1[1] * q2[0] + q1[0] * q2[1] + q1[3] * q2[2] + q1[2] * q2[3] - mult[3] = -q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] + q1[3] * q2[3] - return Quat(mult) + q1 = np.atleast_2d(self.q) + q2 = np.atleast_2d(quat2.q) + assert q1.shape == q2.shape + mult = np.zeros(q1.shape) + mult[...,0] = q1[...,3] * q2[...,0] - q1[...,2] * q2[...,1] + q1[...,1] * q2[...,2] + q1[...,0] * q2[...,3] + mult[...,1] = q1[...,2] * q2[...,0] + q1[...,3] * q2[...,1] - q1[...,0] * q2[...,2] + q1[...,1] * q2[...,3] + mult[...,2] = -q1[...,1] * q2[...,0] + q1[...,0] * q2[...,1] + q1[...,3] * q2[...,2] + q1[...,2] * q2[...,3] + mult[...,3] = -q1[...,0] * q2[...,0] - q1[...,1] * q2[...,1] - q1[...,2] * q2[...,2] + q1[...,3] * q2[...,3] + return Quat(q=mult) def inv(self): """ - Invert the quaternion + Invert the quaternion. :returns: inverted quaternion :rtype: Quat """ - return Quat([self.q[0], self.q[1], self.q[2], -self.q[3]]) + q = np.array(self.q) + q[..., 3] *= -1 + return Quat(q=q) - def dq(self, q2): + def dq(self, q2=None, **kwargs): """ - Return a delta quaternion ``dq`` such that ``q2 = self * dq`` where ``q2`` - is anything that instantiates a ``Quat`` object. + Return a delta quaternion ``dq`` such that ``q2 = self * dq``. + I works with any argument that instantiates a ``Quat`` object. This method returns the delta quaternion which represents the transformation from the frame of this quaternion (``self``) to ``q2``. - q = Quat(numpy.dot(q1.transform, q2.transform)) + q = Quat(q1.transform @ q2.transform) Example usage:: @@ -533,20 +635,28 @@ def dq(self, q2): >>> dq = q1.dq(q2) >>> dq.equatorial array([ 1.79974166e-15, 1.00000000e-01, 1.00000000e+00]) + + :param: q2 Quat or array + + ``q2`` must have the same shape as self. + + :returns: Quat + :rtype: numpy array + """ - if not isinstance(q2, Quat): - q2 = Quat(q2) + if q2 is None or not isinstance(q2, Quat): + q2 = Quat(q2, **kwargs) return self.inv() * q2 def normalize(array): """ - Normalize a 4 element array/list/numpy.array for use as a quaternion + Normalize a 4 (or Nx4) element array/list/numpy.array for use as a quaternion - :param quat_array: 4 element list/array + :param array: 4 or Nx4 element list/array :returns: normalized array :rtype: numpy array """ quat = np.array(array) - return quat / np.sqrt(np.dot(quat, quat)) + return np.squeeze(quat/np.sqrt(np.sum(quat * quat, axis=-1, keepdims=True))) diff --git a/Quaternion/tests/test.py b/Quaternion/tests/test.py new file mode 100644 index 0000000..e69de29 diff --git a/Quaternion/tests/test_all.py b/Quaternion/tests/test_all.py index 5a81fa2..a4ad7e0 100644 --- a/Quaternion/tests/test_all.py +++ b/Quaternion/tests/test_all.py @@ -1,6 +1,21 @@ # Licensed under a 3-clause BSD style license - see LICENSE.rst import numpy as np -from .. import Quat +import pytest + +from .. import Quat, normalize + + +def indices(t): + import itertools + for k in itertools.product(*[range(i) for i in t]): + yield k + +def normalize_angles(x, xmin, xmax): + while np.any(x >= xmax): + x -= np.where(x > xmax, 360, 0) + while np.any(x < xmin): + x += np.where(x < xmin, 360, 0) + ra = 10. dec = 20. @@ -8,6 +23,72 @@ q0 = Quat([ra, dec, roll]) +equatorial_23 = np.array([[[10, 20, 30], + [10, 20, -30], + [10, -60, 30]], + [[10, 20, 0], + [10, 50, 30], + [10, -50, -30]]], dtype=float) + +q_23 = np.zeros(equatorial_23[..., 0].shape+(4,)) +for _i, _j in indices(equatorial_23.shape[:-1]): + q_23[_i, _j] = Quat(equatorial_23[_i, _j]).q + +transform_23 = np.zeros(equatorial_23[..., 0].shape+(3, 3)) +for _i, _j in indices(transform_23.shape[:-2]): + transform_23[_i, _j] = Quat(equatorial_23[_i, _j]).transform + + +def test_shape(): + q = Quat(q=np.zeros(4,)) + assert q.shape == () + with pytest.raises(AttributeError): + q.shape = (4,) + + +def test_init_exceptions(): + with pytest.raises(TypeError): + _ = Quat(q=np.zeros((3, ))) # old-style API, wrong shape + with pytest.raises(TypeError): + _ = Quat(equatorial=np.zeros((4, ))) # old-style API, wrong shape + with pytest.raises(TypeError): + _ = Quat(transform=np.zeros((4, ))) # old-style API, wrong shape + with pytest.raises(TypeError): + _ = Quat(np.zeros((2, ))) # old-style API, wrong shape + with pytest.raises(TypeError): + _ = Quat(np.zeros((5, ))) # old-style API, wrong shape + with pytest.raises(TypeError): + _ = Quat(equatorial_23) # old-style API, wrong shape + with pytest.raises(TypeError): + _ = Quat(q_23) # old-style API, wrong shape + with pytest.raises(TypeError): + _ = Quat(transform_23) # old-style API, wrong shape + with pytest.raises(ValueError): + _ = Quat(q=np.zeros(4), transform=np.zeros((3, 3))) # too many arguments + with pytest.raises(ValueError): + _ = Quat(q=np.zeros(4), equatorial=np.zeros(3)) # too many arguments + with pytest.raises(ValueError): + _ = Quat(equatorial=np.zeros(3), transform=np.zeros((3, 3))) # too many arguments + with pytest.raises(ValueError): + # too many arguments + _ = Quat(q=np.zeros(4), transform=np.zeros((3, 3)), equatorial=np.zeros(3)) + with pytest.raises(ValueError): + _ = Quat(q=[[[1., 0., 0., 1.]]]) # q not normalized + with pytest.raises(ValueError): + _ = Quat([0,1,'s']) # could not convert string to float + + +def test_from_q(): + q = [0.26853582, -0.14487813, 0.12767944, 0.94371436] + q1 = Quat(q) + q2 = Quat(q=q) + q3 = Quat(q1) + q = np.array(q) + assert np.all(q1.q == q) + assert np.all(q2.q == q) + assert np.all(q3.q == q) + + def test_from_eq(): q = Quat([ra, dec, roll]) assert np.allclose(q.q[0], 0.26853582) @@ -16,6 +97,66 @@ def test_from_eq(): assert np.allclose(q.q[3], 0.94371436) assert np.allclose(q.roll0, 30) assert np.allclose(q.ra0, 10) + assert q.pitch == -q.dec + assert q.yaw == q.ra0 + + q1 = Quat(equatorial=[ra, dec, roll]) + assert np.all(q1.q == q.q) + + +def test_from_eq_vectorized(): + # the following line would give unexpected results + # because the input is interpreted as a (non-vectorized) transform + # the shape of the input is (3,3) + # q = Quat(equatorial_23[0]) + + # this is the proper way: + q = Quat(equatorial=equatorial_23[0]) + assert q.q.shape == (3, 4) + for i in indices(q.shape): + # check that Quat(equatorial).q[i] == Quat(equatorial[i]).q + assert np.all(q.q[i] == Quat(equatorial_23[0][i]).q) + + q = Quat(equatorial=equatorial_23) + assert q.q.shape == (2, 3, 4) + for i in indices(q.shape): + # check that Quat(equatorial).q[i] == Quat(equatorial[i]).q + assert np.all(q.q[i] == Quat(equatorial_23[i]).q) + + # test init from list + q = Quat(equatorial=[ra, dec, roll]) + assert np.all(q.q == q0.q) + + q = Quat(equatorial=equatorial_23) + assert np.all(q.q == q_23) + assert np.all(q.equatorial == equatorial_23) + assert np.all(q.transform == transform_23) + +def test_from_eq_shapes(): + q = Quat(equatorial=equatorial_23[0, 0]) + assert q.ra.shape == () + assert q.dec.shape == () + assert q.roll.shape == () + assert q.q.shape == (4, ) + assert q.equatorial.shape == (3, ) + assert q.transform.shape == (3, 3) + + q = Quat(equatorial=equatorial_23[:1, :1]) + assert q.ra.shape == (1, 1) + assert q.dec.shape == (1, 1) + assert q.roll.shape == (1, 1) + assert q.q.shape == (1, 1, 4) + assert q.equatorial.shape == (1, 1, 3) + assert q.transform.shape == (1, 1, 3, 3) + + +def test_transform_from_eq(): + q = Quat(equatorial=equatorial_23) + assert q.transform.shape == (2, 3, 3, 3) + for i in indices(q.shape): + # check that + # Quat(equatorial).transform[i] == Quat(equatorial[i]).transform + assert np.all(q.transform[i] == Quat(equatorial_23[i]).transform) def test_from_transform(): @@ -30,6 +171,71 @@ def test_from_transform(): assert np.allclose(q.roll0, 30) assert np.allclose(q.ra0, 10) + q1 = Quat(transform=q0.transform) + assert np.all(q1.q == q.q) + + +def test_from_transform_vectorized(): + q = Quat(transform=transform_23) + assert q.q.shape == (2, 3, 4) + for i in indices(q.shape): + # check that Quat(transform).q[i] == Quat(transform[i]).q + assert np.all(q.q[i] == Quat(transform=transform_23[i]).q) + + q = Quat(transform=transform_23[:1, :1]) + assert q.q.shape == (1, 1, 4) + + t = [[[[9.25416578e-01, -3.18795778e-01, -2.04874129e-01], + [1.63175911e-01, 8.23172945e-01, -5.43838142e-01], + [3.42020143e-01, 4.69846310e-01, 8.13797681e-01]]]] + q = Quat(transform=t) + assert q.q.shape == (1, 1, 4) + + q = Quat(transform=transform_23) + assert np.allclose(q.q, q_23) + # to compare roll, it has to be normalized to within a fixed angular range (0, 360). + eq = np.array(q.equatorial) + normalize_angles(eq[...,-1], 0, 360) + eq_23 = np.array(equatorial_23) + normalize_angles(eq_23[..., -1], 0, 360) + assert np.allclose(eq, eq_23) + assert np.allclose(q.transform, transform_23) + +def test_eq_from_transform(): + # this raises 'Unexpected negative norm' exception due to roundoff in copy/paste above + # q = Quat(transform=transform_23) + # assert q.equatorial.shape == (2, 3, 3) + # assert np.allclose(q.equatorial, equatorial_23) + + t = np.zeros((4, 5, 3, 3)) + t[:] = q0.transform[np.newaxis][np.newaxis] + q = Quat(transform=t) + assert np.allclose(q.roll0, 30) + assert np.allclose(q.ra0, 10) + + assert q.equatorial.shape == (4, 5, 3) + + +def test_from_q_vectorized(): + q = Quat(q=q_23) + assert q.shape == (2, 3) + # this also tests that quaternions with negative scalar component are flipped + flip = np.sign(q_23[...,-1]).reshape((2,3,1)) + assert np.allclose(q.q, q_23*flip) + # to compare roll, it has to be normalized to within a fixed angular range (0, 360). + eq = np.array(q.equatorial) + normalize_angles(eq[...,-1], 0, 360) + eq_23 = np.array(equatorial_23) + normalize_angles(eq_23[..., -1], 0, 360) + assert np.allclose(eq, eq_23, rtol=0) + assert np.allclose(q.transform, transform_23, rtol=0) + + q = Quat(q=q_23[0]) + assert q.shape == (3,) + + q = Quat(q=q_23[:1, :1]) + assert q.shape == (1, 1) + def test_inv_eq(): q = Quat(q0.equatorial) @@ -42,6 +248,7 @@ def test_inv_eq(): def test_inv_q(): q = Quat(q0.q) + assert q.q.shape == q.inv().q.shape t = q.transform tinv = q.inv().transform t_tinv = np.dot(t, tinv) @@ -49,12 +256,87 @@ def test_inv_q(): assert np.allclose(v1, v2) +def test_inv_vectorized(): + q1 = Quat(q=q_23[:1, :1]) + assert q1.q.shape == (1, 1, 4) + q1_inv = q1.inv() + assert q1_inv.q.shape == q1.q.shape + for i in indices(q1.shape): + # check that Quat(q).inv().q[i] == Quat(q[i]).inv().q + assert np.all(q1_inv.q[i] == Quat(q=q1.q[i]).inv().q) + def test_dq(): q1 = Quat((20, 30, 0)) q2 = Quat((20, 30.1, 1)) dq = q1.dq(q2) assert np.allclose(dq.equatorial, (0, 0.1, 1)) + # same from array instead of Quat + dq = q1.dq(q2.q) + assert np.allclose(dq.equatorial, (0, 0.1, 1)) + + +def test_dq_vectorized(): + q1 = Quat(q=q_23[:1, :2]) + q2 = Quat(q=q_23[1:, 1:]) + assert q1.q.shape == q2.q.shape + + dq = q1.dq(q2) + assert dq.q.shape == q1.q.shape # shape (1,2,4) + + # same but with array argument instead of Quat + dq2 = q1.dq(q=q2.q) + assert dq2.q.shape == dq.q.shape + assert np.all(dq2.q == dq.q) + + for i in indices(q1.shape): + # check that Quat(q1).dq(q2).q[i] == Quat(q1[i]).dq(q2[i]).q + assert np.all(dq.q[i] == Quat(q=q1.q[i]).dq(Quat(q=q2.q[i])).q) + + # note that both quaternions have same _internal_ shape, should this fail? + q1 = Quat((20, 30, 0)) + q2 = Quat(equatorial=[[20, 30.1, 1]]) + assert np.allclose(q1.dq(q2).equatorial, [[0, 0.1, 1]]) + assert np.allclose(q1.dq(q=q2.q).equatorial, [[0, 0.1, 1]]) + assert np.allclose(q1.dq(equatorial=q2.equatorial).equatorial, [[0, 0.1, 1]]) + assert np.allclose(q1.dq(transform=q2.transform).equatorial, [[0, 0.1, 1]]) + # and the interface is the same as the constructor: + with pytest.raises(TypeError): + q1.dq(q2.q) + with pytest.raises(TypeError): + q1.dq(q2.equatorial) + with pytest.raises(TypeError): + q1.dq(q2.transform) + + +def test_vector_to_scalar_correspondence(): + """ + Simple test that all possible transform pathways give the same + answer when done in vectorized form as they do for the scalar version. + """ + atol = 1e-12 + + # Input equatorial has roll not in 0:360, so fix that for comparisons. + eq_23 = equatorial_23.copy() + normalize_angles(eq_23[..., -1], 0, 360) + + # Compare vectorized computations for all possible input/output combos + # with the same for the scalar calculation. + q = Quat(equatorial=equatorial_23) + assert np.all(q.q == q_23) + assert np.all(q.equatorial == equatorial_23) + assert np.all(q.transform == transform_23) + + q = Quat(q=q_23) + assert np.all(q.q == q_23) + assert np.allclose(q.equatorial, eq_23, rtol=0, atol=atol) + assert np.allclose(q.transform, transform_23, rtol=0, atol=atol) + + q = Quat(transform=transform_23) + assert np.allclose(q.q, q_23, rtol=0, atol=atol) + assert np.allclose(q.equatorial, eq_23, rtol=0, atol=atol) + assert np.all(q.transform == transform_23) + def test_ra0_roll0(): q = Quat(Quat([-1, 0, -2]).q) @@ -74,6 +356,9 @@ class SubQuat(Quat): q = SubQuat([1, 2, 3]) assert repr(q) == '' + q = Quat(equatorial=[[1, 2, 3]]) + assert repr(q) == 'Quat(array([[ 0.02632421, -0.01721736, 0.00917905, 0.99946303]]))' + def test_numeric_underflow(): """ @@ -91,7 +376,7 @@ def test_numeric_underflow(): while angle < 360: q = Quat((0, angle, 0)) quat = q * quat - quat.equatorial + _ = quat.equatorial angle += 0.1 @@ -101,3 +386,37 @@ def test_div_mult(): q12d = q1 / q2 q12m = q1 * q2.inv() assert np.all(q12d.q == q12m.q) + + +def test_mult_vectorized(): + q1 = Quat(q=q_23[:1, :2]) # (shape (2,1) + q2 = Quat(q=q_23[1:, 1:]) # (shape (2,1) + assert q1.q.shape == q2.q.shape + q12 = q1*q2 + assert q12.q.shape == q1.q.shape + + +def test_normalize(): + a = [[[1., 0., 0., 1.]]] + b = normalize(a) + assert np.isclose(np.sum(b**2), 1) + + +def test_copy(): + # data members must be copies so they are not modified by accident + q = np.array(q_23[0,0]) + q1 = Quat(q=q) + q[-1] = 0 + assert q1.q[-1] != 0 + + # this one passes + t = np.array(transform_23) + q1 = Quat(transform=t) + t[-1] = 0 + assert not np.all(q1.transform == t) + + # this one passes + eq = np.array([10, 90, 30]) + q1 = Quat(equatorial=eq) + eq[-1] = 0 + assert not np.all(q1.equatorial == eq)