Transforms in ND

This modules contains functions to operate on special matrices in 2D or 3D, for example SE(n), SO(n), se(n) and so(n) where n is 2 or 3.

Vector arguments are what numpy refers to as array_like and can be a list, tuple, numpy array, numpy row vector or numpy column vector.

Ab2M(A, b)[source]

Pack matrix and vector to matrix

Parameters
  • A (ndarray(3,3) or ndarray(2,2)) – square matrix

  • b (ndarray(3) or ndarray(2)) – translation vector

Returns

matrix

Return type

ndarray(4,4) or ndarray(3,3)

Raises

ValueError – bad arguments

M = Ab2M(A, b) is a matrix (N+1xN+1) formed from a matrix R (NxN) and a vector t (Nx1). The bottom row is all zeros.

  • If A is 2x2 and b is 2x1, then M is 3x3

  • If A is 3x3 and b is 3x1, then M is 4x4

>>> from spatialmath.base import *
>>> A = np.c_[[1, 2], [3, 4]].T
>>> b = [5, 6]
>>> Ab2M(A, b)
array([[1., 2., 5.],
       [3., 4., 6.],
       [0., 0., 0.]])
Seealso

rt2tr, tr2rt, r2t

det(m)[source]

Determinant of matrix

Parameters

m (ndarray) – any square matrix

Returns

determinant

Return type

float

det(v) is the determinant of the matrix m.

>>> from spatialmath.base import *
>>> norm([3, 4])
5.0
Seealso

det()

SymPy

supported

e2h(v)[source]

Convert from Euclidean to homogeneous form

Parameters

v (array_like(n), ndarray(n,m)) – Euclidean vector or matrix

Returns

homogeneous vector

Return type

ndarray(n+1,m)

  • If v is an N-vector, return an (N+1)-column vector where a value of 1 has been appended as the last element.

  • If v is a matrix (NxM), return a matrix (N+1xM), where each column has been appended with a value of 1, ie. a row of ones has been appended to the matrix.

>>> from spatialmath.base import *
>>> e2h([2, 4, 6])
array([[2.],
       [4.],
       [6.],
       [1.]])
>>> e = np.c_[[1,2], [3,4], [5,6]]
>>> e
array([[1, 3, 5],
       [2, 4, 6]])
>>> e2h(e)
array([[1., 3., 5.],
       [2., 4., 6.],
       [1., 1., 1.]])

Note

The result is always a 2D array, a 1D input results in a column vector.

Seealso

e2h

h2e(v)[source]

Convert from homogeneous to Euclidean form

Parameters

v (array_like(n), ndarray(n,m)) – homogeneous vector or matrix

Returns

Euclidean vector

Return type

ndarray(n-1), ndarray(n-1,m)

  • If v is an N-vector, return an (N-1)-column vector where the elements have all been scaled by the last element of v.

  • If v is a matrix (NxM), return a matrix (N-1xM), where each column has been scaled by its last element.

>>> from spatialmath.base import *
>>> h2e([2, 4, 6, 1])
array([[2.],
       [4.],
       [6.]])
>>> h2e([2, 4, 6, 2])
array([[1.],
       [2.],
       [3.]])
>>> h = np.c_[[1,2,1], [3,4,2], [5,6,1]]
>>> h
array([[1, 3, 5],
       [2, 4, 6],
       [1, 2, 1]])
>>> h2e(h)
array([[1. , 1.5, 5. ],
       [2. , 2. , 6. ]])

Note

The result is always a 2D array, a 1D input results in a column vector.

Seealso

e2h

homtrans(T, p)[source]

Apply a homogeneous transformation to a Euclidean vector

Parameters
  • T (Numpy array (n,n)) – homogeneous transformation

  • p (array_like(n-1), ndarray(n-1,m)) – Vector(s) to be transformed

Returns

transformed Euclidean vector(s)

Return type

ndarray(n-1,m)

Raises

ValueError – bad argument

  • homtrans(T, p) applies the homogeneous transformation T to the Euclidean points stored columnwise in the array p.

  • homtrans(T, v) as above but v is a 1D array considered to be a column vector, and the retured value will be a column vector.

>>> from spatialmath.base import *
>>> T = trotx(0.3)
>>> v = [1, 2, 3]
>>> h2e( T @ e2h(v))
array([[1.    ],
       [1.0241],
       [3.457 ]])
>>> homtrans(T, v)
array([[1.    ],
       [1.0241],
       [3.457 ]])

Note

  • If T is a homogeneous transformation defining the pose of {B} with respect to {A}, then the points are defined with respect to frame {B} and are transformed to be with respect to frame {A}.

Seealso

e2h() h2e()

isR(R, tol=20)[source]

Test if matrix belongs to SO(n)

Parameters
  • R (ndarray(2,2) or ndarray(3,3)) – matrix to test

  • tol (float) – tolerance in units of eps, defaults to 20

Returns

whether matrix is a proper orthonormal rotation matrix

Return type

bool

Checks orthogonality, ie. \({\bf R} {\bf R}^T = {\bf I}\) and \(\det({\bf R}) > 0\). For the first test we check that the norm of the residual is less than tol * eps.

>>> from spatialmath.base import *
>>> isR(np.eye(3))
True
>>> isR(rot2(0.5))
True
>>> isR(np.zeros((3,3)))
False
Seealso

isrot2, isrot

iseye(S, tol=20)[source]

Test if matrix is identity

Parameters
  • S (ndarray(n,n)) – matrix to test

  • tol (float) – tolerance in units of eps, defaults to 20

Returns

whether matrix is a proper skew-symmetric matrix

Return type

bool

Check if matrix is an identity matrix. We check that the sum of the absolute value of the residual is less than tol * eps.

>>> from spatialmath.base import *
>>> import numpy as np
>>> iseye(np.array([[1,0], [0,1]]))
True
>>> iseye(np.array([[1,2], [0,1]]))
False
Seealso

isskew, isskewa

isskew(S, tol=20)[source]

Test if matrix belongs to so(n)

Parameters
  • S (ndarray(2,2) or ndarray(3,3)) – matrix to test

  • tol (float) – tolerance in units of eps, defaults to 20

Returns

whether matrix is a proper skew-symmetric matrix

Return type

bool

Checks skew-symmetry, ie. \({\bf S} + {\bf S}^T = {\bf 0}\). We check that the norm of the residual is less than tol * eps.

>>> from spatialmath.base import *
>>> import numpy as np
>>> isskew(np.zeros((3,3)))
True
>>> isskew(np.array([[0, -2], [2, 0]]))
True
>>> isskew(np.eye(3))
False
Seealso

isskewa

isskewa(S, tol=20)[source]

Test if matrix belongs to se(n)

Parameters
  • S (ndarray(3,3) or ndarray(4,4)) – matrix to test

  • tol (float) – tolerance in units of eps, defaults to 20

Returns

whether matrix is a proper skew-symmetric matrix

Return type

bool

Check if matrix is augmented skew-symmetric, ie. the top left (n-1xn-1) partition S is skew-symmetric \({\bf S} + {\bf S}^T = {\bf 0}\), and the bottom row is zero We check that the norm of the residual is less than tol * eps.

>>> from spatialmath.base import *
>>> import numpy as np
>>> isskewa(np.zeros((3,3)))
True
>>> isskewa(np.array([[0, -2], [2, 0]])) # this matrix is skew but not skewa
False
>>> isskewa(np.array([[0, -2, 5], [2, 0, 6], [0, 0, 0]]))
True
Seealso

isskew

r2t(R, check=False)[source]

Convert SO(n) to SE(n)

Parameters
  • R (ndarray(2,2) or ndarray(3,3)) – rotation matrix

  • check (bool) – check if rotation matrix is valid (default False, no check)

Returns

homogeneous transformation matrix

Return type

ndarray(3,3) or ndarray(4,4)

Raises

ValueError – bad argument

T = r2t(R) is an SE(2) or SE(3) homogeneous transform equivalent to an SO(2) or SO(3) orthonormal rotation matrix R with a zero translational component

  • if R is 2x2 then T is 3x3: SO(2) → SE(2)

  • if R is 3x3 then T is 4x4: SO(3) → SE(3)

>>> from spatialmath.base import *
>>> R = rot2(0.3)
>>> R
array([[ 0.9553, -0.2955],
       [ 0.2955,  0.9553]])
>>> r2t(R)
array([[ 0.9553, -0.2955,  0.    ],
       [ 0.2955,  0.9553,  0.    ],
       [ 0.    ,  0.    ,  1.    ]])
Seealso

t2r, rt2tr

rt2tr(R, t, check=False)[source]

Convert SO(n) and translation to SE(n)

Parameters
  • R (ndarray(2) or ndarray(3)) – SO(n) matrix

  • t – translation vector

  • check (bool) – check if SO(3) matrix is valid (default False, no check)

Returns

SE(3) matrix

Return type

ndarray(4,4) or (3,3)

Raises

ValueError – bad argument

T = rt2tr(R, t) is a homogeneous transformation matrix (N+1xN+1) formed from an orthonormal rotation matrix R (NxN) and a translation vector t (Nx1).

  • If R is 2x2 and t is 2x1, then T is 3x3

  • If R is 3x3 and t is 3x1, then T is 4x4

>>> from spatialmath.base import *
>>> R = rot2(0.3)
>>> t = [1, 2]
>>> rt2tr(R, t)
array([[ 0.9553, -0.2955,  1.    ],
       [ 0.2955,  0.9553,  2.    ],
       [ 0.    ,  0.    ,  1.    ]])
Seealso

rt2m, tr2rt, r2t

skew(v)[source]

Create skew-symmetric metrix from vector

Parameters

v (array_like(1) or array_like(3)) – vector

Returns

skew-symmetric matrix in so(2) or so(3)

Return type

ndarray(2,2) or ndarray(3,3)

Raises

ValueError – bad argument

skew(V) is a skew-symmetric matrix formed from the elements of V.

  • len(V) is 1 then S = \(\left[ \begin{array}{cc} 0 & -v \\ v & 0 \end{array} \right]\)

  • len(V) is 3 then S = \(\left[ \begin{array}{ccc} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0\end{array} \right]\)

>>> from spatialmath.base import *
>>> skew(2)
array([[ 0., -2.],
       [ 2.,  0.]])
>>> skew([1, 2, 3])
array([[ 0, -3,  2],
       [ 3,  0, -1],
       [-2,  1,  0]])

Note

  • This is the inverse of the function vex().

  • These are the generator matrices for the Lie algebras so(2) and so(3).

Seealso

vex() skewa()

SymPy

supported

skewa(v)[source]

Create augmented skew-symmetric metrix from vector

Parameters

v (array_like(3), array_like(6)) – vector

Returns

augmented skew-symmetric matrix in se(2) or se(3)

Return type

ndarray(3,3) or ndarray(4,4)

Raises

ValueError – bad argument

skewa(V) is an augmented skew-symmetric matrix formed from the elements of V.

  • len(V) is 3 then S = \(\left[ \begin{array}{ccc} 0 & -v_3 & v_1 \\ v_3 & 0 & v_2 \\ 0 & 0 & 0 \end{array} \right]\)

  • len(V) is 6 then S = \(\left[ \begin{array}{cccc} 0 & -v_6 & v_5 & v_1 \\ v_6 & 0 & -v_4 & v_2 \\ -v_5 & v_4 & 0 & v_3 \\ 0 & 0 & 0 & 0 \end{array} \right]\)

>>> from spatialmath.base import *
>>> skewa([1, 2, 3])
array([[ 0., -3.,  1.],
       [ 3.,  0.,  2.],
       [ 0.,  0.,  0.]])
>>> skewa([1, 2, 3, 4, 5, 6])
array([[ 0., -6.,  5.,  1.],
       [ 6.,  0., -4.,  2.],
       [-5.,  4.,  0.,  3.],
       [ 0.,  0.,  0.,  0.]])

Note

  • This is the inverse of the function vexa().

  • These are the generator matrices for the Lie algebras se(2) and se(3).

  • Map twist vectors in 2D and 3D space to se(2) and se(3).

Seealso

vexa() skew()

SymPy

supported

t2r(T, check=False)[source]

Convert SE(n) to SO(n)

Parameters
  • T (ndarray(3,3) or ndarray(4,4)) – homogeneous transformation matrix

  • check (bool) – check if rotation matrix is valid (default False, no check)

Returns

rotation matrix

Return type

ndarray(2,2) or ndarray(3,3)

Raises

ValueError – bad argument

R = T2R(T) is the orthonormal rotation matrix component of homogeneous transformation matrix T

  • if T is 3x3 then R is 2x2: SE(2) → SO(2)

  • if T is 4x4 then R is 3x3: SE(3) → SO(3)

>>> from spatialmath.base import *
>>> T = trot2(0.3, t=[1,2])
>>> T
array([[ 0.9553, -0.2955,  1.    ],
       [ 0.2955,  0.9553,  2.    ],
       [ 0.    ,  0.    ,  1.    ]])
>>> t2r(T)
array([[ 0.9553, -0.2955],
       [ 0.2955,  0.9553]])

Note

Any translational component of T is lost.

Seealso

r2t, tr2rt

tr2rt(T, check=False)[source]

Convert SE(n) to SO(n) and translation

Parameters
  • T (ndarray(3,3) or ndarray(4,4)) – SE(n) matrix

  • check (bool) – check if SO(3) submatrix is valid (default False, no check)

Returns

SO(n) matrix and translation vector

Return type

tuple: (ndarray(2,2), ndarray(2)) or (ndarray(3,3), ndarray(3))

Raises

ValueError – bad argument

(R,t) = tr2rt(T) splits a homogeneous transformation matrix (NxN) into an orthonormal rotation matrix R (MxM) and a translation vector T (Mx1), where N=M+1.

  • if T is 3x3 - in SE(2) - then R is 2x2 and t is 2x1.

  • if T is 4x4 - in SE(3) - then R is 3x3 and t is 3x1.

>>> from spatialmath.base import *
>>> T = trot2(0.3, t=[1,2])
>>> T
array([[ 0.9553, -0.2955,  1.    ],
       [ 0.2955,  0.9553,  2.    ],
       [ 0.    ,  0.    ,  1.    ]])
>>> R, t = tr2rt(T)
>>> R
array([[ 0.9553, -0.2955],
       [ 0.2955,  0.9553]])
>>> t
array([1., 2.])
Seealso

rt2tr, tr2r

vex(s, check=False)[source]

Convert skew-symmetric matrix to vector

Parameters
  • s (ndarray(2,2) or ndarray(3,3)) – skew-symmetric matrix

  • check (bool) – check if matrix is skew symmetric (default False, no check)

Returns

vector of unique values

Return type

ndarray(1) or ndarray(3)

Raises

ValueError – bad argument

vex(S) is the vector which has the corresponding skew-symmetric matrix S.

  • S is 2x2 - so(2) case - where S \(= \left[ \begin{array}{cc} 0 & -v \\ v & 0 \end{array} \right]\) then return \([v]\)

  • S is 3x3 - so(3) case - where S \(= \left[ \begin{array}{ccc} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0\end{array} \right]\) then return \([v_x, v_y, v_z]\).

>>> from spatialmath.base import *
>>> S = skew(2)
>>> print(S)
[[ 0. -2.]
 [ 2.  0.]]
>>> vex(S)
array([2.])
>>> S = skew([1, 2, 3])
>>> print(S)
[[ 0 -3  2]
 [ 3  0 -1]
 [-2  1  0]]
>>> vex(S)
array([1., 2., 3.])

Note

  • This is the inverse of the function skew().

  • Only rudimentary checking (zero diagonal) is done to ensure that the matrix is actually skew-symmetric.

  • The function takes the mean of the two elements that correspond to each unique element of the matrix.

Seealso

skew() vexa()

SymPy

supported

vexa(Omega, check=False)[source]

Convert skew-symmetric matrix to vector

Parameters
  • s (ndarray(3,3) or ndarray(4,4)) – augmented skew-symmetric matrix

  • check (bool) – check if matrix is skew symmetric part is valid (default False, no check)

Returns

vector of unique values

Return type

ndarray(3) or ndarray(6)

Raises

ValueError – bad argument

vexa(S) is the vector which has the corresponding augmented skew-symmetric matrix S.

  • S is 3x3 - se(2) case - where S \(= \left[ \begin{array}{ccc} 0 & -v_3 & v_1 \\ v_3 & 0 & v_2 \\ 0 & 0 & 0 \end{array} \right]\) then return \([v_1, v_2, v_3]\).

  • S is 4x4 - se(3) case - where S \(= \left[ \begin{array}{cccc} 0 & -v_6 & v_5 & v_1 \\ v_6 & 0 & -v_4 & v_2 \\ -v_5 & v_4 & 0 & v_3 \\ 0 & 0 & 0 & 0 \end{array} \right]\) then return \([v_1, v_2, v_3, v_4, v_5, v_6]\).

>>> from spatialmath.base import *
>>> S = skewa([1, 2, 3])
>>> print(S)
[[ 0. -3.  1.]
 [ 3.  0.  2.]
 [ 0.  0.  0.]]
>>> vexa(S)
array([1., 2., 3.])
>>> S = skewa([1, 2, 3, 4, 5, 6])
>>> print(S)
[[ 0. -6.  5.  1.]
 [ 6.  0. -4.  2.]
 [-5.  4.  0.  3.]
 [ 0.  0.  0.  0.]]
>>> vexa(S)
array([1., 2., 3., 4., 5., 6.])

Note

  • This is the inverse of the function skewa.

  • Only rudimentary checking (zero diagonal) is done to ensure that the matrix is actually skew-symmetric.

  • The function takes the mean of the two elements that correspond to each unique element of the matrix.

Seealso

skewa() vex()

SymPy

supported