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_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_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_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_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_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_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_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]
]))
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_apply_multiple_rotations_single_point():
dcm = np.empty((2, 3, 3))
dcm[0] = np.array([
[0, -1, 0],
[1, 0, 0],
[0, 0, 1]
])
dcm[1] = np.array([
[1, 0, 0],
[0, 0, -1],
[0, 1, 0]
])
r = Rotation.from_dcm(dcm)
v1 = np.array([1, 2, 3])
v2 = np.expand_dims(v1, axis=0)
v_rotated = np.array([[-2, 1, 3], [1, -3, 2]])
assert_allclose(r.apply(v1), v_rotated)
assert_allclose(r.apply(v2), v_rotated)
v_inverse = np.array([[2, -1, 3], [1, 3, -2]])
assert_allclose(r.apply(v1, inverse=True), v_inverse)
assert_allclose(r.apply(v2, inverse=True), v_inverse)
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))
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_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 test01():
np.set_printoptions(threshold=np.inf, linewidth=500)
vertices_val = np.array([
[0, 0, 1],
[1, 0, 0],
[0, 1, 0],
[-1, 0, 0],
[0, -1, 0],
], dtype=np.float32)
adjacency = [
[1, 2, 3, 4],
[0, 2, 4],
[0, 1, 3],
[0, 2, 4],
[0, 1, 3]
]
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_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_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)
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)
def test_rotvec_calc_pipeline():
# Include small angles
rotvec = np.array([
[0, 0, 0],
[1, -1, 2],
[-3e-4, 3.5e-4, 7.5e-5]
])
assert_allclose(Rotation.from_rotvec(rotvec).as_rotvec(), rotvec)
def test_from_euler_elementary_extrinsic_rotation():
# Simple test to check if extrinsic rotations are implemented correctly
dcm = Rotation.from_euler('zx', [90, 90], degrees=True).as_dcm()
expected_dcm = np.array([
[0, -1, 0],
[0, 0, -1],
[1, 0, 0]
])
assert_array_almost_equal(dcm, expected_dcm)
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])
def test_apply_single_rotation_multiple_points():
dcm = np.array([
[0, -1, 0],
[1, 0, 0],
[0, 0, 1]
])
r1 = Rotation.from_dcm(dcm)
r2 = Rotation.from_dcm(np.expand_dims(dcm, axis=0))
v = np.array([[1, 2, 3], [4, 5, 6]])
v_rotated = np.array([[-2, 1, 3], [-5, 4, 6]])
assert_allclose(r1.apply(v), v_rotated)
assert_allclose(r2.apply(v), v_rotated)
v_inverse = np.array([[2, -1, 3], [5, -4, 6]])
assert_allclose(r1.apply(v, inverse=True), v_inverse)
assert_allclose(r2.apply(v, inverse=True), v_inverse)
def test_from_single_2d_dcm():
dcm = [
[0, 0, 1],
[1, 0, 0],
[0, 1, 0]
]
expected_quat = [0.5, 0.5, 0.5, 0.5]
assert_array_almost_equal(
Rotation.from_dcm(dcm).as_quat(),
expected_quat)
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)
def test_from_single_3d_dcm():
dcm = np.array([
[0, 0, 1],
[1, 0, 0],
[0, 1, 0]
]).reshape((1, 3, 3))
expected_quat = np.array([0.5, 0.5, 0.5, 0.5]).reshape((1, 4))
assert_array_almost_equal(
Rotation.from_dcm(dcm).as_quat(),
expected_quat)
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)
def test_as_euler_symmetric_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=0, high=np.pi, size=(n,))
angles[:, 2] = np.random.uniform(low=-np.pi, high=np.pi, size=(n,))
for axis1 in ['x', 'y', 'z']:
for axis2 in ['x', 'y', 'z']:
if axis1 == axis2:
continue
# Extrinsic rotations
seq = axis1 + axis2 + axis1
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_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)
def test_from_single_2d_matrix():
mat = [[0, 0, 1], [1, 0, 0], [0, 1, 0]]
expected_quat = [0.5, 0.5, 0.5, 0.5]
assert_array_almost_equal(
Rotation.from_matrix(mat).as_quat(), expected_quat)
def test_slerp_single_rot():
with pytest.raises(ValueError, match="must be a sequence of rotations"):
r = Rotation.from_quat([1, 2, 3, 4])
Slerp([1], r)
def test_malformed_1d_from_rotvec():
with pytest.raises(ValueError, match='Expected `rot_vec` to have shape'):
Rotation.from_rotvec([1, 2])
def rotate_axis(inp):
hacky_trans_matrix = R.from_euler('xyz', [1.57, -1.57, 0]).as_dcm()
hacky_trans_matrix = np.concatenate((hacky_trans_matrix, np.zeros(3)[:, np.newaxis]), axis=1)
hacky_trans_matrix = np.concatenate((hacky_trans_matrix, np.array([[0, 0, 0, 1]])), axis=0)
return np.dot(hacky_trans_matrix, inp.transpose())
def test_malformed_1d_from_mrp():
with pytest.raises(ValueError, match='Expected `mrp` to have shape'):
Rotation.from_mrp([1, 2])
def rotation_ply(filename):
with open('point_cloud/' + filename,'r') as input_file:
header_cnt = 8
cnt = 0
all_lines = input_file.readlines()
#Get header and data
header = all_lines[:header_cnt]
split_line = header[3].strip().split(' ')
vertex_cnt = int(split_line[2])
data = all_lines[header_cnt:(vertex_cnt + header_cnt)]
point_cloud = []
# Random sampling
replace = True if vertex_cnt < 20000 else False
sampled_int = np.random.choice(vertex_cnt, 20000, replace=replace)
if vertex_cnt < 20000:
vertex_cnt = vertex_cnt
else:
vertex_cnt = 20000
header[3] = ' '.join(split_line[:2] + [str(vertex_cnt) + '\n']) #We will randomly choose 20000 points, so change the point cloud info
#Get point cloud
if vertex_cnt >= 20000:
idx = -1
for line in data:
idx += 1
if idx not in sampled_int:
continue
line_split = line.strip().split(' ')
new_line = []
for item in line_split:
new_line.append(float(item))
point_cloud.append(new_line)
else:
for line in data:
line_split = line.strip().split(' ')
new_line = []
for item in line_split:
new_line.append(float(item))
point_cloud.append(new_line)
#Apply rotation by 90 degree along x-axis
axis = [1,0,0]
axis = axis / norm(axis)
theta = math.pi/2
rot = Rotation.from_rotvec(theta * axis)
new_point_cloud = rot.apply(point_cloud)
# Saving point cloud
out_filename = 'rotated_pc.ply'
with open('point_cloud/' + out_filename,'w') as output_file:
output_file.writelines(header)
for line in new_point_cloud.tolist():
print_line = ''
for item in line:
print_line += "{:.5f}".format(item) + ' '
print_line += '\n'
output_file.write(print_line)
print('Rotation completed')
return out_filename
def test_from_single_2d_quaternion():
x = np.array([[3, 4, 0, 0]])
r = Rotation.from_quat(x)
expected_quat = x / 5
assert_array_almost_equal(r.as_quat(), expected_quat)
def convert_quat_to_rpy(self, w, x, y, z):
r = Rotation.from_quat([x, y, z, w])
return r.as_euler('xyz', degrees=True)
def test_from_2d_single_mrp():
mrp = [[0, 0, 1.0]]
expected_quat = np.array([[0, 0, 1, 0]])
result = Rotation.from_mrp(mrp)
assert_array_almost_equal(result.as_quat(), expected_quat)
def test_concatenate_wrong_type():
with pytest.raises(TypeError, match='Rotation objects only'):
Rotation.concatenate([Rotation.identity(), 1, None])
def test_matrix_calculation_pipeline():
mat = special_ortho_group.rvs(3, size=10, random_state=0)
assert_array_almost_equal(Rotation.from_matrix(mat).as_matrix(), mat)
def test_generic_quat_matrix():
x = np.array([[3, 4, 0, 0], [5, 12, 0, 0]])
r = Rotation.from_quat(x)
expected_quat = x / np.array([[5], [13]])
assert_array_almost_equal(r.as_quat(), expected_quat)
def make_arrow(name, midpoint, length=1, radius=0.2, rotation=(0,0,0), radius_ratio=3):
"""
Make an arrowbased on the centre position and rotation
Parameters
----------
name : str
Name of arrow
midpoint : array (3,)
Position in 3D space of arrow
length : float
Length of arrow
radius : float
Radius of arrow cylinder
rotation : array (3,)
Rotation of arrow in Euler XYZ rotation
radius_ratio : float
Ratio of the arrow cylinder to arrow head widths
"""
strut = bpy.ops.mesh.primitive_cylinder_add(
vertices=16,
radius=radius,
depth=length,
location=(float(midpoint[0]),float(midpoint[1]),float(midpoint[2])),
rotation=np.radians(rotation)
)
obj1 = bpy.context.object
m1 = obj1.data
obj1.name = f"strut_{name}"
# Make rotator and get the position of the cone
rotator = R.from_euler("xyz", rotation, degrees=True)
cone_pos = midpoint + length/2 * unit_vector(rotator.apply([0,0,1]))
cone = bpy.ops.mesh.primitive_cone_add(
radius1=radius * radius_ratio,
radius2=0,
depth=length / radius_ratio,
enter_editmode=False,
align='WORLD',
location=cone_pos,
rotation=np.radians(rotation),
scale=(1, 1, 1)
)
obj2 = bpy.context.object
m2 = obj2.data
obj2.name = f"cone_{name}"
# Do blender magic to select both objects and join them
bpy.ops.object.select_all(action='DESELECT')
obj2.select_set(True)
obj1.select_set(True)
bpy.context.view_layer.objects.active = obj2
bpy.context.view_layer.objects.active = obj1
bpy.ops.object.join()
obj = bpy.context.object
obj.name = f"{name}"
mesh = obj.data
mesh.name = f"mesh_{name}"
return obj, mesh
def quat_transform(self, qua): # alpha beta gamma
R1 = Rotation.from_quat(qua).as_matrix()
return R1
def test_malformed_2d_from_rotvec():
with pytest.raises(ValueError, match='Expected `rot_vec` to have shape'):
Rotation.from_rotvec([[1, 2, 3, 4], [5, 6, 7, 8]])
def update(self, t, state, flat_output):
"""
This function receives the current time, true state, and desired flat
outputs. It returns the command inputs.
Inputs:
t, present time in seconds
state, a dict describing the present state with keys # State
x, position, m
v, linear velocity, m/s
q, quaternion [i,j,k,w]
w, angular velocity, rad/s
flat_output, a dict describing the present desired flat outputs with keys # Desired
x, position, m
x_dot, velocity, m/s
x_ddot, acceleration, m/s**2
x_dddot, jerk, m/s**3
x_ddddot, snap, m/s**4
yaw, yaw angle, rad
yaw_dot, yaw rate, rad/s
Outputs:
control_input, a dict describing the present computed control inputs with keys # Output
cmd_motor_speeds, rad/s
cmd_thrust, N (for debugging and laboratory; not used by simulator)
cmd_moment, N*m (for debugging; not used by simulator)
cmd_q, quaternion [i,j,k,w] (for laboratory; not used by simulator)
"""
cmd_motor_speeds = np.zeros((4, ))
cmd_thrust = 0
cmd_moment = np.zeros((3, ))
cmd_q = np.zeros((4, ))
# STUDENT CODE HERE --------------------------------------------------------------------------------------------
# define error
error_pos = state.get('x') - flat_output.get('x')
error_vel = state.get('v') - flat_output.get('x_dot')
error_vel = np.array(error_vel).reshape(3, 1)
error_pos = np.array(error_pos).reshape(3, 1)
# Equation 26
rdd_des = np.array(flat_output.get('x_ddot')).reshape(
3, 1) - np.matmul(self.Kd, error_vel) - np.matmul(
self.Kp, error_pos)
# Equation 28
F_des = (self.mass * rdd_des) + np.array([0, 0, self.mass * self.g
]).reshape(3, 1) # (3 * 1)
# Find Rotation matrix
R = Rotation.as_matrix(Rotation.from_quat(
state.get('q'))) # Quaternions to Rotation Matrix
# print(R.shape)
# Equation 29, Find u1
b3 = R[0:3, 2:3]
# print(b3)
u1 = np.matmul(b3.T, F_des) # u1[0,0] to access value
# print(np.transpose(b3))
# ----------------------- the following is to find u2 ---------------------------------------------------------
# Equation 30
b3_des = F_des / np.linalg.norm(F_des) # 3 * 1
a_Psi = np.array([
np.cos(flat_output.get('yaw')),
np.sin(flat_output.get('yaw')), 0
]).reshape(3, 1) # 3 * 1
b2_des = np.cross(b3_des, a_Psi, axis=0) / np.linalg.norm(
np.cross(b3_des, a_Psi, axis=0))
b1_des = np.cross(b2_des, b3_des, axis=0)
# Equation 33
R_des = np.hstack((b1_des, b2_des, b3_des))
# print(R_des)
# Equation 34
# R_temp = 0.5 * (np.matmul(np.transpose(R_des), R) - np.matmul(np.transpose(R), R_des))
temp = R_des.T @ R - R.T @ R_des
R_temp = 0.5 * temp
# orientation error vector
e_R = 0.5 * np.array([-R_temp[1, 2], R_temp[0, 2], -R_temp[0, 1]
]).reshape(3, 1)
# Equation 35
u2 = self.inertia @ (
-self.K_R @ e_R -
self.K_w @ (np.array(state.get('w')).reshape(3, 1)))
gama = self.k_drag / self.k_thrust
Len = self.arm_length
cof_temp = np.array([
1, 1, 1, 1, 0, Len, 0, -Len, -Len, 0, Len, 0, gama, -gama, gama,
-gama
]).reshape(4, 4)
u = np.vstack((u1, u2))
F_i = np.matmul(np.linalg.inv(cof_temp), u)
for i in range(4):
if F_i[i, 0] < 0:
F_i[i, 0] = 0
cmd_motor_speeds[i] = self.rotor_speed_max
cmd_motor_speeds[i] = np.sqrt(F_i[i, 0] / self.k_thrust)
if cmd_motor_speeds[i] > self.rotor_speed_max:
cmd_motor_speeds[i] = self.rotor_speed_max
cmd_thrust = u1[0, 0]
cmd_moment[0] = u2[0, 0]
cmd_moment[1] = u2[1, 0]
cmd_moment[2] = u2[2, 0]
cmd_q = Rotation.as_quat(Rotation.from_matrix(R_des))
control_input = {
'cmd_motor_speeds': cmd_motor_speeds,
'cmd_thrust': cmd_thrust,
'cmd_moment': cmd_moment,
'cmd_q': cmd_q
}
return control_input
symbol = "P43212"
N_MOS_DOMAINS = 100
MOS_SPREAD = 1
ANISO_MOS_SPREAD = 0.5, 0.75, 1
eta_diffBragg_id = 19
miller_array_GT = fcalc_from_pdb(resolution=2,
wavelength=1,
algorithm='fft',
ucell=ucell,
symbol=symbol)
Ncells_gt = 15, 15, 15
np.random.seed(3142019)
# generate a random rotation
rotation = Rotation.random(num=1, random_state=100)[0]
Q = rec(rotation.as_quat(), n=(4, 1))
rot_ang, rot_axis = Q.unit_quaternion_as_axis_and_angle()
a_real, b_real, c_real = sqr(
uctbx.unit_cell(
ucell).orthogonalization_matrix()).transpose().as_list_of_lists()
x = col((-1, 0, 0))
y = col((0, -1, 0))
z = col((0, 0, -1))
rx, ry, rz = np.random.uniform(-180, 180, 3)
RX = x.axis_and_angle_as_r3_rotation_matrix(rx, deg=True)
RY = y.axis_and_angle_as_r3_rotation_matrix(ry, deg=True)
RZ = z.axis_and_angle_as_r3_rotation_matrix(rz, deg=True)
M = RX * RY * RZ
a_real = M * col(a_real)
def test_from_generic_mrp():
mrp = np.array([[1, 2, 2], [1, -1, 0.5], [0, 0, 0]])
expected_quat = np.array(
[[0.2, 0.4, 0.4, -0.8],
[0.61538462, -0.61538462, 0.30769231, -0.38461538], [0, 0, 0, 1]])
assert_array_almost_equal(Rotation.from_mrp(mrp).as_quat(), expected_quat)
from cv2 import *
# H = K2 * R2^T * [Id - (C1-C2) * (-n^T) / (n^T * (t-C1))] * R1 * K1^-1
# file:
# folderName = 'Built_stereo_image' + '/Normal_case_1'
# if not os.path.exists(folderName):
# os.makedirs(folderName)
text_name = 'ground_truth.txt'
text = open(text_name, 'w+')
# planes
# blender use extrinsic rotation parameter to rotate the object.
n = R.from_euler('xyz', [[89.7747, -6.769, 90], [48.4647, 75.4723, -32.5045],
[-12.878, -54.781, 166.181]],
degrees=True).apply([0.0, 0.0, 1.0])
t = array([[-1.58333, -0.444686, 0.412037], [1.18149, 0.41549, 0.285719],
[1.40202, -0.012275, 0.578975]])
# cameras
C = array([[8.0, -7.0, 5.0], [8.4642, -6.35394, 5]])
# 1 is left camera, H transform left to right
r = R.from_euler('xyz', [[65.0, 0.0, 50.0], [64.6, 0, 54.7]], degrees=True)
# camera internals f = focal_length * k = focal_length * 100
f = 1500
p = array([600, 400])
def test_deepcopy():
r = Rotation.from_quat([0, 0, np.sin(np.pi / 4), np.cos(np.pi / 4)])
r1 = copy.deepcopy(r)
assert_allclose(r.as_matrix(), r1.as_matrix(), atol=1e-15)
dataset = sys.argv[1].replace("_local.txt", "")
dataset = dataset.replace("_global.txt", "")
folder = dataset
# folder='p2at_met'
rot_magnitude = []
with open(sys.argv[1]) as csvfile:
file_reader = csv.DictReader(csvfile, delimiter=' ')
for row in file_reader:
source_file = f"{folder}/{row['source']}"
target_file = f"{folder}/{row['target']}"
trans = [float(row[f"t{index}"]) for index in range(1, 13)]
# trans[3] = 0
# trans[7] = 0
# trans[11] = 0
trans = trans + [0, 0, 0, 1]
trans = numpy.asarray(trans).reshape(4, 4)
rot = Rotation.from_dcm(trans[:3, :3])
rot_magnitude.append(degrees(numpy.linalg.norm(rot.as_rotvec())))
print(f"{source_file} - {target_file} - {row['overlap']}")
source = o3d.io.read_point_cloud(source_file)
target = o3d.io.read_point_cloud(target_file)
moved_source = copy.deepcopy(source)
moved_source.transform(trans)
source.paint_uniform_color([0, 1, 0])
target.paint_uniform_color([1, 0, 0])
moved_source.paint_uniform_color([0, 0, 1])
o3d.visualization.draw_geometries([source, target, moved_source])
# plt.plot(rot_magnitude,'*r')
# plt.show()
def test_pickling():
r = Rotation.from_quat([0, 0, np.sin(np.pi / 4), np.cos(np.pi / 4)])
pkl = pickle.dumps(r)
unpickled = pickle.loads(pkl)
assert_allclose(r.as_matrix(), unpickled.as_matrix(), atol=1e-15)
def transform_and_save(path, df):
ImgIds = []
if os.path.isdir('train_inputs'):
shutil.rmtree('train_inputs', ignore_errors=True)
if os.path.isdir('train_hms'):
shutil.rmtree('train_hms', ignore_errors=True)
if os.path.isdir('train_regs'):
shutil.rmtree('train_regs', ignore_errors=True)
os.mkdir('train_inputs')
os.mkdir('train_hms')
os.mkdir('train_regs')
print('created directories')
for n in range(len(df)):
img_file = path + '_images/%s.jpg' % df.iloc[n].ImageId
mask_file = path + '_masks/%s.jpg' % df.iloc[n].ImageId
img = cv2.imread(img_file)
mask = cv2.imread(mask_file)
parameters = get_parameters()
if mask is not None:
img = img * (mask < 128)
img = img[1355:, :, ::-1]
#parameters = get_parameters()
string = df.iloc[n].PredictionString
roti, trxi = str_to_arrays(string)
#print(rot)
resized_img = cv2.resize(img, (iimg_w, iimg_h))
#print(s)
rot = np.array(roti)
trx = np.array(trxi)
alpha, beta, gamma = parameters['train_rot']
M = parameters['pers']
RotP, RotM = RotateImage(alpha, beta, gamma, dx=img_w / 2)
perspected_img = cv2.warpPerspective(resized_img, M, (ip_w, ip_h))
perspected_img_e = get_enhanced(perspected_img)
rotated_img = cv2.warpPerspective(img, RotP, (img_w, img_h))
r_rotated_img = cv2.resize(rotated_img, (iimg_w, iimg_h))
p_rotated_img = cv2.warpPerspective(r_rotated_img, M, (ip_w, ip_h))
p_rotated_img_e = get_enhanced(p_rotated_img)
r1 = R.from_euler('yxz', [-beta, alpha, gamma], degrees=False)
r2 = R.from_euler('yxz', rot, degrees=False)
r = r1 * r2
rot = r.as_euler('yxz')
trx = np.dot(RotM[:3, :3], trx.T).T
pred_str = ''
for dof_id in range(len(rot)):
dof_str = ' 1 %f %f %f %f %f %f' % (
rot[dof_id, 1], rot[dof_id, 0], rot[dof_id, 2], trx[dof_id, 0],
trx[dof_id, 1], trx[dof_id, 2])
pred_str += dof_str
perspected_img = perspected_img.astype(np.uint8)
perspected_img_e = perspected_img_e.astype(np.uint8)
p_rotated_img = p_rotated_img.astype(np.uint8)
p_rotated_img_e = p_rotated_img_e.astype(np.uint8)
np.save('train_inputs/%s_%d_n.npy' % (df.iloc[n].ImageId, 0),
perspected_img)
np.save('train_inputs/%s_%d_e.npy' % (df.iloc[n].ImageId, 0),
perspected_img_e)
np.save('train_inputs/%s_%d_n.npy' % (df.iloc[n].ImageId, 1),
p_rotated_img)
np.save('train_inputs/%s_%d_e.npy' % (df.iloc[n].ImageId, 1),
p_rotated_img_e)
ImgIds.append('%s_%d' % (df.iloc[n].ImageId, 0))
ImgIds.append('%s_%d' % (df.iloc[n].ImageId, 1))
coords = str_to_coords(string)
hm, reg = pose(coords, iimg_h, iimg_w)
perspected_hm = cv2.warpPerspective(hm, M, (ip_w, ip_h))
perspected_reg = cv2.warpPerspective(reg,
M, (ip_w, ip_h),
flags=cv2.INTER_NEAREST)
perspected_hm_tf = tf.reshape(perspected_hm, [1, ip_h, ip_w, 1])
op_hm = tf.nn.max_pool2d(perspected_hm_tf, 4, 4, padding='VALID')
op_reg_t = cv2.resize(perspected_reg, (op_w, op_h),
interpolation=cv2.INTER_NEAREST)
op_hm = np.squeeze(op_hm.numpy())
op_hm[(op_hm *
(op_hm == maximum_filter(op_hm, footprint=np.ones(
(3, 3)))) > 0.1)] = 1
op_reg = np.zeros_like(op_reg_t)
y, x = np.where(op_hm == 1)
op_reg[y, x, :] = op_reg_t[y, x, :]
op_hm = np.reshape(op_hm, op_hm.shape + (1, ))
np.save('train_hms/%s_%d.npy' % (df.iloc[n].ImageId, 0), op_hm)
np.save('train_regs/%s_%d.npy' % (df.iloc[n].ImageId, 0), op_reg)
coords = str_to_coords(pred_str)
hm, reg = pose(coords, iimg_h, iimg_w)
perspected_hm = cv2.warpPerspective(hm, M, (ip_w, ip_h))
perspected_reg = cv2.warpPerspective(reg,
M, (ip_w, ip_h),
flags=cv2.INTER_NEAREST)
perspected_hm_tf = tf.reshape(perspected_hm, [1, ip_h, ip_w, 1])
op_hm = tf.nn.max_pool2d(perspected_hm_tf, 4, 4, padding='VALID')
reg = cv2.resize(perspected_reg, (op_w, op_h),
interpolation=cv2.INTER_NEAREST)
op_hm = np.squeeze(op_hm.numpy())
op_hm[(op_hm *
(op_hm == maximum_filter(op_hm, footprint=np.ones(
(3, 3)))) > 0.1)] = 1
op_reg = np.zeros_like(reg)
y, x = np.where(op_hm == 1)
op_reg[y, x, :] = reg[y, x, :]
op_hm = np.reshape(op_hm, op_hm.shape + (1, ))
np.save('train_hms/%s_%d.npy' % (df.iloc[n].ImageId, 1), op_hm)
np.save('train_regs/%s_%d.npy' % (df.iloc[n].ImageId, 1), op_reg)
if n % 200 == 0: print('completed %d images' % n)
np.save('image_names.npy', np.array(ImgIds))
def test_multiplication_stability():
qs = Rotation.random(50, random_state=0)
rs = Rotation.random(1000, random_state=1)
for q in qs:
rs *= q * rs
assert_allclose(np.linalg.norm(rs.as_quat(), axis=1), 1)
pfName = file.split(')')[0].split('(')[1]
try:
hkls.append(tuple([int(c) for c in pfName]))
files.append(os.path.join(datadir, file))
except: #not hkls
continue
sortby = [sum([c**2 for c in h]) for h in hkls]
hkls = [x for _, x in sorted(zip(sortby, hkls), key=lambda pair: pair[0])]
files = [
x for _, x in sorted(zip(sortby, files), key=lambda pair: pair[0])
]
rot = R.from_euler('XZY', (13, -88, 90), degrees=True).as_dcm()
pf = poleFigure(files, hkls, crystalSym, 'nd')
""" rotate """
pf.rotate(rot)
od = bunge(cellSize, crystalSym, sampleSym)
hkls = np.array(hkls)
phi = np.linspace(0, 2 * pi, 73)
""" symmetry after """
fibre_e = {}
fibre_q = {}
weights = {}
refls = symmetrise(crystalSym, hkls)
def test_from_single_3d_matrix():
mat = np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]).reshape((1, 3, 3))
expected_quat = np.array([0.5, 0.5, 0.5, 0.5]).reshape((1, 4))
assert_array_almost_equal(
Rotation.from_matrix(mat).as_quat(), expected_quat)
def mrp_to_rotation_matrix(sigma):
sigma_squared = np.inner(sigma, sigma)
q0 = (1 - sigma_squared) / (1 + sigma_squared)
q = [2 * sigma_i / (1 + sigma_squared) for sigma_i in sigma]
q.extend([q0])
return Rotation.from_quat(q).as_matrix().T
def test_slerp_decreasing_times():
with pytest.raises(ValueError, match="strictly increasing order"):
rnd = np.random.RandomState(0)
r = Rotation.from_quat(rnd.uniform(size=(5, 4)))
t = [0, 1, 3, 2, 4]
Slerp(t, r)
def take_snapshot_rotation(queue):
pipeline = rs.pipeline()
pc = rs.pointcloud()
config = rs.config()
config.enable_stream(stream_type=rs.stream.depth,
width=640,
height=480,
format=rs.format.z16,
framerate=30)
config.enable_stream(stream_type=rs.stream.color,
width=640,
height=480,
format=rs.format.bgr8,
framerate=30)
pipeline.start(config)
cnt = 0
#try:
while True:
frames = pipeline.wait_for_frames()
cnt += 1
if cnt > 5:
depth_frame = frames.get_depth_frame()
color_frame = frames.get_color_frame()
if not depth_frame or not color_frame:
continue
print("Get depth frame")
color_image = np.asanyarray(color_frame.get_data())
color_image = color_image[..., ::-1]
#Get intrinsic
depth_intrinsics = rs.video_stream_profile(
depth_frame.profile).get_intrinsics()
w, h = depth_intrinsics.width, depth_intrinsics.height
Rtilt = np.reshape(np.eye(3), -1)
K = np.array([
depth_intrinsics.fx, 0, 0, 0, depth_intrinsics.fy, 0,
depth_intrinsics.ppx, depth_intrinsics.ppy, 1
])
print(Rtilt, K)
if save_file:
filename = 'snapshot_' + str(currentTime) + '.ply'
if not os.path.exists('point_cloud'): os.mkdir('point_cloud')
ply = rs.save_to_ply('point_cloud/' + filename)
ply.set_option(rs.save_to_ply.option_ply_binary, False)
ply.set_option(rs.save_to_ply.option_ignore_color, True)
ply.set_option(rs.save_to_ply.option_ply_normals, False)
ply.set_option(rs.save_to_ply.option_ply_mesh, False)
print("Saving point cloud...")
ply.process(depth_frame)
print("Point cloud is created as {}".format(filename))
rgb_filename = filename[:-3] + 'jpg'
if not os.path.exists('rgb_image'): os.mkdir('rgb_image')
imageio.imwrite('rgb_image/' + rgb_filename, color_image)
print("RGB Image is created as {}".format(rgb_filename))
else:
decimate = rs.decimation_filter()
decimate.set_option(rs.option.filter_magnitude, 2**1)
depth_frame = decimate.process(depth_frame)
points = pc.calculate(depth_frame)
pc.map_to(depth_frame)
pipeline.stop()
break
if save_file:
with open('point_cloud/' + filename, 'r') as input_file:
header_cnt = 8
cnt = 0
all_lines = input_file.readlines()
#Get header and data
header = all_lines[:header_cnt]
split_line = header[3].strip().split(' ')
vertex_cnt = int(split_line[2])
data = all_lines[header_cnt:(vertex_cnt + header_cnt)]
point_cloud = []
# Random sampling
replace = True if vertex_cnt < 20000 else False
sampled_int = np.random.choice(vertex_cnt, 20000, replace=replace)
if vertex_cnt < 20000:
vertex_cnt = vertex_cnt
else:
vertex_cnt = 20000
header[3] = ' '.join(
split_line[:2] + [str(vertex_cnt) + '\n']
) #We will randomly choose 20000 points, so change the point cloud info
#Get point cloud
if vertex_cnt >= 20000:
idx = -1
for line in data:
idx += 1
if idx not in sampled_int:
continue
line_split = line.strip().split(' ')
new_line = []
for item in line_split:
new_line.append(float(item))
point_cloud.append(new_line)
else:
for line in data:
line_split = line.strip().split(' ')
new_line = []
for item in line_split:
new_line.append(float(item))
point_cloud.append(new_line)
#Apply rotation by 90 degree along x-axis
axis = [1, 0, 0]
axis = axis / norm(axis)
theta = math.pi / 2
rot = Rotation.from_rotvec(theta * axis)
new_point_cloud = rot.apply(point_cloud)
# Saving point cloud
out_filename = filename[:-4] + '_r.ply'
with open('point_cloud/' + out_filename, 'w') as output_file:
output_file.writelines(header)
for line in new_point_cloud.tolist():
print_line = ''
for item in line:
print_line += "{:.5f}".format(item) + ' '
print_line += '\n'
output_file.write(print_line)
print('Rotation completed')
#queue.put(out_filename)
else:
v = points.get_vertices()
verts = np.asanyarray(v).view(np.float32).reshape(-1, 3) # xyz
verts = verts[verts[:, 2] < 5] # clip where z is farther than 10
verts = verts[verts[:, 2] > 0.1] # clip where z is closer than 0.1
data = verts
vertex_cnt = data.shape[0]
# Random sampling
replace = True if vertex_cnt < 20000 else False
sampled_int = np.random.choice(vertex_cnt, 20000, replace=replace)
point_cloud = verts[sampled_int]
if vertex_cnt < 20000:
vertex_cnt = vertex_cnt
else:
vertex_cnt = 20000
#Apply rotation by 90 degree along x-axis
axis = [1, 0, 0]
axis = axis / norm(axis)
theta = -math.pi / 2
rot = Rotation.from_rotvec(theta * axis)
new_point_cloud = rot.apply(point_cloud)
print('Rotation completed')
#print("Point cloud shape", new_point_cloud.shape) # (20000, 3)
pc_path = os.path.join(BASE_DIR, 'point_cloud', 'pc.npy')
np.save(pc_path, new_point_cloud)
img_path = os.path.join(BASE_DIR, 'rgb_image', 'rgb.npy')
#print("Image shape", color_image.shape) #(480, 640, 3)
np.save(img_path, color_image)
queue.put(pc_path)
queue.put(img_path)
queue.put((Rtilt, K))
print("Sending queue finished")
""" Using shared array for multiprocessing... replaced
def test_from_2d_single_rotvec():
rotvec = [[1, 0, 0]]
expected_quat = np.array([[0.4794255, 0, 0, 0.8775826]])
result = Rotation.from_rotvec(rotvec)
assert_array_almost_equal(result.as_quat(), expected_quat)
