Python pyquaternion.Quaternion方法代码示例

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def inverse_subtract(self, initial_absolute, relative):
        size = relative.size()
        result = np.zeros(size)
        #the following for loops should be optimized
        for batch_id in range(size[0]):
            for seq_id in range(size[1]):
                for imu_id in range(size[2]):

                    if seq_id == 0:
                        q1 = Quaternion(
                            initial_absolute[batch_id, seq_id, imu_id])
                    else:
                        q1 = Quaternion(result[batch_id, seq_id - 1, imu_id])

                    diff = Quaternion(relative[batch_id, seq_id, imu_id])
                    q2 = (q1.normalised * diff.normalised).normalised.q
                    result[batch_id, seq_id, imu_id] = q2

        return result 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def get_angle_diff(self, target, result):
        size = target.size()
        sequence_length = size[1]
        all_averages = np.zeros((sequence_length)).astype(np.float)
        for seq_id in range(sequence_length):
            average = AverageMeter()
            for batch_id in range(size[0]):
                for imu_id in range(size[2]):
                    goal = Quaternion(target[batch_id, seq_id, imu_id])
                    out = Quaternion(result[batch_id, seq_id, imu_id])
                    acos = (2 * (np.dot(out.normalised.q, goal.normalised.q)**2)
                            - 1)
                    acos = round(acos, 6)
                    if acos > 1 or acos < -1:
                        pdb.set_trace()
                    radian = math.acos(acos)
                    average.update(radian)

            all_averages[seq_id] = (average.avg)

        return all_averages 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def augment(self, pc, chained_flowed, center):
        flip = np.random.uniform(0, 1) > 0.5
        if flip:
            pc = (pc - center)
            pc[:, 0] *= -1
            pc += center

            chained_flowed = (chained_flowed - center)
            chained_flowed[:, :, 0] *= -1
            chained_flowed += center

        scale = np.random.uniform(0.8, 1.2)
        pc = (pc - center) * scale + center
        chained_flowed = (chained_flowed - center) * scale + center

        rot_axis = np.array([0, 1, 0])
        rot_angle = np.random.uniform(np.pi * 2)
        q = Quaternion(axis=rot_axis, angle=rot_angle)
        R = q.rotation_matrix

        pc = np.dot(pc - center, R) + center
        chained_flowed = np.dot(chained_flowed - center, R) + center
        return pc, chained_flowed 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def augment(self, pc, center):
        flip = np.random.uniform(0, 1) > 0.5
        if flip:
            pc = (pc - center)
            pc[:, 0] *= -1
            pc += center

        scale = np.random.uniform(0.8, 1.2)
        pc = (pc - center) * scale + center

        rot_axis = np.array([0, 1, 0])
        rot_angle = np.random.uniform(np.pi * 2)
        q = Quaternion(axis=rot_axis, angle=rot_angle)
        R = q.rotation_matrix

        pc = np.dot(pc - center, R) + center
        return pc 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def __init__(self, path):
		self.images = []
		with open(path) as f:
			lines = f.readlines()
		self.length = int(re.match(r"# Number of images: (\d+),",lines[3]).groups()[0])
		for i in range(0, self.length):
			words = lines[4 + i * 2].split()
			id = int(words[0])
			extrinsic = Quaternion(float(words[1]), float(words[2]), float(words[3]), float(words[4])).transformation_matrix
			extrinsic[0,3] = float(words[5])
			extrinsic[1,3] = float(words[6])
			extrinsic[2,3] = float(words[7])
			camera_id = int(words[8])
			filename = words[9]
			point_list = PointList(lines[4 + i * 2 + 1])
			self.images.append(self.Image(id, extrinsic, camera_id, filename, point_list)) 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def build_bbox(pose, width, length, height):
    """
    Convert bounding box to label format 

    """
    R = np.array(
        [
            [np.cos(pose[3]), -np.sin(pose[3]), 0],
            [np.sin(pose[3]), np.cos(pose[3]), 0],
            [0, 0, 1],
        ]
    )
    q = Quaternion(matrix=R)
    return ObjectLabelRecord(
        quaternion=[q.scalar, q.vector[0], q.vector[1], q.vector[2]],
        translation=pose[0:3].copy(),
        length=length,
        width=width,
        height=height,
        occlusion=0,
    ) 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def test_init_from_elements(self):
        a, b, c, d = randomElements()
        q1 = Quaternion(a, b, c, d)
        q2 = Quaternion(repr(a), repr(b), repr(c), repr(d))
        q3 = Quaternion(a, repr(b), c, d)
        self.assertIsInstance(q1, Quaternion)
        self.assertIsInstance(q2, Quaternion)
        self.assertIsInstance(q3, Quaternion)
        self.assertTrue(np.array_equal(q1.q, [a, b, c, d]))
        self.assertEqual(q1, q2)
        self.assertEqual(q2, q3)
        with self.assertRaises(TypeError):
            q = Quaternion(None, b, c, d)
        with self.assertRaises(ValueError):
            q = Quaternion(a, b, "String", d)
        with self.assertRaises(ValueError):
            q = Quaternion(a, b, c)
        with self.assertRaises(ValueError):
            q = Quaternion(a, b, c, d, random()) 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def test_init_from_explicit_elements(self):
        e1, e2, e3, e4 = randomElements()
        q1 = Quaternion(w=e1, x=e2, y=e3, z=e4)
        q2 = Quaternion(a=e1, b=repr(e2), c=e3, d=e4)
        q3 = Quaternion(a=e1, i=e2, j=e3, k=e4)
        q4 = Quaternion(a=e1)
        self.assertIsInstance(q1, Quaternion)
        self.assertIsInstance(q2, Quaternion)
        self.assertIsInstance(q3, Quaternion)
        self.assertIsInstance(q4, Quaternion)
        self.assertEqual(q1, Quaternion(e1, e2, e3, e4))
        self.assertEqual(q1, q2)
        self.assertEqual(q2, q3)
        self.assertEqual(q4, Quaternion(e1))
        with self.assertRaises(TypeError):
            q = Quaternion(a=None, b=e2, c=e3, d=e4)
        with self.assertRaises(ValueError):
            q = Quaternion(a=e1, b=e2, c="String", d=e4)
        with self.assertRaises(ValueError):
            q = Quaternion(w=e1, x=e2)
        with self.assertRaises(ValueError):
            q = Quaternion(a=e1, b=e2, c=e3, d=e4, e=e1) 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def test_init_from_explicit_matrix_with_optional_tolerance_arguments(self):
        """
            The matrix defined in this test is orthogonal was carefully crafted
            such that it's orthogonal to a precision of 1e-07, but not to a precision
            of 1e-08. The default value for numpy's atol function is 1e-08, but
            developers should have the option to use a lower precision if they choose
            to.

            Reference: https://docs.scipy.org/doc/numpy/reference/generated/numpy.allclose.html
        """
        m = [[ 0.73297226, -0.16524626, -0.65988294, -0.07654548],
             [ 0.13108627,  0.98617666, -0.10135052, -0.04878795],
             [ 0.66750896, -0.01221443,  0.74450167, -0.05474513],
             [ 0,          0,          0,          1,        ]]
        npm = np.matrix(m)

        with self.assertRaises(ValueError):
            Quaternion(matrix=npm)

        try:
            Quaternion(matrix=npm, atol=1e-07)
        except ValueError:
            self.fail("Quaternion() raised ValueError unexpectedly!") 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def test_equality(self):
        r = randomElements()
        self.assertEqual(Quaternion(*r), Quaternion(*r))
        q = Quaternion(*r)
        self.assertEqual(q, q)
        # Equality should work with other types, if they can be interpreted as quaternions
        self.assertEqual(q, r)
        self.assertEqual(Quaternion(1., 0., 0., 0.), 1.0)
        self.assertEqual(Quaternion(1., 0., 0., 0.), "1.0")
        self.assertNotEqual(q, q + Quaternion(0.0, 0.002, 0.0, 0.0))

        # Equality should also cover small rounding and floating point errors
        self.assertEqual(Quaternion(1., 0., 0., 0.), Quaternion(1.0 - 1e-14, 0., 0., 0.))
        self.assertNotEqual(Quaternion(1., 0., 0., 0.), Quaternion(1.0 - 1e-12, 0., 0., 0.))
        self.assertNotEqual(Quaternion(160., 0., 0., 0.), Quaternion(160.0 - 1e-10, 0., 0., 0.))
        self.assertNotEqual(Quaternion(1600., 0., 0., 0.), Quaternion(1600.0 - 1e-9, 0., 0., 0.))

        with self.assertRaises(TypeError):
            q == None
        with self.assertRaises(ValueError):
            q == 's' 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def test_add(self):
        r1 = randomElements()
        r2 = randomElements()
        r  = random()
        n = None

        q1 = Quaternion(*r1)
        q2 = Quaternion(*r2)
        q3 = Quaternion(array= np.array(r1) + np.array(r2))
        q4 = Quaternion(array= np.array(r2) + np.array([r, 0.0, 0.0, 0.0]))
        self.assertEqual(q1 + q2, q3)
        q1 += q2
        self.assertEqual(q1, q3)
        self.assertEqual(q2 + r, q4)
        self.assertEqual(r + q2, q4)

        with self.assertRaises(TypeError):
            q1 += n
        with self.assertRaises(TypeError):
            n += q1 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def test_subtract(self):
        r1 = randomElements()
        r2 = randomElements()
        r  = random()
        n = None

        q1 = Quaternion(*r1)
        q2 = Quaternion(*r2)
        q3 = Quaternion(array= np.array(r1) - np.array(r2))
        q4 = Quaternion(array= np.array(r2) - np.array([r, 0.0, 0.0, 0.0]))
        self.assertEqual(q1 - q2, q3)
        q1 -= q2
        self.assertEqual(q1, q3)
        self.assertEqual(q2 - r, q4)
        self.assertEqual(r - q2, -q4)

        with self.assertRaises(TypeError):
            q1 -= n
        with self.assertRaises(TypeError):
            n -= q1 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def test_multiplication_of_bases(self):
        one = Quaternion(1.0, 0.0, 0.0, 0.0)
        i   = Quaternion(0.0, 1.0, 0.0, 0.0)
        j   = Quaternion(0.0, 0.0, 1.0, 0.0)
        k   = Quaternion(0.0, 0.0, 0.0, 1.0)

        self.assertEqual(i * i, j * j)
        self.assertEqual(j * j, k * k)
        self.assertEqual(k * k, i * j * k)
        self.assertEqual(i * j * k, -one)

        self.assertEqual(i * j, k)
        self.assertEqual(i * i, -one)
        self.assertEqual(i * k, -j)
        self.assertEqual(j * i, -k)
        self.assertEqual(j * j, -one)
        self.assertEqual(j * k, i)
        self.assertEqual(k * i, j)
        self.assertEqual(k * j, -i)
        self.assertEqual(k * k, -one)
        self.assertEqual(i * j * k, -one) 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def test_divide(self):
        r = randomElements()
        q = Quaternion(*r)
        if q:
            self.assertEqual(q / q, Quaternion())
            self.assertEqual(q / r, Quaternion())
        else:
            with self.assertRaises(ZeroDivisionError):
                q / q

        with self.assertRaises(ZeroDivisionError):
            q / Quaternion(0.0)
        with self.assertRaises(TypeError):
            q / None
        with self.assertRaises(ValueError):
            q / [1, 1, 1, 1, 1]
        with self.assertRaises(ValueError):
            q / np.array([[1, 2, 3], [4, 5, 6]])
        with self.assertRaises(ValueError):
            q / 's' 

# 需要导入模块: import pyquaternion [as 别名]
# 或者: from pyquaternion import Quaternion [as 别名]
def test_division_of_bases(self):
        one = Quaternion(1.0, 0.0, 0.0, 0.0)
        i   = Quaternion(0.0, 1.0, 0.0, 0.0)
        j   = Quaternion(0.0, 0.0, 1.0, 0.0)
        k   = Quaternion(0.0, 0.0, 0.0, 1.0)

        self.assertEqual(i / i, j / j)
        self.assertEqual(j / j, k / k)
        self.assertEqual(k / k, one)
        self.assertEqual(k / -k, -one)

        self.assertEqual(i / j, -k)
        self.assertEqual(i / i, one)
        self.assertEqual(i / k, j)
        self.assertEqual(j / i, k)
        self.assertEqual(j / j, one)
        self.assertEqual(j / k, -i)
        self.assertEqual(k / i, -j)
        self.assertEqual(k / j, i)
        self.assertEqual(k / k, one)
        self.assertEqual(i / -j, k)