Esempio n. 1
0
    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.")
Esempio n. 2
0
 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)
Esempio n. 3
0
    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))
Esempio n. 4
0
 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)