def test_quaternions_from_matrices_no_batch():
random_state = np.random.RandomState(85)
for _ in range(5):
q = pr.random_quaternion(random_state)
R = pr.matrix_from_quaternion(q)
q2 = pbr.quaternions_from_matrices(R)
pr.assert_quaternion_equal(q, q2)
a = np.array([1.0, 0.0, 0.0, np.pi])
q = pr.quaternion_from_axis_angle(a)
R = pr.matrix_from_axis_angle(a)
q_from_R = pbr.quaternions_from_matrices(R)
assert_array_almost_equal(q, q_from_R)
a = np.array([0.0, 1.0, 0.0, np.pi])
q = pr.quaternion_from_axis_angle(a)
R = pr.matrix_from_axis_angle(a)
q_from_R = pbr.quaternions_from_matrices(R)
assert_array_almost_equal(q, q_from_R)
a = np.array([0.0, 0.0, 1.0, np.pi])
q = pr.quaternion_from_axis_angle(a)
R = pr.matrix_from_axis_angle(a)
q_from_R = pbr.quaternions_from_matrices(R)
assert_array_almost_equal(q, q_from_R)
def test_dual_quaternions_from_pqs_0dims():
random_state = np.random.RandomState(844)
pq = random_state.randn(7)
pq[3:] /= np.linalg.norm(pq[3:], axis=-1)[..., np.newaxis]
dq = dual_quaternions_from_pqs(pq)
pq2 = pqs_from_dual_quaternions(dq)
assert_array_almost_equal(pq[:3], pq2[:3])
assert_quaternion_equal(pq[3:], pq2[3:])
def test_dual_quaternions_from_pqs_2dims():
random_state = np.random.RandomState(846)
pqs = random_state.randn(5, 5, 7)
pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis]
dqs = dual_quaternions_from_pqs(pqs)
pqs2 = pqs_from_dual_quaternions(dqs)
for pq, pq2 in zip(pqs.reshape(-1, 7), pqs2.reshape(-1, 7)):
assert_array_almost_equal(pq[:3], pq2[:3])
assert_quaternion_equal(pq[3:], pq2[3:])
def test_dual_quaternions_from_pqs_1dim():
random_state = np.random.RandomState(845)
pqs = random_state.randn(20, 7)
pqs[..., 3:] /= np.linalg.norm(pqs[..., 3:], axis=-1)[..., np.newaxis]
dqs = dual_quaternions_from_pqs(pqs)
pqs2 = pqs_from_dual_quaternions(dqs)
for pq, pq2 in zip(pqs, pqs2):
assert_array_almost_equal(pq[:3], pq2[:3])
assert_quaternion_equal(pq[3:], pq2[3:])
def test_quaternion_slerp_batch():
random_state = np.random.RandomState(229)
q_start = pr.random_quaternion(random_state)
q_end = pr.random_quaternion(random_state)
t = np.linspace(0, 1, 101)
Q = pbr.quaternion_slerp_batch(q_start, q_end, t)
for i in range(len(t)):
qi = pr.quaternion_slerp(q_start, q_end, t[i])
pr.assert_quaternion_equal(Q[i], qi)
def test_matrices_from_pos_quat():
"""Test conversion from positions and quaternions to matrices."""
P = np.empty((10, 7))
random_state = np.random.RandomState(0)
P[:, :3] = random_state.randn(10, 3)
for t in range(10):
P[t, 3:] = random_quaternion(random_state)
H = matrices_from_pos_quat(P)
for t in range(10):
assert_array_almost_equal(P[t, :3], H[t, :3, 3])
assert_quaternion_equal(P[t, 3:], quaternion_from_matrix(H[t, :3, :3]))
def test_matrices_from_quaternions():
random_state = np.random.RandomState(83)
for _ in range(5):
q = pr.random_quaternion(random_state)
R = pbr.matrices_from_quaternions([q], normalize_quaternions=False)[0]
q2 = pr.quaternion_from_matrix(R)
pr.assert_quaternion_equal(q, q2)
for _ in range(5):
q = random_state.randn(4)
R = pbr.matrices_from_quaternions([q], normalize_quaternions=True)[0]
q2 = pr.quaternion_from_matrix(R)
pr.assert_quaternion_equal(q / np.linalg.norm(q), q2)
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:])
def test_quaternions_from_matrices_4d():
random_state = np.random.RandomState(84)
for _ in range(5):
q = pr.random_quaternion(random_state)
R = pr.matrix_from_quaternion(q)
q2 = pbr.quaternions_from_matrices([[R, R], [R, R]])
pr.assert_quaternion_equal(q, q2[0, 0])
pr.assert_quaternion_equal(q, q2[0, 1])
pr.assert_quaternion_equal(q, q2[1, 0])
pr.assert_quaternion_equal(q, q2[1, 1])
def test_quaternion_slerp_batch_zero_angle():
random_state = np.random.RandomState(228)
q = pr.random_quaternion(random_state)
Q = pbr.quaternion_slerp_batch(q, q, [0.5])
pr.assert_quaternion_equal(q, Q[0])
