def test_transforms_from_pqs_4dims():
random_state = np.random.RandomState(0)
P = random_state.randn(2, 3, 4, 5, 7)
P[..., 3:] = norm_vectors(P[..., 3:])
H = transforms_from_pqs(P)
P2 = pqs_from_transforms(H)
assert_array_almost_equal(P[..., :3], H[..., :3, 3])
assert_array_almost_equal(P[..., :3], P2[..., :3])
def test_transforms_from_pqs_1dim():
P = np.empty((10, 7))
random_state = np.random.RandomState(0)
P[:, :3] = random_state.randn(len(P), 3)
P[:, 3:] = norm_vectors(random_state.randn(len(P), 4))
H = transforms_from_pqs(P)
P2 = pqs_from_transforms(H)
assert_array_almost_equal(P[:, :3], H[:, :3, 3])
assert_array_almost_equal(P[:, :3], P2[:, :3])
for t in range(len(P)):
assert_quaternion_equal(P[t, 3:], quaternion_from_matrix(H[t, :3, :3]))
assert_quaternion_equal(P[t, 3:], P2[t, 3:])