def __init__(self, *args): """Initializes the orientation in any of the following ways: u, theta: (array_like, float) Axis-angle. Represents a single rotation by a given angle `theta` in radians about a fixed axis represented by the unit vector `u`. rot_vec: (array_like) Rotation vector. Corresponds to the 3-element representation of the axis-angle, i.e. `rot_vec = theta * u`. rotm: (array_like) a `3x3` orthogonal rotation matrix. quat: (Quaternion or UnitQuaternion) a unit quaternion. The quaternion is normalized if it's a `Quaternion` object. If no arguments are passed, an identity Orientation is initialized. """ if len(args) == 0: self._quat = quaternion.UnitQuaternion() elif len(args) == 1: if utils.is_iterable(args[0]): if len(args[0]) == 3 and utils.is_1d_iterable( args[0]): # rot_vec self._rot_vec = np.asarray(args[0], dtype="float64") self._rotvec2quat() elif len(args[0]) == 3 and np.asarray( args[0]).shape == (3, 3): # rotm rotm = np.asarray(args[0], dtype="float64") self._rotm2quat(rotm) else: raise ValueError( "[!] Could not parse constructor arguments.") elif isinstance(args[0], quaternion.Quaternion): # quat self._quat = quaternion.UnitQuaternion(args[0]) else: raise ValueError("[!] Expecting array_like or Quaternion.") elif len(args) == 2: if utils.is_iterable(args[0]) and isinstance( args[1], (int, float)): # axang if len(args[0]) != 3: raise ValueError("[!] Axis must be length 3 array_like.") self._u = np.asarray(args[0], dtype="float64") self._theta = float(args[1]) if np.isclose(self._theta, 0.0): # no unique axis, default to i self._u = np.array([1, 0, 0], dtype="float64") if not np.isclose(np.dot(self._u, self._u), 1.0): self._u /= np.linalg.norm(self._u) self._axang2quat() else: raise ValueError("[!] Expecting (axis, angle) tuple.") else: raise ValueError("[!] Incorrect number of arguments.")
def _rotm2quat(self, rotm): """Converts a rotation matrix to a unit-quaternion. """ m00 = rotm[0, 0] m01 = rotm[0, 1] m02 = rotm[0, 2] m10 = rotm[1, 0] m11 = rotm[1, 1] m12 = rotm[1, 2] m20 = rotm[2, 0] m21 = rotm[2, 1] m22 = rotm[2, 2] if is_proper_rotm(rotm): # Shepperd's algorithm if m22 >= 0: a = m00 + m11 b = m10 - m01 c = 1. + m22 if a >= 0: s = c + a q = [s, m21 - m12, m02 - m20, b] else: s = c - a q = [b, m02 + m20, m21 + m12, s] else: a = m00 - m11 b = m10 + m01 c = 1. - m22 if a >= 0: s = c + a q = [m21 - m12, s, b, m02 + m20] else: s = c - a q = [m02 - m20, b, s, m21 + m12] q = 0.5 * np.array(q) * (1. / np.sqrt(s)) else: # Bar-Itzhack's algorithm Q = np.array([ [m00 - m11 - m22, 0.0, 0.0, 0.0], [m01 + m10, m11 - m00 - m22, 0.0, 0.0], [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0], [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22], ]) Q /= 3. eig_values, eig_vectors = np.linalg.eigh(Q) q = eig_vectors[np.argmax(eig_values)][[3, 0, 1, 2]] if q[0] < 0.: q = -q self._quat = quaternion.UnitQuaternion(q)
def from_vecs(cls, u, v): """Computes the rotation that rotates vector `u` to vector `v`. Implements the algorithm described in [3]. Args: u: (array_like) the starting 3-D vector. v: (array_like) the final 3-D vector. """ norm_uv = np.sqrt(np.dot(u, u) * np.dot(v, v)) s = norm_uv + np.dot(u, v) if s < (1e-6 * norm_uv): # if u and v point in opposite directions # rotate 180 degrees about any orthogonal axis s = 0. if abs(u[0]) > abs(u[2]): v = np.array([-u[1], u[0], 0.]) else: v = np.array([0., -u[2], u[1]]) else: v = np.cross(u, v) return cls(quaternion.UnitQuaternion(s, v))
def _axang2quat(self): """Converts an axis-angle to a unit-quaternion. """ s = np.cos(self._theta / 2) v = self._u * np.sin(self._theta / 2) self._quat = quaternion.UnitQuaternion(s, v)