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 matrixR
(NxN) and a vectort
(Nx1). The bottom row is all zeros.If
A
is 2x2 andb
is 2x1, thenM
is 3x3If
A
is 3x3 andb
is 3x1, thenM
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 matrixm
.>>> from spatialmath.base import * >>> norm([3, 4]) 5.0
- Seealso
- 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 ofv
.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 transformationT
to the Euclidean points stored columnwise in the arrayp
.homtrans(T, v)
as above butv
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}.
- 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 thantol * 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 matrixR
with a zero translational componentif
R
is 2x2 thenT
is 3x3: SO(2) → SE(2)if
R
is 3x3 thenT
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 matrixR
(NxN) and a translation vectort
(Nx1).If
R
is 2x2 andt
is 2x1, thenT
is 3x3If
R
is 3x3 andt
is 3x1, thenT
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 ofV
.len(V)
is 1 thenS
= \(\left[ \begin{array}{cc} 0 & -v \\ v & 0 \end{array} \right]\)len(V)
is 3 thenS
= \(\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).
- 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 ofV
.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).
- 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 matrixT
if
T
is 3x3 thenR
is 2x2: SE(2) → SO(2)if
T
is 4x4 thenR
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) - thenR
is 2x2 andt
is 2x1.if
T
is 4x4 - in SE(3) - thenR
is 3x3 andt
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 matrixS
.S
is 2x2 - so(2) case - whereS
\(= \left[ \begin{array}{cc} 0 & -v \\ v & 0 \end{array} \right]\) then return \([v]\)S
is 3x3 - so(3) case - whereS
\(= \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.
- 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 matrixS
.S
is 3x3 - se(2) case - whereS
\(= \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 - whereS
\(= \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.