|
- # emacs: -*- mode: python-mode; py-indent-offset: 4; indent-tabs-mode: nil -*-
- # vi: set ft=python sts=4 ts=4 sw=4 et:
- ### ### ### ### ### ### ### ### ### ### ### ### ### ### ### ### ### ### ### ##
- #
- # See COPYING file distributed along with the NiBabel package for the
- # copyright and license terms.
- #
- ### ### ### ### ### ### ### ### ### ### ### ### ### ### ### ### ### ### ### ##
- ''' Module implementing Euler angle rotations and their conversions
-
- See:
-
- * http://en.wikipedia.org/wiki/Rotation_matrix
- * http://en.wikipedia.org/wiki/Euler_angles
- * http://mathworld.wolfram.com/EulerAngles.html
-
- See also: *Representing Attitude with Euler Angles and Quaternions: A
- Reference* (2006) by James Diebel. A cached PDF link last found here:
-
- http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.110.5134
-
- Euler's rotation theorem tells us that any rotation in 3D can be
- described by 3 angles. Let's call the 3 angles the *Euler angle vector*
- and call the angles in the vector :math:`alpha`, :math:`beta` and
- :math:`gamma`. The vector is [ :math:`alpha`,
- :math:`beta`. :math:`gamma` ] and, in this description, the order of the
- parameters specifies the order in which the rotations occur (so the
- rotation corresponding to :math:`alpha` is applied first).
-
- In order to specify the meaning of an *Euler angle vector* we need to
- specify the axes around which each of the rotations corresponding to
- :math:`alpha`, :math:`beta` and :math:`gamma` will occur.
-
- There are therefore three axes for the rotations :math:`alpha`,
- :math:`beta` and :math:`gamma`; let's call them :math:`i` :math:`j`,
- :math:`k`.
-
- Let us express the rotation :math:`alpha` around axis `i` as a 3 by 3
- rotation matrix `A`. Similarly :math:`beta` around `j` becomes 3 x 3
- matrix `B` and :math:`gamma` around `k` becomes matrix `G`. Then the
- whole rotation expressed by the Euler angle vector [ :math:`alpha`,
- :math:`beta`. :math:`gamma` ], `R` is given by::
-
- R = np.dot(G, np.dot(B, A))
-
- See http://mathworld.wolfram.com/EulerAngles.html
-
- The order :math:`G B A` expresses the fact that the rotations are
- performed in the order of the vector (:math:`alpha` around axis `i` =
- `A` first).
-
- To convert a given Euler angle vector to a meaningful rotation, and a
- rotation matrix, we need to define:
-
- * the axes `i`, `j`, `k`
- * whether a rotation matrix should be applied on the left of a vector to
- be transformed (vectors are column vectors) or on the right (vectors
- are row vectors).
- * whether the rotations move the axes as they are applied (intrinsic
- rotations) - compared the situation where the axes stay fixed and the
- vectors move within the axis frame (extrinsic)
- * the handedness of the coordinate system
-
- See: http://en.wikipedia.org/wiki/Rotation_matrix#Ambiguities
-
- We are using the following conventions:
-
- * axes `i`, `j`, `k` are the `z`, `y`, and `x` axes respectively. Thus
- an Euler angle vector [ :math:`alpha`, :math:`beta`. :math:`gamma` ]
- in our convention implies a :math:`alpha` radian rotation around the
- `z` axis, followed by a :math:`beta` rotation around the `y` axis,
- followed by a :math:`gamma` rotation around the `x` axis.
- * the rotation matrix applies on the left, to column vectors on the
- right, so if `R` is the rotation matrix, and `v` is a 3 x N matrix
- with N column vectors, the transformed vector set `vdash` is given by
- ``vdash = np.dot(R, v)``.
- * extrinsic rotations - the axes are fixed, and do not move with the
- rotations.
- * a right-handed coordinate system
-
- The convention of rotation around ``z``, followed by rotation around
- ``y``, followed by rotation around ``x``, is known (confusingly) as
- "xyz", pitch-roll-yaw, Cardan angles, or Tait-Bryan angles.
- '''
-
- import math
-
- import sys
- if sys.version_info >= (3,0):
- from functools import reduce
-
- import numpy as np
-
-
- _FLOAT_EPS_4 = np.finfo(float).eps * 4.0
-
-
- def euler2mat(z=0, y=0, x=0):
- ''' Return matrix for rotations around z, y and x axes
-
- Uses the z, then y, then x convention above
-
- Parameters
- ----------
- z : scalar
- Rotation angle in radians around z-axis (performed first)
- y : scalar
- Rotation angle in radians around y-axis
- x : scalar
- Rotation angle in radians around x-axis (performed last)
-
- Returns
- -------
- M : array shape (3,3)
- Rotation matrix giving same rotation as for given angles
-
- Examples
- --------
- >>> zrot = 1.3 # radians
- >>> yrot = -0.1
- >>> xrot = 0.2
- >>> M = euler2mat(zrot, yrot, xrot)
- >>> M.shape == (3, 3)
- True
-
- The output rotation matrix is equal to the composition of the
- individual rotations
-
- >>> M1 = euler2mat(zrot)
- >>> M2 = euler2mat(0, yrot)
- >>> M3 = euler2mat(0, 0, xrot)
- >>> composed_M = np.dot(M3, np.dot(M2, M1))
- >>> np.allclose(M, composed_M)
- True
-
- You can specify rotations by named arguments
-
- >>> np.all(M3 == euler2mat(x=xrot))
- True
-
- When applying M to a vector, the vector should column vector to the
- right of M. If the right hand side is a 2D array rather than a
- vector, then each column of the 2D array represents a vector.
-
- >>> vec = np.array([1, 0, 0]).reshape((3,1))
- >>> v2 = np.dot(M, vec)
- >>> vecs = np.array([[1, 0, 0],[0, 1, 0]]).T # giving 3x2 array
- >>> vecs2 = np.dot(M, vecs)
-
- Rotations are counter-clockwise.
-
- >>> zred = np.dot(euler2mat(z=np.pi/2), np.eye(3))
- >>> np.allclose(zred, [[0, -1, 0],[1, 0, 0], [0, 0, 1]])
- True
- >>> yred = np.dot(euler2mat(y=np.pi/2), np.eye(3))
- >>> np.allclose(yred, [[0, 0, 1],[0, 1, 0], [-1, 0, 0]])
- True
- >>> xred = np.dot(euler2mat(x=np.pi/2), np.eye(3))
- >>> np.allclose(xred, [[1, 0, 0],[0, 0, -1], [0, 1, 0]])
- True
-
- Notes
- -----
- The direction of rotation is given by the right-hand rule (orient
- the thumb of the right hand along the axis around which the rotation
- occurs, with the end of the thumb at the positive end of the axis;
- curl your fingers; the direction your fingers curl is the direction
- of rotation). Therefore, the rotations are counterclockwise if
- looking along the axis of rotation from positive to negative.
- '''
- Ms = []
- if z:
- cosz = math.cos(z)
- sinz = math.sin(z)
- Ms.append(np.array(
- [[cosz, -sinz, 0],
- [sinz, cosz, 0],
- [0, 0, 1]]))
- if y:
- cosy = math.cos(y)
- siny = math.sin(y)
- Ms.append(np.array(
- [[cosy, 0, siny],
- [0, 1, 0],
- [-siny, 0, cosy]]))
- if x:
- cosx = math.cos(x)
- sinx = math.sin(x)
- Ms.append(np.array(
- [[1, 0, 0],
- [0, cosx, -sinx],
- [0, sinx, cosx]]))
- if Ms:
- return reduce(np.dot, Ms[::-1])
- return np.eye(3)
-
-
- def mat2euler(M, cy_thresh=None):
- ''' Discover Euler angle vector from 3x3 matrix
-
- Uses the conventions above.
-
- Parameters
- ----------
- M : array-like, shape (3,3)
- cy_thresh : None or scalar, optional
- threshold below which to give up on straightforward arctan for
- estimating x rotation. If None (default), estimate from
- precision of input.
-
- Returns
- -------
- z : scalar
- y : scalar
- x : scalar
- Rotations in radians around z, y, x axes, respectively
-
- Notes
- -----
- If there was no numerical error, the routine could be derived using
- Sympy expression for z then y then x rotation matrix, which is::
-
- [ cos(y)*cos(z), -cos(y)*sin(z), sin(y)],
- [cos(x)*sin(z) + cos(z)*sin(x)*sin(y), cos(x)*cos(z) - sin(x)*sin(y)*sin(z), -cos(y)*sin(x)],
- [sin(x)*sin(z) - cos(x)*cos(z)*sin(y), cos(z)*sin(x) + cos(x)*sin(y)*sin(z), cos(x)*cos(y)]
-
- with the obvious derivations for z, y, and x
-
- z = atan2(-r12, r11)
- y = asin(r13)
- x = atan2(-r23, r33)
-
- Problems arise when cos(y) is close to zero, because both of::
-
- z = atan2(cos(y)*sin(z), cos(y)*cos(z))
- x = atan2(cos(y)*sin(x), cos(x)*cos(y))
-
- will be close to atan2(0, 0), and highly unstable.
-
- The ``cy`` fix for numerical instability below is from: *Graphics
- Gems IV*, Paul Heckbert (editor), Academic Press, 1994, ISBN:
- 0123361559. Specifically it comes from EulerAngles.c by Ken
- Shoemake, and deals with the case where cos(y) is close to zero:
-
- See: http://www.graphicsgems.org/
-
- The code appears to be licensed (from the website) as "can be used
- without restrictions".
- '''
- M = np.asarray(M)
- if cy_thresh is None:
- try:
- cy_thresh = np.finfo(M.dtype).eps * 4
- except ValueError:
- cy_thresh = _FLOAT_EPS_4
- r11, r12, r13, r21, r22, r23, r31, r32, r33 = M.flat
- # cy: sqrt((cos(y)*cos(z))**2 + (cos(x)*cos(y))**2)
- cy = math.sqrt(r33*r33 + r23*r23)
- if cy > cy_thresh: # cos(y) not close to zero, standard form
- z = math.atan2(-r12, r11) # atan2(cos(y)*sin(z), cos(y)*cos(z))
- y = math.atan2(r13, cy) # atan2(sin(y), cy)
- x = math.atan2(-r23, r33) # atan2(cos(y)*sin(x), cos(x)*cos(y))
- else: # cos(y) (close to) zero, so x -> 0.0 (see above)
- # so r21 -> sin(z), r22 -> cos(z) and
- z = math.atan2(r21, r22)
- y = math.atan2(r13, cy) # atan2(sin(y), cy)
- x = 0.0
- return z, y, x
-
-
- def euler2quat(z=0, y=0, x=0):
- ''' Return quaternion corresponding to these Euler angles
-
- Uses the z, then y, then x convention above
-
- Parameters
- ----------
- z : scalar
- Rotation angle in radians around z-axis (performed first)
- y : scalar
- Rotation angle in radians around y-axis
- x : scalar
- Rotation angle in radians around x-axis (performed last)
-
- Returns
- -------
- quat : array shape (4,)
- Quaternion in w, x, y z (real, then vector) format
-
- Notes
- -----
- We can derive this formula in Sympy using:
-
- 1. Formula giving quaternion corresponding to rotation of theta radians
- about arbitrary axis:
- http://mathworld.wolfram.com/EulerParameters.html
- 2. Generated formulae from 1.) for quaternions corresponding to
- theta radians rotations about ``x, y, z`` axes
- 3. Apply quaternion multiplication formula -
- http://en.wikipedia.org/wiki/Quaternions#Hamilton_product - to
- formulae from 2.) to give formula for combined rotations.
- '''
- z = z/2.0
- y = y/2.0
- x = x/2.0
- cz = math.cos(z)
- sz = math.sin(z)
- cy = math.cos(y)
- sy = math.sin(y)
- cx = math.cos(x)
- sx = math.sin(x)
- return np.array([
- cx*cy*cz - sx*sy*sz,
- cx*sy*sz + cy*cz*sx,
- cx*cz*sy - sx*cy*sz,
- cx*cy*sz + sx*cz*sy])
-
-
- def quat2euler(q):
- ''' Return Euler angles corresponding to quaternion `q`
-
- Parameters
- ----------
- q : 4 element sequence
- w, x, y, z of quaternion
-
- Returns
- -------
- z : scalar
- Rotation angle in radians around z-axis (performed first)
- y : scalar
- Rotation angle in radians around y-axis
- x : scalar
- Rotation angle in radians around x-axis (performed last)
-
- Notes
- -----
- It's possible to reduce the amount of calculation a little, by
- combining parts of the ``quat2mat`` and ``mat2euler`` functions, but
- the reduction in computation is small, and the code repetition is
- large.
- '''
- # delayed import to avoid cyclic dependencies
- import nibabel.quaternions as nq
- return mat2euler(nq.quat2mat(q))
-
-
- def euler2angle_axis(z=0, y=0, x=0):
- ''' Return angle, axis corresponding to these Euler angles
-
- Uses the z, then y, then x convention above
-
- Parameters
- ----------
- z : scalar
- Rotation angle in radians around z-axis (performed first)
- y : scalar
- Rotation angle in radians around y-axis
- x : scalar
- Rotation angle in radians around x-axis (performed last)
-
- Returns
- -------
- theta : scalar
- angle of rotation
- vector : array shape (3,)
- axis around which rotation occurs
-
- Examples
- --------
- >>> theta, vec = euler2angle_axis(0, 1.5, 0)
- >>> print(theta)
- 1.5
- >>> np.allclose(vec, [0, 1, 0])
- True
- '''
- # delayed import to avoid cyclic dependencies
- import nibabel.quaternions as nq
- return nq.quat2angle_axis(euler2quat(z, y, x))
-
-
- def angle_axis2euler(theta, vector, is_normalized=False):
- ''' Convert angle, axis pair to Euler angles
-
- Parameters
- ----------
- theta : scalar
- angle of rotation
- vector : 3 element sequence
- vector specifying axis for rotation.
- is_normalized : bool, optional
- True if vector is already normalized (has norm of 1). Default
- False
-
- Returns
- -------
- z : scalar
- y : scalar
- x : scalar
- Rotations in radians around z, y, x axes, respectively
-
- Examples
- --------
- >>> z, y, x = angle_axis2euler(0, [1, 0, 0])
- >>> np.allclose((z, y, x), 0)
- True
-
- Notes
- -----
- It's possible to reduce the amount of calculation a little, by
- combining parts of the ``angle_axis2mat`` and ``mat2euler``
- functions, but the reduction in computation is small, and the code
- repetition is large.
- '''
- # delayed import to avoid cyclic dependencies
- import nibabel.quaternions as nq
- M = nq.angle_axis2mat(theta, vector, is_normalized)
- return mat2euler(M)
|