Python Rotation.from_quat方法代码示例

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_inv_single_rotation():
    np.random.seed(0)
    p = Rotation.from_quat(np.random.normal(size=(4,)))
    q = p.inv()

    p_dcm = p.as_dcm()
    q_dcm = q.as_dcm()
    res1 = np.dot(p_dcm, q_dcm)
    res2 = np.dot(q_dcm, p_dcm)

    eye = np.eye(3)

    assert_array_almost_equal(res1, eye)
    assert_array_almost_equal(res2, eye)

    x = Rotation.from_quat(np.random.normal(size=(1, 4)))
    y = x.inv()

    x_dcm = x.as_dcm()
    y_dcm = y.as_dcm()
    result1 = np.einsum('...ij,...jk->...ik', x_dcm, y_dcm)
    result2 = np.einsum('...ij,...jk->...ik', y_dcm, x_dcm)

    eye3d = np.empty((1, 3, 3))
    eye3d[:] = np.eye(3)

    assert_array_almost_equal(result1, eye3d)
    assert_array_almost_equal(result2, eye3d)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_zero_norms_from_quat():
    x = np.array([
            [3, 4, 0, 0],
            [0, 0, 0, 0],
            [5, 0, 12, 0]
            ])
    with pytest.raises(ValueError):
        Rotation.from_quat(x)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_as_dcm_from_square_input():
    quats = [
            [0, 0, 1, 1],
            [0, 1, 0, 1],
            [0, 0, 0, 1],
            [0, 0, 0, -1]
            ]
    mat = Rotation.from_quat(quats).as_dcm()
    assert_equal(mat.shape, (4, 3, 3))

    expected0 = np.array([
        [0, -1, 0],
        [1, 0, 0],
        [0, 0, 1]
        ])
    assert_array_almost_equal(mat[0], expected0)

    expected1 = np.array([
        [0, 0, 1],
        [0, 1, 0],
        [-1, 0, 0]
        ])
    assert_array_almost_equal(mat[1], expected1)

    assert_array_almost_equal(mat[2], np.eye(3))
    assert_array_almost_equal(mat[3], np.eye(3))

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_slerp_num_rotations_mismatch():
    with pytest.raises(ValueError, match="number of rotations to be equal to "
                                         "number of timestamps"):
        np.random.seed(0)
        r = Rotation.from_quat(np.random.uniform(size=(5, 4)))
        t = np.arange(7)
        Slerp(t, r)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_as_dcm_from_generic_input():
    quats = [
            [0, 0, 1, 1],
            [0, 1, 0, 1],
            [1, 2, 3, 4]
            ]
    mat = Rotation.from_quat(quats).as_dcm()
    assert_equal(mat.shape, (3, 3, 3))

    expected0 = np.array([
        [0, -1, 0],
        [1, 0, 0],
        [0, 0, 1]
        ])
    assert_array_almost_equal(mat[0], expected0)

    expected1 = np.array([
        [0, 0, 1],
        [0, 1, 0],
        [-1, 0, 0]
        ])
    assert_array_almost_equal(mat[1], expected1)

    expected2 = np.array([
        [0.4, -2, 2.2],
        [2.8, 1, 0.4],
        [-1, 2, 2]
        ]) / 3
    assert_array_almost_equal(mat[2], expected2)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_match_vectors_no_noise():
    np.random.seed(0)
    c = Rotation.from_quat(np.random.normal(size=4))
    b = np.random.normal(size=(5, 3))
    a = c.apply(b)

    est, cov = Rotation.match_vectors(a, b)
    assert_allclose(c.as_quat(), est.as_quat())

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_as_rotvec_single_2d_input():
    quat = np.array([[1, 2, -3, 2]])
    expected_rotvec = np.array([[0.5772381, 1.1544763, -1.7317144]])

    actual_rotvec = Rotation.from_quat(quat).as_rotvec()

    assert_equal(actual_rotvec.shape, (1, 3))
    assert_allclose(actual_rotvec, expected_rotvec)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_slerp_time_dim_mismatch():
    with pytest.raises(ValueError,
                       match="times to be specified in a 1 dimensional array"):
        np.random.seed(0)
        r = Rotation.from_quat(np.random.uniform(size=(2, 4)))
        t = np.array([[1],
                      [2]])
        Slerp(t, r)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_slerp_call_time_out_of_range():
    np.random.seed(0)
    r = Rotation.from_quat(np.random.uniform(size=(5, 4)))
    t = np.arange(5) + 1
    s = Slerp(t, r)

    with pytest.raises(ValueError, match="times must be within the range"):
        s([0, 1, 2])
    with pytest.raises(ValueError, match="times must be within the range"):
        s([1, 2, 6])

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_as_dcm_single_2d_quaternion():
    quat = [[0, 0, 1, 1]]
    mat = Rotation.from_quat(quat).as_dcm()
    assert_equal(mat.shape, (1, 3, 3))
    expected_mat = np.array([
        [0, -1, 0],
        [1, 0, 0],
        [0, 0, 1]
        ])
    assert_array_almost_equal(mat[0], expected_mat)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_slerp_call_time_dim_mismatch():
    np.random.seed(0)
    r = Rotation.from_quat(np.random.uniform(size=(5, 4)))
    t = np.arange(5)
    s = Slerp(t, r)

    with pytest.raises(ValueError,
                       match="times to be specified in a 1 dimensional array"):
        interp_times = np.array([[3.5],
                                 [4.2]])
        s(interp_times)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_from_square_quat_matrix():
    # Ensure proper norm array broadcasting
    x = np.array([
        [3, 0, 0, 4],
        [5, 0, 12, 0],
        [0, 0, 0, 1],
        [0, 0, 0, -1]
        ])
    r = Rotation.from_quat(x)
    expected_quat = x / np.array([[5], [13], [1], [1]])
    assert_array_almost_equal(r.as_quat(), expected_quat)

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_quat_ownership():
    # Ensure that users cannot accidentally corrupt object
    quat = np.array([
        [1, 0, 0, 0],
        [0, 1, 0, 0],
        [0, 0, 1, 0]
    ])
    r = Rotation.from_quat(quat, normalized=True)
    s = r[0:2]

    r._quat[0] = np.array([0, -1, 0, 0])
    assert_allclose(s._quat[0], np.array([1, 0, 0, 0]))

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_as_generic_rotvec():
    quat = np.array([
            [1, 2, -1, 0.5],
            [1, -1, 1, 0.0003],
            [0, 0, 0, 1]
            ])
    quat /= np.linalg.norm(quat, axis=1)[:, None]

    rotvec = Rotation.from_quat(quat).as_rotvec()
    angle = np.linalg.norm(rotvec, axis=1)

    assert_allclose(quat[:, 3], np.cos(angle/2))
    assert_allclose(np.cross(rotvec, quat[:, :3]), np.zeros((3, 3)))

# 需要导入模块: from scipy.spatial.transform import Rotation [as 别名]
# 或者: from scipy.spatial.transform.Rotation import from_quat [as 别名]
def test_inv():
    np.random.seed(0)
    n = 10
    p = Rotation.from_quat(np.random.normal(size=(n, 4)))
    q = p.inv()

    p_dcm = p.as_dcm()
    q_dcm = q.as_dcm()
    result1 = np.einsum('...ij,...jk->...ik', p_dcm, q_dcm)
    result2 = np.einsum('...ij,...jk->...ik', q_dcm, p_dcm)

    eye3d = np.empty((n, 3, 3))
    eye3d[:] = np.eye(3)

    assert_array_almost_equal(result1, eye3d)
    assert_array_almost_equal(result2, eye3d)