Python Quaternion.inverse方法代码示例

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import inverse [as 别名]
def fqa(msr):
    ax = msr['accel.x']
    ay = msr['accel.y']
    az = msr['accel.z']
    norm = math.sqrt(ax*ax + ay*ay + az*az)
    # estimation of elevation quaternion (around y axes)
    sin_theta_a = (ax / norm)
    cos_theta_a = math.sqrt(1 - sin_theta_a ** 2)
    sin_half_theta = math.copysign(1, sin_theta_a) * math.sqrt((1 - cos_theta_a) / 2)
    cos_half_theta = math.sqrt((1 + cos_theta_a) / 2)
    q_e = Quaternion(cos_half_theta, 0, sin_half_theta, 0)
    # estimation of roll quaternion (around x axis)
    sin_phi = (-ay / norm) / cos_theta_a
    cos_phi = (-az / norm) / cos_theta_a
    sin_half_phi = half_sin(sin_phi, cos_phi)
    cos_half_phi = half_cos(sin_phi, cos_phi)
# TODO singularity avoidance!!!
    q_r = Quaternion(cos_half_phi, sin_half_phi, 0, 0)
    # estimation of azimuth quaternion (around z axis)
    mx = msr['mag.x']
    my = msr['mag.y']
    mz = msr['mag.z']
    norm = math.sqrt(mx*mx + my*my + mz*mz)
    qm = Quaternion(0, mx / norm, my / norm, mz / norm)
    qm_a = q_e.multiply(q_r).multiply(qm).multiply(q_r.inverse()).multiply(q_e.inverse())
    Quaternion.normalize(qm_a)
    cos_zeta = qm_a.c
    sin_zeta = qm_a.b
    sin_half_zeta = half_sin(sin_zeta, cos_zeta)
    cos_half_zeta = half_cos(sin_zeta, cos_zeta)
    q_a = Quaternion(cos_half_zeta, 0, 0, sin_half_zeta)
    q_fqa = q_a.multiply(q_e).multiply(q_r)
    return q_fqa

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import inverse [as 别名]
    def test_inverse(self):
        q1 = Quaternion(randomElements())
        q2 = Quaternion.random()
        if q1:
            self.assertEqual(q1 * q1.inverse(), Quaternion(1.0, 0.0, 0.0, 0.0))
        else:
            with self.assertRaises(ZeroDivisionError):
                q1 * q1.inverse()

        self.assertEqual(q2 * q2.inverse(), Quaternion(1.0, 0.0, 0.0, 0.0))

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import inverse [as 别名]
class RigidTransform(object):
    def __init__(self, rotation_quat, translation_vec):
        self.quat = Quaternion(rotation_quat)
        self.tvec = numpy.array(translation_vec)

    def inverse(self):
        """ returns a new RigidTransform that corresponds to the inverse of this one """
        qinv = self.quat.inverse()
        return RigidTransform(qinv, qinv.rotate(- self.tvec))

    def interpolate(self, other_transform, this_weight):
        assert this_weight >= 0 and this_weight <= 1
        t = self.tvec * this_weight + other_transform.tvec * (1 - this_weight)
        r = self.quat.interpolate(other_transform.quat, this_weight)
        return RigidTransform(r, t)

    def __mul__(self, other):
        if isinstance(other, RigidTransform):
            t = self.quat.rotate(other.tvec) + self.tvec
            r = self.quat * other.quat
            return RigidTransform(r, t)
        else:
            olen = len(other)
            if olen == 3:
                r = numpy.array(self.quat.rotate(other))
                return r + self.tvec
            elif olen == 4:
                return np.dot(self.to_homogeneous_matrix(), other)
            else:
                raise ValueError()

    def to_homogeneous_matrix(self):
        result = self.quat.to_matrix_homogeneous()
	result.A[:3, 3] = self.tvec
        return result

    def to_roll_pitch_yaw_x_y_z(self):
        r, p, y = self.quat.to_roll_pitch_yaw()
        return numpy.array((r, p, y, self.tvec[0], self.tvec[1], self.tvec[2]))

    @staticmethod
    def from_roll_pitch_yaw_x_y_z(r, p, yaw, x, y, z):
        q = Quaternion.from_roll_pitch_yaw(r, p, yaw)
        return RigidTransform(q, (x, y, z))

    def quaternion(self):
        return self.quat

    def translation(self):
        return self.tvec

    @staticmethod
    def identity():
        return RigidTransform((1, 0, 0, 0), (0, 0, 0))

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import inverse [as 别名]
  def test_quaternion_inverse(self):
    '''Test that the quaternion inverse works.'''
    # First construct any random unit quaternion. Not uniform.
    s = 2*random.random() - 1.
    p1 = (2. - 2*np.abs(s))*random.random() - (1. - np.abs(s))
    p2 = ((2. - 2.*np.abs(s) - 2.*np.abs(p1))*random.random() - 
          (1. - np.abs(s) - np.abs(p1)))
    p3 = np.sqrt(1. - s**2 - p1**2 - p2**2)
    theta = Quaternion(np.array([s, p1, p2, p3]))

    theta_inv = theta.inverse()
    
    identity = theta*theta_inv
    self.assertAlmostEqual(identity.s, 1.0)
    self.assertAlmostEqual(identity.p[0], 0.0)
    self.assertAlmostEqual(identity.p[1], 0.0)
    self.assertAlmostEqual(identity.p[2], 0.0)

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import inverse [as 别名]
    def test_divide_by_scalar(self):
        a, b, c, d = randomElements()
        q1 = Quaternion(a, b, c, d)
        for s in [30.0, 0.3, -2, -4.7]:
            q2 = Quaternion(a/s, b/s, c/s, d/s)
            q3 = q1
            self.assertEqual(q1 / s, q2)
            if q1:
                self.assertEqual(s / q1, q2.inverse())
            else:
                with self.assertRaises(ZeroDivisionError):
                    s / q1

            q3 /= repr(s)
            self.assertEqual(q3, q2)

        with self.assertRaises(ZeroDivisionError):
            q4 = q1 / 0.0
        with self.assertRaises(TypeError):
            q4 = q1 / None
        with self.assertRaises(ValueError):
            q4 = q1 / 's'

# 需要导入模块: from quaternion import Quaternion [as 别名]
# 或者: from quaternion.Quaternion import inverse [as 别名]
  return np.array([
    [0., -x[0], -x[1], -x[2]],
    [x[0], 0., x[2], -x[1]],
    [x[1], -x[2], 0., x[0]],
    [x[2], x[1], -x[0], 0.]])

def ToLeftQuaternionProductMatrix(x):
  return np.array([
    [0., -x[0], -x[1], -x[2]],
    [x[0], 0., -x[2], x[1]],
    [x[1], x[2], 0., -x[0]],
    [x[2], -x[1], x[0], 0.]])

if __name__ == "__main__":
  q = Quaternion(1.,0.,0.,0.)
  print q.inverse()
  print q.toAxisAngle()
  q2 = Quaternion(1.,.01,0.,0.)
  q2.normalize()
  print q2
  print q2.inverse()
  print q2.inverse().dot(q2)
  print q2.dot(q2.inverse())
  print q.dot(q2)
  print q.angleTo(q2)
  raw_input()

  q2 = Quaternion(1.,1.,0.,0.)
  q2.fromAxisAngle(np.pi/2.0,np.array([0.,1.,1.]))

  q3 = q.slerp(q2,0.5)