Python quaternion.Quaternion类代码示例

 def test_quaternion_rot_matrix_det_one(self):
   ''' Test that the determinant of the rotation matrix is 1.'''
   for _ in range(10):
     theta = np.random.normal(0., 1., 4)
     theta = Quaternion(theta/np.linalg.norm(theta))
     R = theta.rotation_matrix()
     self.assertAlmostEqual(np.linalg.det(R), 1.0)

 def __init__(self, src=None, timestamp=None):
     if src is not None:
         Quaternion.__init__(self, src.w, src.x, src.y, src.z)
     else:
         Quaternion.__init__(self)
     self.timestamp = timestamp
     self.valid = True
     self.interpolated = False

def generate_quaternion():
	q1 = Quaternion.random()
	q2 = Quaternion.random()
	while True:
		for q in Quaternion.intermediates(q1, q2, 20, include_endpoints=True):
			yield q
		#q1, q2 = q2, q1
		q1 = q2
		q2 = Quaternion.random()

 def test_q_bar_matrix(self):
     a, b, c, d = randomElements()
     q = Quaternion(a, b, c, d)
     M = np.array([   
         [a, -b, -c, -d],
         [b,  a,  d, -c],
         [c, -d,  a,  b],
         [d,  c, -b,  a]])
     self.assertTrue(np.array_equal(q._q_bar_matrix(), M))

    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))

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

 def update(self, msr):
     t = msr['t']
     dt = t - self.t
     self.t = t
     w = Quaternion(0, msr['gyro.x'], msr['gyro.y'], msr['gyro.z'])
     q_fqa = fqa(msr)
     dq_static = q_fqa.add((self.q.scalar_multiply(-1))).scalar_multiply(1/dt)
     dq_dynamic = w.multiply(self.q).scalar_multiply(0.5)
     dq = dq_static.scalar_multiply(self.k).add(dq_dynamic.scalar_multiply(1 - self.k))
     self.q = self.q.add(dq.scalar_multiply(dt))
     self.q = Quaternion.normalize(self.q)

    def test_matrix_io(self):
        v = np.random.uniform(-100, 100, 3)

        for i in range(10):
            q0 = Quaternion.random()
            R  = q0.rotation_matrix()
            q1 = Quaternion(matrix=R)
            np.testing.assert_almost_equal(q0.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE)
            np.testing.assert_almost_equal(q0.rotate(v), q1.rotate(v), decimal=ALMOST_EQUAL_TOLERANCE)
            np.testing.assert_almost_equal(q1.rotate(v), np.dot(R, v), decimal=ALMOST_EQUAL_TOLERANCE)

            self.assertTrue((q0 == q1) or (q0 == -q1)) # q1 and -q1 are equivalent rotations

 def __init__(self, cx, cy, radius):
     """
     Initialize instance of ArcBall with no constraint on axis of rotation
     """
     self.center_x = cx
     self.center_y = cy
     self.radius = radius
     self.v_down = PVector()
     self.v_drag = PVector()
     self.q_now = Quaternion()
     self.q_down = Quaternion()
     self.q_drag = Quaternion() 
     self.axis_set = [PVector(1.0, 0.0, 0.0), PVector(0.0, 1.0, 0.0), PVector(0.0, 0.0, 1.0)]
     self.axis = -1  

    def get(self, requested_timestamp):
        if len(self.history) == 0:
            return None

        if requested_timestamp in self.history:
            match = self.history[requested_timestamp]
            return TimestampedQuaternion(match.w, match.x, match.y, match.z, requested_timestamp)
        else:

            keys = list(self.history.keys())
            keys.sort()

            if keys[0] > requested_timestamp or keys[-1] < requested_timestamp:
                return None

            for i, key in enumerate(keys):
                if key > requested_timestamp:
                    previous_timestamp = keys[i - 1]
                    next_timestamp = key
                    timestamp_delta = next_timestamp - previous_timestamp
                    requested_timestamp_offset = requested_timestamp - previous_timestamp
                    requested_timestamp_ratio = requested_timestamp_offset / timestamp_delta
                    interpolated_quaternion = Quaternion.slerp(self.history[previous_timestamp],
                                                               self.history[next_timestamp],
                                                               requested_timestamp_ratio)

                    match = TimestampedQuaternion(interpolated_quaternion.w,
                                                  interpolated_quaternion.x,
                                                  interpolated_quaternion.y,
                                                  interpolated_quaternion.z, requested_timestamp)
                    match.setInterpolated(True)
                    return match
            return None

    def test_differentiation(self):
        q = Quaternion.random()
        omega = np.random.uniform(-1, 1, 3) # Random angular velocity

        q_dash = 0.5 * q * Quaternion(vector=omega)

        self.assertEqual(q_dash, q.derivative(omega))

  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)

  def test_quaternion_rotation_angle(self):
    ''' Test generating rotation angle from quaternion. '''
    # 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]))

    rotation_angle = theta.rotation_angle()
    phi = Quaternion.from_rotation(rotation_angle)

    self.assertAlmostEqual(phi.s, theta.s)
    self.assertAlmostEqual(phi.p[0], theta.p[0])
    self.assertAlmostEqual(phi.p[1], theta.p[1])
    self.assertAlmostEqual(phi.p[2], theta.p[2])

class ComplimentaryFilter:
    def __init__(self, k=0.5):
        self.q = Quaternion(1, 0, 0, 0)
        self.t = 0
        self.k = k

    def update(self, msr):
        t = msr['t']
        dt = t - self.t
        self.t = t
        w = Quaternion(0, msr['gyro.x'], msr['gyro.y'], msr['gyro.z'])
        q_fqa = fqa(msr)
        dq_static = q_fqa.add((self.q.scalar_multiply(-1))).scalar_multiply(1/dt)
        dq_dynamic = w.multiply(self.q).scalar_multiply(0.5)
        dq = dq_static.scalar_multiply(self.k).add(dq_dynamic.scalar_multiply(1 - self.k))
        self.q = self.q.add(dq.scalar_multiply(dt))
        self.q = Quaternion.normalize(self.q)

    def test_interpolate(self):
        q1 = Quaternion(axis=[1, 0, 0], angle=0.0)
        q2 = Quaternion(axis=[1, 0, 0], angle=2*pi/3)
        num_intermediates = 3
        base = pi/6
        list1 = list(Quaternion.intermediates(q1, q2, num_intermediates, include_endpoints=False))
        list2 = list(Quaternion.intermediates(q1, q2, num_intermediates, include_endpoints=True))
        self.assertEqual(len(list1), num_intermediates)
        self.assertEqual(len(list2), num_intermediates+2)
        self.assertEqual(list1[0], list2[1])
        self.assertEqual(list1[1], list2[2])
        self.assertEqual(list1[2], list2[3])

        self.assertEqual(list2[0], q1)
        self.assertEqual(list2[1], Quaternion(axis=[1, 0, 0], angle=base))
        self.assertEqual(list2[2], Quaternion(axis=[1, 0, 0], angle=2*base))
        self.assertEqual(list2[3], Quaternion(axis=[1, 0, 0], angle=3*base))
        self.assertEqual(list2[4], q2)