Package tf :: Module transformations
[hide private]
[frames] | no frames]

Module transformations

source code

Homogeneous Transformation Matrices and Quaternions.

A library for calculating 4x4 matrices for translating, rotating, reflecting, scaling, shearing, projecting, orthogonalizing, and superimposing arrays of 3D homogeneous coordinates as well as for converting between rotation matrices, Euler angles, and quaternions. Also includes an Arcball control object and functions to decompose transformation matrices.

Requirements

Notes

Matrices (M) can be inverted using numpy.linalg.inv(M), concatenated using numpy.dot(M0, M1), or used to transform homogeneous coordinates (v) using numpy.dot(M, v) for shape (4, *) "point of arrays", respectively numpy.dot(v, M.T) for shape (*, 4) "array of points".

Calculations are carried out with numpy.float64 precision.

This Python implementation is not optimized for speed.

Vector, point, quaternion, and matrix function arguments are expected to be "array like", i.e. tuple, list, or numpy arrays.

Return types are numpy arrays unless specified otherwise.

Angles are in radians unless specified otherwise.

Quaternions ix+jy+kz+w are represented as [x, y, z, w].

Use the transpose of transformation matrices for OpenGL glMultMatrixd().

A triple of Euler angles can be applied/interpreted in 24 ways, which can be specified using a 4 character string or encoded 4-tuple:

Axes 4-string: e.g. 'sxyz' or 'ryxy'

Axes 4-tuple: e.g. (0, 0, 0, 0) or (1, 1, 1, 1)

References

  1. Matrices and transformations. Ronald Goldman. In "Graphics Gems I", pp 472-475. Morgan Kaufmann, 1990.
  2. More matrices and transformations: shear and pseudo-perspective. Ronald Goldman. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
  3. Decomposing a matrix into simple transformations. Spencer Thomas. In "Graphics Gems II", pp 320-323. Morgan Kaufmann, 1991.
  4. Recovering the data from the transformation matrix. Ronald Goldman. In "Graphics Gems II", pp 324-331. Morgan Kaufmann, 1991.
  5. Euler angle conversion. Ken Shoemake. In "Graphics Gems IV", pp 222-229. Morgan Kaufmann, 1994.
  6. Arcball rotation control. Ken Shoemake. In "Graphics Gems IV", pp 175-192. Morgan Kaufmann, 1994.
  7. Representing attitude: Euler angles, unit quaternions, and rotation vectors. James Diebel. 2006.
  8. A discussion of the solution for the best rotation to relate two sets of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828.
  9. Closed-form solution of absolute orientation using unit quaternions. BKP Horn. J Opt Soc Am A. 1987. 4(4), 629-642.
  10. Quaternions. Ken Shoemake. http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf
  11. From quaternion to matrix and back. JMP van Waveren. 2005. http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm
  12. Uniform random rotations. Ken Shoemake. In "Graphics Gems III", pp 124-132. Morgan Kaufmann, 1992.

Examples

>>> alpha, beta, gamma = 0.123, -1.234, 2.345
>>> origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)
>>> I = identity_matrix()
>>> Rx = rotation_matrix(alpha, xaxis)
>>> Ry = rotation_matrix(beta, yaxis)
>>> Rz = rotation_matrix(gamma, zaxis)
>>> R = concatenate_matrices(Rx, Ry, Rz)
>>> euler = euler_from_matrix(R, 'rxyz')
>>> numpy.allclose([alpha, beta, gamma], euler)
True
>>> Re = euler_matrix(alpha, beta, gamma, 'rxyz')
>>> is_same_transform(R, Re)
True
>>> al, be, ga = euler_from_matrix(Re, 'rxyz')
>>> is_same_transform(Re, euler_matrix(al, be, ga, 'rxyz'))
True
>>> qx = quaternion_about_axis(alpha, xaxis)
>>> qy = quaternion_about_axis(beta, yaxis)
>>> qz = quaternion_about_axis(gamma, zaxis)
>>> q = quaternion_multiply(qx, qy)
>>> q = quaternion_multiply(q, qz)
>>> Rq = quaternion_matrix(q)
>>> is_same_transform(R, Rq)
True
>>> S = scale_matrix(1.23, origin)
>>> T = translation_matrix((1, 2, 3))
>>> Z = shear_matrix(beta, xaxis, origin, zaxis)
>>> R = random_rotation_matrix(numpy.random.rand(3))
>>> M = concatenate_matrices(T, R, Z, S)
>>> scale, shear, angles, trans, persp = decompose_matrix(M)
>>> numpy.allclose(scale, 1.23)
True
>>> numpy.allclose(trans, (1, 2, 3))
True
>>> numpy.allclose(shear, (0, math.tan(beta), 0))
True
>>> is_same_transform(R, euler_matrix(axes='sxyz', *angles))
True
>>> M1 = compose_matrix(scale, shear, angles, trans, persp)
>>> is_same_transform(M, M1)
True

Author: Christoph Gohlke, Laboratory for Fluorescence Dynamics, University of California, Irvine

Version: 20090418

Classes [hide private]
  Arcball
Virtual Trackball Control.
Functions [hide private]
 
identity_matrix()
Return 4x4 identity/unit matrix.
source code
 
translation_matrix(direction)
Return matrix to translate by direction vector.
source code
 
translation_from_matrix(matrix)
Return translation vector from translation matrix.
source code
 
reflection_matrix(point, normal)
Return matrix to mirror at plane defined by point and normal vector.
source code
 
reflection_from_matrix(matrix)
Return mirror plane point and normal vector from reflection matrix.
source code
 
rotation_matrix(angle, direction, point=None)
Return matrix to rotate about axis defined by point and direction.
source code
 
rotation_from_matrix(matrix)
Return rotation angle and axis from rotation matrix.
source code
 
scale_matrix(factor, origin=None, direction=None)
Return matrix to scale by factor around origin in direction.
source code
 
scale_from_matrix(matrix)
Return scaling factor, origin and direction from scaling matrix.
source code
 
projection_matrix(point, normal, direction=None, perspective=None, pseudo=False)
Return matrix to project onto plane defined by point and normal.
source code
 
projection_from_matrix(matrix, pseudo=False)
Return projection plane and perspective point from projection matrix.
source code
 
clip_matrix(left, right, bottom, top, near, far, perspective=False)
Return matrix to obtain normalized device coordinates from frustrum.
source code
 
shear_matrix(angle, direction, point, normal)
Return matrix to shear by angle along direction vector on shear plane.
source code
 
shear_from_matrix(matrix)
Return shear angle, direction and plane from shear matrix.
source code
 
decompose_matrix(matrix)
Return sequence of transformations from transformation matrix.
source code
 
compose_matrix(scale=None, shear=None, angles=None, translate=None, perspective=None)
Return transformation matrix from sequence of transformations.
source code
 
orthogonalization_matrix(lengths, angles)
Return orthogonalization matrix for crystallographic cell coordinates.
source code
 
superimposition_matrix(v0, v1, scaling=False, usesvd=True)
Return matrix to transform given vector set into second vector set.
source code
 
euler_matrix(ai, aj, ak, axes='sxyz')
Return homogeneous rotation matrix from Euler angles and axis sequence.
source code
 
euler_from_matrix(matrix, axes='sxyz')
Return Euler angles from rotation matrix for specified axis sequence.
source code
 
euler_from_quaternion(quaternion, axes='sxyz')
Return Euler angles from quaternion for specified axis sequence.
source code
 
quaternion_from_euler(ai, aj, ak, axes='sxyz')
Return quaternion from Euler angles and axis sequence.
source code
 
quaternion_about_axis(angle, axis)
Return quaternion for rotation about axis.
source code
 
quaternion_matrix(quaternion)
Return homogeneous rotation matrix from quaternion.
source code
 
quaternion_from_matrix(matrix)
Return quaternion from rotation matrix.
source code
 
quaternion_multiply(quaternion1, quaternion0)
Return multiplication of two quaternions.
source code
 
quaternion_conjugate(quaternion)
Return conjugate of quaternion.
source code
 
quaternion_inverse(quaternion)
Return inverse of quaternion.
source code
 
quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True)
Return spherical linear interpolation between two quaternions.
source code
 
random_quaternion(rand=None)
Return uniform random unit quaternion.
source code
 
random_rotation_matrix(rand=None)
Return uniform random rotation matrix.
source code
 
arcball_map_to_sphere(point, center, radius)
Return unit sphere coordinates from window coordinates.
source code
 
arcball_constrain_to_axis(point, axis)
Return sphere point perpendicular to axis.
source code
 
arcball_nearest_axis(point, axes)
Return axis, which arc is nearest to point.
source code
 
vector_norm(data, axis=None, out=None)
Return length, i.e. eucledian norm, of ndarray along axis.
source code
 
unit_vector(data, axis=None, out=None)
Return ndarray normalized by length, i.e. eucledian norm, along axis.
source code
 
random_vector(size)
Return array of random doubles in the half-open interval [0.0, 1.0).
source code
 
inverse_matrix(matrix)
Return inverse of square transformation matrix.
source code
 
concatenate_matrices(*matrices)
Return concatenation of series of transformation matrices.
source code
 
is_same_transform(matrix0, matrix1)
Return True if two matrices perform same transformation.
source code
 
_import_module(module_name, warn=True, prefix='_py_', ignore='_')
Try import all public attributes from module into global namespace.
source code
Variables [hide private]
  _EPS = 8.8817841970012523e-16
  _NEXT_AXIS = [1, 2, 0, 1]
  _AXES2TUPLE = {'rxyx': (0, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rx...
  _TUPLE2AXES = {(0, 0, 0, 0): 'sxyz', (0, 0, 0, 1): 'rzyx', (0,...
  __package__ = 'tf'
Function Details [hide private]

scale_matrix(factor, origin=None, direction=None)

source code 

Return matrix to scale by factor around origin in direction.

Use factor -1 for point symmetry.

>>> v = (numpy.random.rand(4, 5) - 0.5) * 20.0
>>> v[3] = 1.0
>>> S = scale_matrix(-1.234)
>>> numpy.allclose(numpy.dot(S, v)[:3], -1.234*v[:3])
True
>>> factor = random.random() * 10 - 5
>>> origin = numpy.random.random(3) - 0.5
>>> direct = numpy.random.random(3) - 0.5
>>> S = scale_matrix(factor, origin)
>>> S = scale_matrix(factor, origin, direct)

projection_matrix(point, normal, direction=None, perspective=None, pseudo=False)

source code 

Return matrix to project onto plane defined by point and normal.

Using either perspective point, projection direction, or none of both.

If pseudo is True, perspective projections will preserve relative depth such that Perspective = dot(Orthogonal, PseudoPerspective).

>>> P = projection_matrix((0, 0, 0), (1, 0, 0))
>>> numpy.allclose(P[1:, 1:], numpy.identity(4)[1:, 1:])
True
>>> point = numpy.random.random(3) - 0.5
>>> normal = numpy.random.random(3) - 0.5
>>> direct = numpy.random.random(3) - 0.5
>>> persp = numpy.random.random(3) - 0.5
>>> P0 = projection_matrix(point, normal)
>>> P1 = projection_matrix(point, normal, direction=direct)
>>> P2 = projection_matrix(point, normal, perspective=persp)
>>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True)
>>> is_same_transform(P2, numpy.dot(P0, P3))
True
>>> P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0))
>>> v0 = (numpy.random.rand(4, 5) - 0.5) * 20.0
>>> v0[3] = 1.0
>>> v1 = numpy.dot(P, v0)
>>> numpy.allclose(v1[1], v0[1])
True
>>> numpy.allclose(v1[0], 3.0-v1[1])
True

projection_from_matrix(matrix, pseudo=False)

source code 

Return projection plane and perspective point from projection matrix.

Return values are same as arguments for projection_matrix function: point, normal, direction, perspective, and pseudo.

>>> point = numpy.random.random(3) - 0.5
>>> normal = numpy.random.random(3) - 0.5
>>> direct = numpy.random.random(3) - 0.5
>>> persp = numpy.random.random(3) - 0.5
>>> P0 = projection_matrix(point, normal)
>>> result = projection_from_matrix(P0)
>>> P1 = projection_matrix(*result)
>>> is_same_transform(P0, P1)
True
>>> P0 = projection_matrix(point, normal, direct)
>>> result = projection_from_matrix(P0)
>>> P1 = projection_matrix(*result)
>>> is_same_transform(P0, P1)
True
>>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False)
>>> result = projection_from_matrix(P0, pseudo=False)
>>> P1 = projection_matrix(*result)
>>> is_same_transform(P0, P1)
True
>>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True)
>>> result = projection_from_matrix(P0, pseudo=True)
>>> P1 = projection_matrix(*result)
>>> is_same_transform(P0, P1)
True

clip_matrix(left, right, bottom, top, near, far, perspective=False)

source code 

Return matrix to obtain normalized device coordinates from frustrum.

The frustrum bounds are axis-aligned along x (left, right), y (bottom, top) and z (near, far).

Normalized device coordinates are in range [-1, 1] if coordinates are inside the frustrum.

If perspective is True the frustrum is a truncated pyramid with the perspective point at origin and direction along z axis, otherwise an orthographic canonical view volume (a box).

Homogeneous coordinates transformed by the perspective clip matrix need to be dehomogenized (devided by w coordinate).

>>> frustrum = numpy.random.rand(6)
>>> frustrum[1] += frustrum[0]
>>> frustrum[3] += frustrum[2]
>>> frustrum[5] += frustrum[4]
>>> M = clip_matrix(*frustrum, perspective=False)
>>> numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0])
array([-1., -1., -1.,  1.])
>>> numpy.dot(M, [frustrum[1], frustrum[3], frustrum[5], 1.0])
array([ 1.,  1.,  1.,  1.])
>>> M = clip_matrix(*frustrum, perspective=True)
>>> v = numpy.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0])
>>> v / v[3]
array([-1., -1., -1.,  1.])
>>> v = numpy.dot(M, [frustrum[1], frustrum[3], frustrum[4], 1.0])
>>> v / v[3]
array([ 1.,  1., -1.,  1.])

shear_matrix(angle, direction, point, normal)

source code 

Return matrix to shear by angle along direction vector on shear plane.

The shear plane is defined by a point and normal vector. The direction vector must be orthogonal to the plane's normal vector.

A point P is transformed by the shear matrix into P" such that the vector P-P" is parallel to the direction vector and its extent is given by the angle of P-P'-P", where P' is the orthogonal projection of P onto the shear plane.

>>> angle = (random.random() - 0.5) * 4*math.pi
>>> direct = numpy.random.random(3) - 0.5
>>> point = numpy.random.random(3) - 0.5
>>> normal = numpy.cross(direct, numpy.random.random(3))
>>> S = shear_matrix(angle, direct, point, normal)
>>> numpy.allclose(1.0, numpy.linalg.det(S))
True

decompose_matrix(matrix)

source code 

Return sequence of transformations from transformation matrix.

matrix : array_like
Non-degenerative homogeneous transformation matrix
Return tuple of:
scale : vector of 3 scaling factors shear : list of shear factors for x-y, x-z, y-z axes angles : list of Euler angles about static x, y, z axes translate : translation vector along x, y, z axes perspective : perspective partition of matrix

Raise ValueError if matrix is of wrong type or degenerative.

>>> T0 = translation_matrix((1, 2, 3))
>>> scale, shear, angles, trans, persp = decompose_matrix(T0)
>>> T1 = translation_matrix(trans)
>>> numpy.allclose(T0, T1)
True
>>> S = scale_matrix(0.123)
>>> scale, shear, angles, trans, persp = decompose_matrix(S)
>>> scale[0]
0.123
>>> R0 = euler_matrix(1, 2, 3)
>>> scale, shear, angles, trans, persp = decompose_matrix(R0)
>>> R1 = euler_matrix(*angles)
>>> numpy.allclose(R0, R1)
True

compose_matrix(scale=None, shear=None, angles=None, translate=None, perspective=None)

source code 

Return transformation matrix from sequence of transformations.

This is the inverse of the decompose_matrix function.

Sequence of transformations:
scale : vector of 3 scaling factors shear : list of shear factors for x-y, x-z, y-z axes angles : list of Euler angles about static x, y, z axes translate : translation vector along x, y, z axes perspective : perspective partition of matrix
>>> scale = numpy.random.random(3) - 0.5
>>> shear = numpy.random.random(3) - 0.5
>>> angles = (numpy.random.random(3) - 0.5) * (2*math.pi)
>>> trans = numpy.random.random(3) - 0.5
>>> persp = numpy.random.random(4) - 0.5
>>> M0 = compose_matrix(scale, shear, angles, trans, persp)
>>> result = decompose_matrix(M0)
>>> M1 = compose_matrix(*result)
>>> is_same_transform(M0, M1)
True

orthogonalization_matrix(lengths, angles)

source code 

Return orthogonalization matrix for crystallographic cell coordinates.

Angles are expected in degrees.

The de-orthogonalization matrix is the inverse.

>>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.))
>>> numpy.allclose(O[:3, :3], numpy.identity(3, float) * 10)
True
>>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
>>> numpy.allclose(numpy.sum(O), 43.063229)
True

superimposition_matrix(v0, v1, scaling=False, usesvd=True)

source code 

Return matrix to transform given vector set into second vector set.

v0 and v1 are shape (3, *) or (4, *) arrays of at least 3 vectors.

If usesvd is True, the weighted sum of squared deviations (RMSD) is minimized according to the algorithm by W. Kabsch [8]. Otherwise the quaternion based algorithm by B. Horn [9] is used (slower when using this Python implementation).

The returned matrix performs rotation, translation and uniform scaling (if specified).

>>> v0 = numpy.random.rand(3, 10)
>>> M = superimposition_matrix(v0, v0)
>>> numpy.allclose(M, numpy.identity(4))
True
>>> R = random_rotation_matrix(numpy.random.random(3))
>>> v0 = ((1,0,0), (0,1,0), (0,0,1), (1,1,1))
>>> v1 = numpy.dot(R, v0)
>>> M = superimposition_matrix(v0, v1)
>>> numpy.allclose(v1, numpy.dot(M, v0))
True
>>> v0 = (numpy.random.rand(4, 100) - 0.5) * 20.0
>>> v0[3] = 1.0
>>> v1 = numpy.dot(R, v0)
>>> M = superimposition_matrix(v0, v1)
>>> numpy.allclose(v1, numpy.dot(M, v0))
True
>>> S = scale_matrix(random.random())
>>> T = translation_matrix(numpy.random.random(3)-0.5)
>>> M = concatenate_matrices(T, R, S)
>>> v1 = numpy.dot(M, v0)
>>> v0[:3] += numpy.random.normal(0.0, 1e-9, 300).reshape(3, -1)
>>> M = superimposition_matrix(v0, v1, scaling=True)
>>> numpy.allclose(v1, numpy.dot(M, v0))
True
>>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False)
>>> numpy.allclose(v1, numpy.dot(M, v0))
True
>>> v = numpy.empty((4, 100, 3), dtype=numpy.float64)
>>> v[:, :, 0] = v0
>>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False)
>>> numpy.allclose(v1, numpy.dot(M, v[:, :, 0]))
True

euler_matrix(ai, aj, ak, axes='sxyz')

source code 

Return homogeneous rotation matrix from Euler angles and axis sequence.

ai, aj, ak : Euler's roll, pitch and yaw angles axes : One of 24 axis sequences as string or encoded tuple

>>> R = euler_matrix(1, 2, 3, 'syxz')
>>> numpy.allclose(numpy.sum(R[0]), -1.34786452)
True
>>> R = euler_matrix(1, 2, 3, (0, 1, 0, 1))
>>> numpy.allclose(numpy.sum(R[0]), -0.383436184)
True
>>> ai, aj, ak = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
>>> for axes in _AXES2TUPLE.keys():
...    R = euler_matrix(ai, aj, ak, axes)
>>> for axes in _TUPLE2AXES.keys():
...    R = euler_matrix(ai, aj, ak, axes)

euler_from_matrix(matrix, axes='sxyz')

source code 

Return Euler angles from rotation matrix for specified axis sequence.

axes : One of 24 axis sequences as string or encoded tuple

Note that many Euler angle triplets can describe one matrix.

>>> R0 = euler_matrix(1, 2, 3, 'syxz')
>>> al, be, ga = euler_from_matrix(R0, 'syxz')
>>> R1 = euler_matrix(al, be, ga, 'syxz')
>>> numpy.allclose(R0, R1)
True
>>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
>>> for axes in _AXES2TUPLE.keys():
...    R0 = euler_matrix(axes=axes, *angles)
...    R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
...    if not numpy.allclose(R0, R1): print axes, "failed"

quaternion_from_euler(ai, aj, ak, axes='sxyz')

source code 

Return quaternion from Euler angles and axis sequence.

ai, aj, ak : Euler's roll, pitch and yaw angles axes : One of 24 axis sequences as string or encoded tuple

>>> q = quaternion_from_euler(1, 2, 3, 'ryxz')
>>> numpy.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953])
True

random_quaternion(rand=None)

source code 

Return uniform random unit quaternion.

rand: array like or None
Three independent random variables that are uniformly distributed between 0 and 1.
>>> q = random_quaternion()
>>> numpy.allclose(1.0, vector_norm(q))
True
>>> q = random_quaternion(numpy.random.random(3))
>>> q.shape
(4,)

random_rotation_matrix(rand=None)

source code 

Return uniform random rotation matrix.

rnd: array like
Three independent random variables that are uniformly distributed between 0 and 1 for each returned quaternion.
>>> R = random_rotation_matrix()
>>> numpy.allclose(numpy.dot(R.T, R), numpy.identity(4))
True

_import_module(module_name, warn=True, prefix='_py_', ignore='_')

source code 

Try import all public attributes from module into global namespace.

Existing attributes with name clashes are renamed with prefix. Attributes starting with underscore are ignored by default.

Return True on successful import.


Variables Details [hide private]

_AXES2TUPLE

Value:
{'rxyx': (0, 0, 1, 1),
 'rxyz': (2, 1, 0, 1),
 'rxzx': (0, 1, 1, 1),
 'rxzy': (1, 0, 0, 1),
 'ryxy': (1, 1, 1, 1),
 'ryxz': (2, 0, 0, 1),
 'ryzx': (0, 1, 0, 1),
 'ryzy': (1, 0, 1, 1),
...

_TUPLE2AXES

Value:
{(0, 0, 0, 0): 'sxyz',
 (0, 0, 0, 1): 'rzyx',
 (0, 0, 1, 0): 'sxyx',
 (0, 0, 1, 1): 'rxyx',
 (0, 1, 0, 0): 'sxzy',
 (0, 1, 0, 1): 'ryzx',
 (0, 1, 1, 0): 'sxzx',
 (0, 1, 1, 1): 'rxzx',
...