Python transform.Rotation类代码示例

def test_as_euler_degenerate_asymmetric_axes():
    # Since we cannot check for angle equality, we check for dcm equality
    angles = np.array([
        [45, 90, 35],
        [35, -90, 20],
        [35, 90, 25],
        [25, -90, 15]
        ])

    with pytest.warns(UserWarning, match="Gimbal lock"):
        for seq_tuple in permutations('xyz'):
            # Extrinsic rotations
            seq = ''.join(seq_tuple)
            rotation = Rotation.from_euler(seq, angles, degrees=True)
            dcm_expected = rotation.as_dcm()

            angle_estimates = rotation.as_euler(seq, degrees=True)
            dcm_estimated = Rotation.from_euler(
                seq, angle_estimates, degrees=True
                ).as_dcm()

            assert_array_almost_equal(dcm_expected, dcm_estimated)

            # Intrinsic rotations
            seq = seq.upper()
            rotation = Rotation.from_euler(seq, angles, degrees=True)
            dcm_expected = rotation.as_dcm()

            angle_estimates = rotation.as_euler(seq, degrees=True)
            dcm_estimated = Rotation.from_euler(
                seq, angle_estimates, degrees=True
                ).as_dcm()

            assert_array_almost_equal(dcm_expected, dcm_estimated)

def test_apply_single_rotation_single_point():
    dcm = np.array([
        [0, -1, 0],
        [1, 0, 0],
        [0, 0, 1]
    ])
    r_1d = Rotation.from_dcm(dcm)
    r_2d = Rotation.from_dcm(np.expand_dims(dcm, axis=0))

    v_1d = np.array([1, 2, 3])
    v_2d = np.expand_dims(v_1d, axis=0)
    v1d_rotated = np.array([-2, 1, 3])
    v2d_rotated = np.expand_dims(v1d_rotated, axis=0)

    assert_allclose(r_1d.apply(v_1d), v1d_rotated)
    assert_allclose(r_1d.apply(v_2d), v2d_rotated)
    assert_allclose(r_2d.apply(v_1d), v2d_rotated)
    assert_allclose(r_2d.apply(v_2d), v2d_rotated)

    v1d_inverse = np.array([2, -1, 3])
    v2d_inverse = np.expand_dims(v1d_inverse, axis=0)

    assert_allclose(r_1d.apply(v_1d, inverse=True), v1d_inverse)
    assert_allclose(r_1d.apply(v_2d, inverse=True), v2d_inverse)
    assert_allclose(r_2d.apply(v_1d, inverse=True), v2d_inverse)
    assert_allclose(r_2d.apply(v_2d, inverse=True), v2d_inverse)

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)

def test_match_vectors_noise():
    np.random.seed(0)
    n_vectors = 100
    rot = Rotation.from_euler('xyz', np.random.normal(size=3))
    vectors = np.random.normal(size=(n_vectors, 3))
    result = rot.apply(vectors)

    # The paper adds noise as indepedently distributed angular errors
    sigma = np.deg2rad(1)
    tolerance = 1.5 * sigma
    noise = Rotation.from_rotvec(
        np.random.normal(
            size=(n_vectors, 3),
            scale=sigma
        )
    )

    # Attitude errors must preserve norm. Hence apply individual random
    # rotations to each vector.
    noisy_result = noise.apply(result)

    est, cov = Rotation.match_vectors(noisy_result, vectors)

    # Use rotation compositions to find out closeness
    error_vector = (rot * est.inv()).as_rotvec()
    assert_allclose(error_vector[0], 0, atol=tolerance)
    assert_allclose(error_vector[1], 0, atol=tolerance)
    assert_allclose(error_vector[2], 0, atol=tolerance)

    # Check error bounds using covariance matrix
    cov *= sigma
    assert_allclose(cov[0, 0], 0, atol=tolerance)
    assert_allclose(cov[1, 1], 0, atol=tolerance)
    assert_allclose(cov[2, 2], 0, atol=tolerance)

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)

def test_from_euler_rotation_order():
    # Intrinsic rotation is same as extrinsic with order reversed
    np.random.seed(0)
    a = np.random.randint(low=0, high=180, size=(6, 3))
    b = a[:, ::-1]
    x = Rotation.from_euler('xyz', a, degrees=True).as_quat()
    y = Rotation.from_euler('ZYX', b, degrees=True).as_quat()
    assert_allclose(x, y)

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

def test_from_dcm_calculation():
    expected_quat = np.array([1, 1, 6, 1]) / np.sqrt(39)
    dcm = np.array([
            [-0.8974359, -0.2564103, 0.3589744],
            [0.3589744, -0.8974359, 0.2564103],
            [0.2564103, 0.3589744, 0.8974359]
            ])
    assert_array_almost_equal(
            Rotation.from_dcm(dcm).as_quat(),
            expected_quat)
    assert_array_almost_equal(
            Rotation.from_dcm(dcm.reshape((1, 3, 3))).as_quat(),
            expected_quat.reshape((1, 4)))

def test_as_euler_asymmetric_axes():
    np.random.seed(0)
    n = 10
    angles = np.empty((n, 3))
    angles[:, 0] = np.random.uniform(low=-np.pi, high=np.pi, size=(n,))
    angles[:, 1] = np.random.uniform(low=-np.pi / 2, high=np.pi / 2, size=(n,))
    angles[:, 2] = np.random.uniform(low=-np.pi, high=np.pi, size=(n,))

    for seq_tuple in permutations('xyz'):
        # Extrinsic rotations
        seq = ''.join(seq_tuple)
        assert_allclose(angles, Rotation.from_euler(seq, angles).as_euler(seq))
        # Intrinsic rotations
        seq = seq.upper()
        assert_allclose(angles, Rotation.from_euler(seq, angles).as_euler(seq))

def test_from_euler_intrinsic_rotation_312():
    angles = [
        [30, 60, 45],
        [30, 60, 30],
        [45, 30, 60]
        ]
    dcm = Rotation.from_euler('ZXY', angles, degrees=True).as_dcm()

    assert_array_almost_equal(dcm[0], np.array([
        [0.3061862, -0.2500000, 0.9185587],
        [0.8838835, 0.4330127, -0.1767767],
        [-0.3535534, 0.8660254, 0.3535534]
    ]))

    assert_array_almost_equal(dcm[1], np.array([
        [0.5334936, -0.2500000, 0.8080127],
        [0.8080127, 0.4330127, -0.3995191],
        [-0.2500000, 0.8660254, 0.4330127]
    ]))

    assert_array_almost_equal(dcm[2], np.array([
        [0.0473672, -0.6123725, 0.7891491],
        [0.6597396, 0.6123725, 0.4355958],
        [-0.7500000, 0.5000000, 0.4330127]
    ]))

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)

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)

def test_spline_properties():
    times = np.array([0, 5, 15, 27])
    angles = [[-5, 10, 27], [3, 5, 38], [-12, 10, 25], [-15, 20, 11]]

    rotations = Rotation.from_euler('xyz', angles, degrees=True)
    spline = RotationSpline(times, rotations)

    assert_allclose(spline(times).as_euler('xyz', degrees=True), angles)
    assert_allclose(spline(0).as_euler('xyz', degrees=True), angles[0])

    h = 1e-8
    rv0 = spline(times).as_rotvec()
    rvm = spline(times - h).as_rotvec()
    rvp = spline(times + h).as_rotvec()
    assert_allclose(rv0, 0.5 * (rvp + rvm), rtol=1e-15)

    r0 = spline(times, 1)
    rm = spline(times - h, 1)
    rp = spline(times + h, 1)
    assert_allclose(r0, 0.5 * (rm + rp), rtol=1e-14)

    a0 = spline(times, 2)
    am = spline(times - h, 2)
    ap = spline(times + h, 2)
    assert_allclose(a0, am, rtol=1e-7)
    assert_allclose(a0, ap, rtol=1e-7)

def test02():

    geom = geom_tools.from_obj_file("/home/daiver/Downloads/R3DS_Wrap_3.3.17_Linux/Models/Basemeshes/WrapHand.obj")
    # geom = geom_tools.from_obj_file("/home/daiver/tmp.obj")
    print(f"Model loaded, "
          f"n vertices: {geom.n_vertices()}, "
          f"n vts: {geom.n_texture_vertices()}, "
          f"n polygons: {geom.n_polygons()}, "
          f"n triangles: {geom.n_triangles()}")

    np.set_printoptions(threshold=np.inf, linewidth=500)
    vertices_val = geom.vertices

    adjacency = adjacency_table_from_topology(geom.triangle_vertex_indices)

    target_to_base_indices = list(range(len(vertices_val)))
    targets_val = vertices_val.copy()
    rot_mat = Rotation.from_euler('z', 90, degrees=True).as_dcm()
    print(rot_mat)
    targets_val = targets_val @ rot_mat.T

    new_vertices = deform(
        vertices_val.T, adjacency,
        target_to_base_indices, targets_val.T
    )
    print(new_vertices)

def test_from_euler_extrinsic_rotation_313():
    angles = [
        [30, 60, 45],
        [30, 60, 30],
        [45, 30, 60]
        ]
    dcm = Rotation.from_euler('zxz', angles, degrees=True).as_dcm()

    assert_array_almost_equal(dcm[0], np.array([
        [0.43559574, -0.65973961, 0.61237244],
        [0.78914913, -0.04736717, -0.61237244],
        [0.4330127, 0.75000000, 0.500000]
    ]))

    assert_array_almost_equal(dcm[1], np.array([
        [0.62500000, -0.64951905, 0.4330127],
        [0.64951905, 0.12500000, -0.750000],
        [0.4330127, 0.75000000, 0.500000]
    ]))

    assert_array_almost_equal(dcm[2], np.array([
        [-0.1767767, -0.88388348, 0.4330127],
        [0.91855865, -0.30618622, -0.250000],
        [0.35355339, 0.35355339, 0.8660254]
    ]))