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