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