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]