def ishom2(T, check=False):
"""
Test if matrix belongs to SE(2)
:param T: SE(2) matrix to test
:type T: ndarray(3,3)
:param check: check validity of rotation submatrix
:type check: bool
:return: whether matrix is an SE(2) homogeneous transformation matrix
:rtype: bool
- ``ishom2(T)`` is True if the argument ``T`` is of dimension 3x3
- ``ishom2(T, check=True)`` as above, but also checks orthogonality of the
rotation sub-matrix and validitity of the bottom row.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> import numpy as np
>>> T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]])
>>> ishom2(T)
>>> T = np.array([[1, 1, 3], [0, 1, 4], [0, 0, 1]]) # invalid SE(2)
>>> ishom2(T) # a quick check says it is an SE(2)
>>> ishom2(T, check=True) # but if we check more carefully...
>>> R = np.array([[1, 0], [0, 1]])
>>> ishom2(R)
:seealso: isR, isrot2, ishom, isvec
"""
return isinstance(T, np.ndarray) and T.shape == (3, 3) \
and (not check or (base.isR(T[:2, :2])
and np.all(T[2, :] == np.array([0, 0, 1]))))
def isrot2(R, check=False):
"""
Test if matrix belongs to SO(2)
:param R: SO(2) matrix to test
:type R: ndarray(3,3)
:param check: check validity of rotation submatrix
:type check: bool
:return: whether matrix is an SO(2) rotation matrix
:rtype: bool
- ``isrot2(R)`` is True if the argument ``R`` is of dimension 2x2
- ``isrot2(R, check=True)`` as above, but also checks orthogonality of the rotation matrix.
.. runblock:: pycon
>>> from spatialmath.base import *
>>> import numpy as np
>>> T = np.array([[1, 0, 3], [0, 1, 4], [0, 0, 1]])
>>> isrot2(T)
>>> R = np.array([[1, 0], [0, 1]])
>>> isrot2(R)
>>> R = np.array([[1, 1], [0, 1]]) # invalid SO(2)
>>> isrot2(R) # a quick check says it is an SO(2)
>>> isrot2(R, check=True) # but if we check more carefully...
:seealso: isR, ishom2, isrot
"""
return isinstance(R, np.ndarray) and R.shape == (2, 2) \
and (not check or base.isR(R))
def r2q(R, check=True):
"""
Convert SO(3) rotation matrix to unit-quaternion
:arg R: rotation matrix
:type R: numpy.ndarray, shape=(3,3)
:return: unit-quaternion
:rtype: numpy.ndarray, shape=(3,)
Returns a unit-quaternion corresponding to the input SO(3) rotation matrix.
.. warning:: There is no check that the passed matrix is a valid rotation matrix.
:seealso: q2r
"""
assert R.shape == (3,
3) and tr.isR(R), "Argument must be 3x3 rotation matrix"
qs = math.sqrt(np.trace(R) + 1) / 2.0
kx = R[2, 1] - R[1, 2] # Oz - Ay
ky = R[0, 2] - R[2, 0] # Ax - Nz
kz = R[1, 0] - R[0, 1] # Ny - Ox
if (R[0, 0] >= R[1, 1]) and (R[0, 0] >= R[2, 2]):
kx1 = R[0, 0] - R[1, 1] - R[2, 2] + 1 # Nx - Oy - Az + 1
ky1 = R[1, 0] + R[0, 1] # Ny + Ox
kz1 = R[2, 0] + R[0, 2] # Nz + Ax
add = (kx >= 0)
elif R[1, 1] >= R[2, 2]:
kx1 = R[1, 0] + R[0, 1] # Ny + Ox
ky1 = R[1, 1] - R[0, 0] - R[2, 2] + 1 # Oy - Nx - Az + 1
kz1 = R[2, 1] + R[1, 2] # Oz + Ay
add = (ky >= 0)
else:
kx1 = R[2, 0] + R[0, 2] # Nz + Ax
ky1 = R[2, 1] + R[1, 2] # Oz + Ay
kz1 = R[2, 2] - R[0, 0] - R[1, 1] + 1 # Az - Nx - Oy + 1
add = (kz >= 0)
if add:
kx = kx + kx1
ky = ky + ky1
kz = kz + kz1
else:
kx = kx - kx1
ky = ky - ky1
kz = kz - kz1
kv = np.r_[kx, ky, kz]
nm = np.linalg.norm(kv)
if abs(nm) < 100 * _eps:
return eye()
else:
return np.r_[qs, (math.sqrt(1.0 - qs**2) / nm) * kv]
