def pointing(self, time):
'''Calculate the pointing direction for a set of times
There are the steps to convert the dither (which are offsets) to the total
pointing direction with the roll properly taken care of.
(See marxasp.c in the marx code for more details.)
1. Convert ra/dec offsets to absolute pointing.
2. Roll resulting vector about nominal pointing
3. Convert result to ra/dec.
Parameters
----------
time : np.array
Array of times
Results
-------
pointing : (n, 3) np.array
Ra, Dec, roll values in radian for the pointing direction at time t.
'''
nominal = np.deg2rad(np.array([self.ra, self.dec, self.roll]))
dither = self.dither(time)
# roll in astronomical system is defined opposite of the usual mathematical angle
# because the ra in the coordinate system increases in the other direction.
roll = nominal[2] + dither[:, 2]
# Express directions as x,y,z vectors
phi = nominal[0]
theta = np.pi/2. - nominal[1]
e_nominal = np.array([np.sin(phi) * np.sin(theta),
np.cos(phi) * np.sin(theta),
np.cos(theta)])
phi = nominal[0] + dither[:, 0]
theta = np.pi/2.- (nominal[1] + dither[:, 1])
e_dither = np.vstack([np.sin(phi) * np.sin(theta),
np.cos(phi) * np.sin(theta),
np.cos(theta)]).T
pointing_dir = np.zeros_like(dither)
# common case for Chandra
if np.allclose(roll, roll[0]):
mat = axangle2mat(e_nominal, -roll[0], is_normalized=True)
constant_roll = True
for i in range(len(time)):
if not constant_roll:
mat = axangle2mat(e_nominal, -roll[i], is_normalized=True)
pointing_dir[i, :] = np.dot(mat, e_dither[i, :])
# convert x,y,z pointing back to ra, dec, roll
pointing = np.vstack([np.arctan2(pointing_dir[:, 0], pointing_dir[:, 1]) % (2.*np.pi),
np.pi / 2. - np.arccos(pointing_dir[:, 2]),
roll]).T
return pointing
def make_translation_pattern_coromeridian(coromeridian_pole,
translation_velocity,
frame_rate,
epoch_duration=2.25,
max_sensory_radius=2,
star_density=50,
display_shape=(96, 32)):
#translate in a direction on the equator.
from scipy.misc import imresize
frame_period = 1 / frame_rate
frame_distance = frame_period * translation_velocity
frames_per_cycle = around(epoch_duration * frame_rate).astype(int)
####################
xyz = make_universe(star_density=star_density,
max_sensory_radius=max_sensory_radius + frame_distance)
###align the orientation vector with arena coordinates
unit_vector = np.array([0., 1., 0.])
zp_offset = (2 * pi) / 96 * 4
orientation_vector = dot(
axangles.axangle2mat([1, 0, 0], coromeridian_pole)[:3, :3],
unit_vector)
orientation_vector = dot(axangles.axangle2mat([0, 0, 1], zp_offset),
orientation_vector) * translation_velocity
xyzp = integrate_frame(xyz,
rotation_vect=np.array([0.1, 0.1, 0.1]),
translation_vect=orientation_vector,
frame_period=1 / frame_rate)
###crop to max_sensory_radius
xyzp = crop_universe(xyzp, max_sensory_radius)
proj = arena_project(xyzp)
imgs = digitize_points(*proj, display_shape=display_shape)
imgs = imgs[:, :, newaxis]
xyzp = refill_universe(xyzp, star_density, max_sensory_radius,
frame_distance)
#####################
for i in range(frames_per_cycle):
xyzp = integrate_frame(xyzp,
rotation_vect=np.array([1e-10, 1e-10, 1e-10]),
translation_vect=orientation_vector,
frame_period=1 / frame_rate)
xyzp = crop_universe(xyzp, max_sensory_radius)
proj = arena_project(xyzp)
img = digitize_points(*proj, display_shape=display_shape)
imgs = concatenate((imgs, img[:, :, newaxis]), axis=2)
xyzp = refill_universe(xyzp, star_density, max_sensory_radius,
frame_distance)
from scipy import io
return imgs
def produce_video():
axangles = np.load('./axangles.npy')
mesh = MANOModel('./model.pkl')
view_mat = np.matmul(
axangle2mat([1, 0, 0], -np.pi/2), axangle2mat([0, 1, 0], -np.pi/2)
)
verts = []
for a in axangles:
a = np.concatenate([np.array([[0, 0, 0]]), a], 0)
v = mesh.set_params(pose_abs=a)
verts.append(np.matmul(view_mat, v.T).T)
viewer = TriMeshViewer()
viewer.run(verts, mesh.faces, './all_scans_view2.avi', 10)
def test_change_position_after_init():
'''Regression: In MARXS 0.1 the geometry could not be changed after init
because a lot of stuff was pre-calculated in __init__. That should no
longer be the case. This test is here to check that for gratings.'''
photons = generate_test_photons(5)
g1 = FlatGrating(d=1. / 500,
order_selector=OrderSelector([1]),
groove_angle=.3)
g1.order_name = 'one'
p1 = g1(photons.copy())
pos3d = axangles.axangle2mat([1, 0, 0], .3)
trans = np.array([0., -.3, .5])
# Make grating at some point
g2 = FlatGrating(d=1. / 500,
order_selector=OrderSelector([1]),
position=trans)
# then move it so that pos and groove direction match g1
g2.geometry.pos4d = affines.compose(np.zeros(3), pos3d, 25. * np.ones(3))
g2.blaze_name = 'myblaze'
p2 = g2(photons.copy())
assert np.allclose(p1['dir'], p2['dir'])
assert np.allclose(p1['pos'], p2['pos'])
# Check naming of the grating output columns
assert np.all(p1['one'] == 1)
assert 'myblaze' in p2.colnames
def make_spin_pattern_equator(equator_pole,
angular_velocity,
frame_rate,
max_sensory_radius=2,
star_density=50,
display_shape=(96, 32)):
#Spin the world around a equatorial pole
from scipy.misc import imresize
unit_vector = np.array([0., 1., 0.])
zp_offset = (2 * pi) / 96 * 4
orientation_vector = dot(
axangles.axangle2mat([0, 0, 1], equator_pole + zp_offset)[:3, :3],
unit_vector) * angular_velocity
xyz = make_universe(star_density=star_density,
max_sensory_radius=max_sensory_radius)
xyzp = integrate_frame(xyz,
rotation_vect=orientation_vector,
translation_vect=np.array([0, 0, 0]),
frame_period=1 / frame_rate)
proj = arena_project(xyzp)
imgs = digitize_points(*proj, display_shape=display_shape)
imgs = imgs[:, :, newaxis]
frames_per_cycle = int(((2 * pi) / angular_velocity) * frame_rate)
for i in range(frames_per_cycle):
xyzp = integrate_frame(xyzp,
rotation_vect=orientation_vector,
translation_vect=np.array([0, 0, 0]),
frame_period=1 / frame_rate)
proj = arena_project(xyzp)
img = digitize_points(*proj, display_shape=display_shape)
imgs = concatenate((imgs, img[:, :, newaxis]), axis=2)
from scipy import io
return imgs
def from_rowland(cls, rowland, width, rotation=0., kwargs={}):
'''Generate a `Cylinder` from a `RowlandTorus`.
According to the definition of the `marxs.design.rowland.RowlandTorus`
the origin phi=0 is at the "top". When this class method is used to
make a detector that catches all dispersed grating signal on the
Rowland torus, a ``rotation=np.pi`` places the center of the Cylinder
close to the center of the torus (the location of the focal point in
the standard Rowland geometry).
Parameters
----------
rowland : `~marxs.design.RowlandTorus`
The circular detector is constructed to fit exactly into the
Rowland Circle defined by ``rowland``.
width : float
Half-width of the tube in the flat direction (z-axis) in mm
rotation : float
Rotation angle of the Cylinder around its z-axis compared to the
phi=0 position of the Rowland torus.
'''
# Step 1: Rotate around z axis
rot = axangle2mat(np.array([0, 0, 1.]), rotation)
# Step 2: Get position and size from Rowland torus
pos4d_circ = compose([rowland.R, 0, 0], rot,
[rowland.r, rowland.r, width])
# Step 3: Transform to global coordinate system
pos4d_circ = np.dot(rowland.pos4d, pos4d_circ)
# Step 4: Make detector
det_kwargs = {'pos4d': pos4d_circ}
det_kwargs.update(kwargs)
return cls(det_kwargs)
def test_groove_direction():
'''Direction of grooves may not be parallel to z axis.'''
photons = generate_test_photons(5)
order1 = OrderSelector([1])
g = FlatGrating(d=1./500, order_selector=order1)
e_groove, e_perp_groove, n = g.e_groove_coos(np.ones((2,1)))
assert np.isclose(np.dot(e_groove[0, :], e_perp_groove[0, :]), 0.)
p = g(photons.copy())
g1 = FlatGrating(d=1./500, order_selector=order1, groove_angle=.3)
p1 = g1(photons.copy())
pos3d = axangles.axangle2mat([1,0,0], .3)
g2 = FlatGrating(d=1./500, order_selector=order1, orientation=pos3d)
p2 = g2(photons.copy())
pos4d = axangles.axangle2aff([1,0,0], .1)
g3 = FlatGrating(d=1./500, order_selector=order1, groove_angle=.2, pos4d=pos4d)
p3 = g3(photons.copy())
def angle_in_yz(vec1, vec2):
'''project in the y,z plane (the plane of the grating) and calculate angle.'''
v1 = vec1[1:3]
v2 = vec2[1:3]
arccosalpha = np.dot(v1, v2) / np.sqrt(np.dot(v1, v1) * np.dot(v2, v2))
return np.arccos(arccosalpha)
assert np.allclose(angle_in_yz(p1['dir'][0,:], p2['dir'][0, :]), 0)
assert np.allclose(angle_in_yz(p3['dir'][0,:], p2['dir'][0, :]), 0)
for px in [p1, p2, p3]:
assert np.allclose(angle_in_yz(p['dir'][0,:], px['dir'][0, :]), 0.3)
def from_rowland(cls, rowland, width, rotation=0., kwargs={}):
'''Generate a `Cylinder` from a `RowlandTorus`.
According to the definition of the `marxs.design.rowland.RowlandTorus`
the origin phi=0 is at the "top". When this class method is used to
make a detector that catches all dispersed grating signal on the
Rowland torus, a ``rotation=np.pi`` places the center of the Cylinder
close to the center of the torus (the location of the focal point in
the standard Rowland geometry).
Parameters
----------
rowland : `~marxs.design.RowlandTorus`
The circular detector is constructed to fit exactly into the
Rowland Circle defined by ``rowland``.
width : float
Half-width of the tube in the flat direction (z-axis) in mm
rotation : float
Rotation angle of the Cylinder around its z-axis compared to the
phi=0 position of the Rowland torus.
'''
# Step 1: Rotate around z axis
rot = axangle2mat(np.array([0, 0, 1.]), rotation)
# Step 2: Get position and size from Rowland torus
pos4d_circ = compose([rowland.R, 0, 0], rot, [rowland.r, rowland.r, width])
# Step 3: Transform to global coordinate system
pos4d_circ = np.dot(rowland.pos4d, pos4d_circ)
# Step 4: Make detector
det_kwargs = {'pos4d': pos4d_circ}
det_kwargs.update(kwargs)
return cls(det_kwargs)
def test_angle_axis_imps():
for x, y, z in euler_tuples:
M = ttb.euler2mat(z, y, x)
q = tq.mat2quat(M)
vec, theta = tq.quat2axangle(q)
M1 = axangle2mat(vec, theta)
M2 = axangle2aff(vec, theta)[:3,:3]
assert_array_almost_equal(M1, M2)
v1, t1 = mat2axangle(M1)
M3 = axangle2mat(v1, t1)
assert_array_almost_equal(M, M3)
A = np.eye(4)
A[:3,:3] = M
v2, t2, point = aff2axangle(A)
M4 = axangle2mat(v2, t2)
assert_array_almost_equal(M, M4)
def test_groove_direction():
'''Direction of grooves may not be parallel to z axis.'''
photons = generate_test_photons(5)
g = FlatGrating(d=1./500, order_selector=constant_order_factory(1))
assert np.allclose(np.dot(g.geometry['e_groove'], g.geometry['e_perp_groove']), 0.)
p = g.process_photons(photons.copy())
g1 = FlatGrating(d=1./500, order_selector=constant_order_factory(1), groove_angle=.3)
p1 = g1.process_photons(photons.copy())
pos3d = axangles.axangle2mat([1,0,0], .3)
g2 = FlatGrating(d=1./500, order_selector=constant_order_factory(1), orientation=pos3d)
p2 = g2.process_photons(photons.copy())
pos4d = axangles.axangle2aff([1,0,0], .1)
g3 = FlatGrating(d=1./500, order_selector=constant_order_factory(1), groove_angle=.2, pos4d=pos4d)
p3 = g3.process_photons(photons.copy())
def angle_in_yz(vec1, vec2):
'''project in the y,z plane (the plane of the grating) and calculate angle.'''
v1 = vec1[1:3]
v2 = vec2[1:3]
arccosalpha = np.dot(v1, v2) / np.sqrt(np.dot(v1, v1) * np.dot(v2, v2))
return np.arccos(arccosalpha)
assert np.allclose(angle_in_yz(p1['dir'][0,:], p2['dir'][0, :]), 0)
assert np.allclose(angle_in_yz(p3['dir'][0,:], p2['dir'][0, :]), 0)
for px in [p1, p2, p3]:
assert np.allclose(angle_in_yz(p['dir'][0,:], px['dir'][0, :]), 0.3)
def test_change_position_after_init():
'''Regression: In MARXS 0.1 the geometry could not be changed after init
because a lot of stuff was pre-calculated in __init__. That should no
longer be the case. This test is here to check that for gratings.'''
photons = generate_test_photons(5)
g1 = FlatGrating(d=1./500, order_selector=OrderSelector([1]), groove_angle=.3)
g1.order_name = 'one'
p1 = g1(photons.copy())
pos3d = axangles.axangle2mat([1,0,0], .3)
trans = np.array([0., -.3, .5])
# Make grating at some point
g2 = FlatGrating(d=1./500, order_selector=OrderSelector([1]), position=trans)
# then move it so that pos and groove direction match g1
g2.geometry.pos4d = affines.compose(np.zeros(3), pos3d, 25.*np.ones(3))
g2.blaze_name = 'myblaze'
p2 = g2(photons.copy())
assert np.allclose(p1['dir'], p2['dir'])
assert np.allclose(p1['pos'], p2['pos'])
# Check naming of the grating output columns
assert np.all(p1['one'] == 1)
assert 'myblaze' in p2.colnames
def p3d(joints, cam, proc_param):
#(0 - r ankle,
# 1 - r knee,
# 2 - r hip,
# 3 - l hip,
# 4 - l knee,
# 5 - l ankle,
# 6 - pelvis,
# 7 - thorax,
# 8 - upper neck,
# 9 - head top,
# 10 - r wrist,
# 10 - r wrist,
# 12 - r shoulder,
# 13 - l shoulder,
# 14 - l elbow,
# 15 - l wrist)
limb_parents = [1, 2, 12, 12, 3, 4, 7, 8, 12, 12, 9, 10, 13, 13, 13]
import matplotlib.pyplot as plt
from mpl_toolkits import mplot3d
print(joints.shape)
joints = joints[0, :14, :]
img_size = proc_param['img_size']
cam_s = cam[0]
cam_pos = cam[1:]
flength = 500.
tz = flength / (0.5 * img_size * cam_s)
trans = np.hstack([cam_pos, tz])
#joints = joints + trans
R = axangle2mat([1, 0, 0], np.deg2rad(90))
joints = joints.dot(R)
# plt.ion()
plt.figure(1)
plt.clf()
ax = plt.axes(projection='3d')
#ax.view_init(elev=-70,azim=-90)
#ax.scatter3D(joints[:,0],
# joints[:,1],
# joints[:,2], 'gray')
idx = 12
ax.scatter3D(joints[idx, 0],
joints[idx, 1],
joints[idx, 2],
c='red',
s=100)
for i in range(joints.shape[0]):
x_pair = [joints[i, 0], joints[limb_parents[i], 0]]
y_pair = [joints[i, 1], joints[limb_parents[i], 1]]
z_pair = [joints[i, 2], joints[limb_parents[i], 2]]
ax.plot(x_pair, y_pair, zs=z_pair, linewidth=3)
#ax.axis('off')
plt.show()
plt.savefig("test.jpg")
def allocentric_to_egocentric(allo_pose, src_type="mat", dst_type="mat"):
"""Given an allocentric (object-centric) pose, compute new camera-centric
pose Since we do detection on the image plane and our kernels are
2D-translationally invariant, we need to ensure that rendered objects
always look identical, independent of where we render them.
Since objects further away from the optical center undergo skewing,
we try to visually correct by rotating back the amount between
optical center ray and object centroid ray. Another way to solve
that might be translational variance
(https://arxiv.org/abs/1807.03247)
"""
# Compute rotation between ray to object centroid and optical center ray
cam_ray = np.asarray([0, 0, 1.0])
if src_type == "mat":
trans = allo_pose[:3, 3]
elif src_type == "quat":
trans = allo_pose[4:7]
else:
raise ValueError(
"src_type should be mat or quat, got: {}".format(src_type))
obj_ray = trans.copy() / np.linalg.norm(trans)
angle = math.acos(cam_ray.dot(obj_ray))
# Rotate back by that amount
if angle > 0:
if dst_type == "mat":
ego_pose = np.zeros((3, 4), dtype=allo_pose.dtype)
ego_pose[:3, 3] = trans
rot_mat = axangle2mat(axis=np.cross(cam_ray, obj_ray), angle=angle)
if src_type == "mat":
ego_pose[:3, :3] = np.dot(rot_mat, allo_pose[:3, :3])
elif src_type == "quat":
ego_pose[:3, :3] = np.dot(rot_mat, quat2mat(allo_pose[:4]))
elif dst_type == "quat":
ego_pose = np.zeros((7, ), dtype=allo_pose.dtype)
ego_pose[4:7] = trans
rot_q = axangle2quat(np.cross(cam_ray, obj_ray), angle)
if src_type == "quat":
ego_pose[:4] = qmult(rot_q, allo_pose[:4])
elif src_type == "mat":
ego_pose[:4] = qmult(rot_q, mat2quat(allo_pose[:3, :3]))
else:
raise ValueError(
"dst_type should be mat or quat, got: {}".format(dst_type))
else: # allo to ego
if src_type == "mat" and dst_type == "quat":
ego_pose = np.zeros((7, ), dtype=allo_pose.dtype)
ego_pose[:4] = mat2quat(allo_pose[:3, :3])
ego_pose[4:7] = allo_pose[:3, 3]
elif src_type == "quat" and dst_type == "mat":
ego_pose = np.zeros((3, 4), dtype=allo_pose.dtype)
ego_pose[:3, :3] = quat2mat(allo_pose[:4])
ego_pose[:3, 3] = allo_pose[4:7]
else:
ego_pose = allo_pose.copy()
return ego_pose
def test_axangle2mat():
'''Check that vectorized version gives same answers.'''
axis = np.random.rand(4, 3)
angles = np.arange(4.)
out = axangle2mat(axis, angles)
assert np.all(out[0, :, :] == np.eye(3))
for i in range(3):
out1 = axangles.axangle2mat(axis[i + 1, :], angles[i + 1])
assert np.allclose(out[i + 1, :, :], out1)
def test_axangle2mat():
'''Check that vectorized version gives same answers.'''
axis = np.random.rand(4, 3)
angles = np.arange(4.)
out = axangle2mat(axis, angles)
assert np.all(out[0, :, :] == np.eye(3))
for i in range(3):
out1 = axangles.axangle2mat(axis[i + 1, :], angles[i + 1])
assert np.allclose(out[i + 1, :, :], out1)
def get_rotation_matrix_between_vectors(k1, k2, ref_k1, ref_k2):
"""Calculates the rotation matrix to two experimentally measured
diffraction vectors from the corresponding vectors in a reference structure.
Parameters
----------
k1 : np.array()
Experimentally measured scattering vector 1.
k2 : np.array()
Experimentally measured scattering vector 2.
ref_k1 : np.array()
Reference scattering vector 1.
ref_k2 : np.array()
Reference scattering vector 2.
Returns
-------
R : np.array()
Rotation matrix describing transformation from experimentally measured
scattering vectors to equivalent reference vectors.
"""
ref_plane_normal = np.cross(ref_k1, ref_k2)
k_plane_normal = np.cross(k1, k2)
axis = np.cross(ref_plane_normal, k_plane_normal)
# Avoid 0 degree including angle
if np.linalg.norm(axis) == 0:
R = np.identity(3)
else:
# Rotate ref plane into k plane
angle = get_angle_cartesian(ref_plane_normal, k_plane_normal)
R1 = axangle2mat(axis, angle)
rot_ref_k1, rot_ref_k2 = R1.dot(ref_k1), R1.dot(ref_k2)
# Rotate ref vectors in plane
angle1 = get_angle_cartesian(k1, rot_ref_k1)
angle2 = get_angle_cartesian(k2, rot_ref_k2)
angle = 0.5 * (angle1 + angle2)
# k plane normal still the same
R2 = axangle2mat(k_plane_normal, angle)
# Total rotation is the combination of to plane R1 and in plane R2
R = R2.dot(R1)
return R
def test_axangle_euler():
# Conversion between axis, angle and euler
for x, y, z in euler_tuples:
M1 = ttb.euler2mat(z, y, x)
ax, angle = ttb.euler2axangle(z, y, x)
M2 = taa.axangle2mat(ax, angle)
assert_array_almost_equal(M1, M2)
zp, yp, xp = ttb.axangle2euler(ax, angle)
M3 = ttb.euler2mat(zp, yp, xp)
assert_array_almost_equal(M1, M3)
def DA_Rotation(X, rng=None, f=1.0):
x = X[0] if isinstance(X, (list, tuple)) else X
rng = np.random if rng is None else rng
axis = rng.uniform(low=-1, high=1, size=x.shape[1])
angle = rng.uniform(low=-np.pi * f, high=np.pi * f)
R = axangle2mat(axis, angle)
Xr = [apply_rotation(x, R)
for x in X] if isinstance(X,
(list, tuple)) else apply_rotation(X, R)
return Xr
def test_axis_angle():
for M, q in eg_pairs:
vec, theta = tq.quat2axangle(q)
q2 = tq.axangle2quat(vec, theta)
assert tq.nearly_equivalent(q, q2)
aa_mat = taa.axangle2mat(vec, theta)
assert_array_almost_equal(aa_mat, M)
aa_mat2 = sympy_aa2mat(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat2)
aa_mat22 = sympy_aa2mat2(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat22)
def test_axis_angle():
for M, q in eg_pairs:
vec, theta = tq.quat2axangle(q)
q2 = tq.axangle2quat(vec, theta)
assert tq.nearly_equivalent(q, q2)
aa_mat = taa.axangle2mat(vec, theta)
assert_array_almost_equal(aa_mat, M)
aa_mat2 = sympy_aa2mat(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat2)
aa_mat22 = sympy_aa2mat2(vec, theta)
assert_array_almost_equal(aa_mat, aa_mat22)
def offset_angle(self, angle, axis = [1,0,0]): self.angleOffset = angle axis = np.array(axis) Rotation = axangle2mat(axis, angle) currentPosition, currentRotation, currentZoom, currentShear = decompose(self.first.pos4d) matrix = compose( currentPosition ,Rotation,[1,1,1],[0,0,0]) self.first.offset_apparatus(matrix)
def __init__(self, conf, channels=['1', '2', '1m', '2m'], **kwargs):
elements = []
self.order_selector = self.order_selector_class()
self.gratquality = self.gratquality_class()
blazemat = axangle2mat(np.array([0, 0, 1]),
np.deg2rad(-conf['blazeang']))
blazematm = axangle2mat(np.array([0, 0, 1]),
np.deg2rad(conf['blazeang']))
gratinggrid = {
'd_element': 32.,
'z_range': [1e4, 1.4e4],
'elem_class': CATL1L2Stack,
'elem_args': {
'zoom': [1., 13.5, 13.]
},
'parallel_spec': np.array([1., 0., 0., 0.])
}
for chan in channels:
gratinggrid['rowland'] = conf['rowland_' + chan]
b = blazematm if 'm' in chan else blazemat
gratinggrid['elem_args']['orientation'] = b
gratinggrid['normal_spec'] = conf['pos_opt_ax'][chan].copy()
xm, ym = conf['pos_opt_ax'][chan][:2].copy()
sig = 1 if '1' in chan else -1
x_range = [-self.grid_width_x + xm, +self.grid_width_x + xm]
y_range = [
sig * (600 - ym - self.grid_width_y),
sig * (600 - ym + self.grid_width_y)
]
y_range.sort()
elements.append(
RectangularGrid(x_range=x_range,
y_range=y_range,
id_num_offset=arcus.arcus.id_num_offset[chan],
**gratinggrid))
elements.extend([catsupport, catsupportbars, self.gratquality])
super(CATGratings, self).__init__(elements=elements, **kwargs)
def egocentric_to_allocentric(ego_pose,
src_type="mat",
dst_type="mat",
cam_ray=(0, 0, 1.0)):
# Compute rotation between ray to object centroid and optical center ray
cam_ray = np.asarray(cam_ray)
if src_type == "mat":
trans = ego_pose[:3, 3]
elif src_type == "quat":
trans = ego_pose[4:7]
else:
raise ValueError(
"src_type should be mat or quat, got: {}".format(src_type))
obj_ray = trans.copy() / np.linalg.norm(trans)
angle = math.acos(cam_ray.dot(obj_ray))
# Rotate back by that amount
if angle > 0:
if dst_type == "mat":
allo_pose = np.zeros((3, 4), dtype=ego_pose.dtype)
allo_pose[:3, 3] = trans
rot_mat = axangle2mat(axis=np.cross(cam_ray, obj_ray),
angle=-angle)
if src_type == "mat":
allo_pose[:3, :3] = np.dot(rot_mat, ego_pose[:3, :3])
elif src_type == "quat":
allo_pose[:3, :3] = np.dot(rot_mat, quat2mat(ego_pose[:4]))
elif dst_type == "quat":
allo_pose = np.zeros((7, ), dtype=ego_pose.dtype)
allo_pose[4:7] = trans
rot_q = axangle2quat(np.cross(cam_ray, obj_ray), -angle)
if src_type == "quat":
allo_pose[:4] = qmult(rot_q, ego_pose[:4])
elif src_type == "mat":
allo_pose[:4] = qmult(rot_q, mat2quat(ego_pose[:3, :3]))
else:
raise ValueError(
"dst_type should be mat or quat, got: {}".format(dst_type))
else:
if src_type == "mat" and dst_type == "quat":
allo_pose = np.zeros((7, ), dtype=ego_pose.dtype)
allo_pose[:4] = mat2quat(ego_pose[:3, :3])
allo_pose[4:7] = ego_pose[:3, 3]
elif src_type == "quat" and dst_type == "mat":
allo_pose = np.zeros((3, 4), dtype=ego_pose.dtype)
allo_pose[:3, :3] = quat2mat(ego_pose[:4])
allo_pose[:3, 3] = ego_pose[4:7]
else:
allo_pose = ego_pose.copy()
return allo_pose
def p3d(joints):
#(0 - r ankle,
# 1 - r knee,
# 2 - r hip,
# 3 - l hip,
# 4 - l knee,
# 5 - l ankle,
# 6 - pelvis,
# 7 - thorax,
# 8 - upper neck,
# 9 - head top,
# 10 - r wrist,
# 10 - r wrist,
# 12 - r shoulder,
# 13 - l shoulder,
# 14 - l elbow,
# 15 - l wrist)
limb_parents = [1, 2, 12, 12, 3, 4, 7, 8, 12, 12, 9, 10, 13, 13, 13]
#y_temp = joints[:,1]
R = axangle2mat([1, 0, 0], np.deg2rad(90))
joints = joints.dot(R)
print(joints.shape)
#temp = joints[:,1]
#joints[:,1] = joints[:,2]
#joints[:,2] = -temp
# plt.ion()
plt.figure(1)
plt.clf()
ax = plt.axes(projection='3d')
#ax.view_init(elev=-70,azim=-90)
#ax.scatter3D(joints[:,0],
# joints[:,1],
# joints[:,2], 'gray')
idx = 13
ax.scatter3D(joints[idx, 0],
joints[idx, 1],
joints[idx, 2],
c='red',
s=100)
for i in range(joints.shape[0]):
x_pair = [joints[i, 0], joints[limb_parents[i], 0]]
y_pair = [joints[i, 1], joints[limb_parents[i], 1]]
z_pair = [joints[i, 2], joints[limb_parents[i], 2]]
ax.plot(x_pair, y_pair, zs=z_pair, linewidth=3)
#ax.axis('off')
plt.show()
def make_control_pattern_equator(equator_poles,
angular_velocity,
frame_rate,
max_sensory_radius=2,
star_density=50,
display_shape=(96, 32)):
#Spin the world around a equatorial pole
from scipy.misc import imresize
unit_vector = np.array([0., 1., 0.])
zp_offset = (2 * pi) / 96 * 4
orientation_vectors = list()
for equator_pole in equator_poles:
orientation_vectors.append(
dot(
axangles.axangle2mat([0, 0, 1], equator_pole +
zp_offset)[:3, :3], unit_vector) *
angular_velocity)
xyz_list = list()
for equator_pole in equator_poles:
xyz_list.append(
make_universe(star_density=star_density /
float(len(equator_poles)),
max_sensory_radius=max_sensory_radius))
xyzp_list = list()
#xyz = make_universe(star_density = star_density,max_sensory_radius = max_sensory_radius)
for xyz, orientation_vector in zip(xyz_list, orientation_vectors):
xyzp_list.append(
integrate_frame(xyz,
rotation_vect=orientation_vector,
translation_vect=np.array([0, 0, 0]),
frame_period=1 / frame_rate))
proj = arena_project(np.hstack(xyzp_list))
imgs = digitize_points(*proj, display_shape=display_shape)
imgs = imgs[:, :, newaxis]
frames_per_cycle = int(((2 * pi) / angular_velocity) * frame_rate)
for i in range(frames_per_cycle):
for ptidx in range(len(xyzp_list)):
#,orientation_vector in zip(xyzp_list,orientation_vectors):
xyzp_list[ptidx] = integrate_frame(
xyzp_list[ptidx],
rotation_vect=orientation_vectors[ptidx],
translation_vect=np.array([0, 0, 0]),
frame_period=1 / frame_rate)
proj = arena_project(np.hstack(xyzp_list))
img = digitize_points(*proj, display_shape=display_shape)
imgs = concatenate((imgs, img[:, :, newaxis]), axis=2)
#from scipy import io
return imgs
def DA_Rotation(X):
#np.random.seed(0)
assert X.shape[1] % 3 == 0, "No of columns should be divisible by 3!"
X_new = np.zeros(X.shape)
axis = np.random.uniform(low=-1, high=1, size=X.shape[1])
angle = np.random.uniform(low=-np.pi, high=np.pi)
#print(angle)
# this is to rotate all triples using same rotation matrix
rotation_matrix = axangle2mat(axis[0:3], angle)
#print(rotation_matrix)
for i in range(0, X.shape[1], 3):
#rotation_matrix = axangle2mat(axis[i:i+3],angle)
#print(rotation_matrix)
X_new[:, i:i + 3] = np.matmul(X[:, i:i + 3], rotation_matrix)
return X_new
def setUp(self):
np.random.seed(self.seed)
self.locs -= np.sum(self.locs.T * self.masses, axis=1).T / self.M
self.box = sim3.InfiniteBox()
self.atoms = sim3.AtomVec(self.masses)
vs = np.random.normal(size=np.shape(self.locs)) if self.vs is None else self.vs
fs = np.random.normal(size=np.shape(self.locs)) if self.fs is None else self.fs
for a, loc, v, f in zip(self.atoms, self.locs, vs, fs):
a.x = loc
a.v = v
a.f = f
self.rigid = sim3.RigidConstraint(self.box, self.atoms)
self.rotmatrix = axangle2mat(self.axis, self.dtheta)
for a, loc in zip(self.atoms, self.locs.dot(self.rotmatrix.T)):
a.x = loc
def setUp(self):
np.random.seed(self.seed)
self.locs -= np.sum(self.locs.T*self.masses, axis=1).T/self.M
self.box = sim3.InfiniteBox()
self.atoms = sim3.AtomVec(self.masses)
vs = np.random.normal(size=np.shape(self.locs)) if self.vs is None else self.vs
fs = np.random.normal(size=np.shape(self.locs)) if self.fs is None else self.fs
for a, loc, v, f in zip(self.atoms, self.locs, vs, fs):
a.x = loc
a.v = v
a.f = f
self.rigid = sim3.RigidConstraint(self.box, self.atoms)
self.rotmatrix = axangle2mat(self.axis, self.dtheta)
for a, loc in zip(self.atoms, self.locs.dot(self.rotmatrix.T)):
a.x = loc
def test_qrot_points():
from lib.pysixd.inout import load_ply
data_root = osp.normpath(osp.join(cur_dir, "../../datasets/BOP_DATASETS/lm/"))
models_cad_files = [osp.join(data_root, "models/obj_{:06d}.ply".format(i)) for i in range(1, 15 + 1)]
obj_id = 0
points = load_ply(models_cad_files[obj_id])["pts"]
axis = np.array([1, 2, 0])
rot = axangle2mat(axis, -pi / 3)
quat = mat2quat(rot)
points_q = qrot_points_th(torch.from_numpy(quat), torch.from_numpy(points))
# N = points.shape[0]
# points_q = qrot_torch(torch.from_numpy(quat).expand(N, 4), torch.from_numpy(points))
points_r = rot.dot(points.T).T
print(np.allclose(points_q.numpy(), points_r))
def integrate_frame(pts,
rotation_vect=np.array([1.0, 1.0, 0.0]),
translation_vect=np.array([0.0, 0.0, 0.0]),
frame_period=0.1):
rotational_velocity = linalg.norm(rotation_vect)
rotational_direction = rotation_vect / rotational_velocity
dt = frame_period / 100.0
dtheta = rotational_velocity * dt
drot = axangles.axangle2mat(rotational_direction, dtheta)
#dtrans = tran.translation_matrix(translation_vect*dt)
dA = affines.compose(translation_vect * dt, drot, np.array([1, 1, 1]))
#dA = np.dot(dtrans,drot)
for x in range(int(frame_period / dt)):
pts_H = vstack([pts, ones(shape(pts)[1])])
pts = np.dot(dA, pts_H)[:-1, :]
return pts
def rotation(x):
flag = False
if x.ndim == 3:
x = x[0, :, :]
flag = True
n_sensor = x.shape[1] / 3
original_sensor = np.split(x, n_sensor, axis=1)
for axis, sensor in zip(np.arange(0, x.shape[1], 3), original_sensor):
axis_temp = np.random.uniform(low=-1, high=1, size=3)
angle_temp = np.random.uniform(low=-np.pi, high=np.pi)
x[:, [axis, axis + 1, axis + 2]] = np.matmul(
sensor, axangle2mat(axis_temp, angle_temp))
if flag:
return x[np.newaxis, :, :]
else:
return x
def __init__(self):
self.view_mat = axangle2mat(
[1, 0, 0], np.pi) # align different coordinate systems
self.window_size = 1080
self.mesh_color = np.array(
constants.mesh_color_dict[args.webcam_mesh_color]) / 255.
smpl_param_dict = pickle.load(open(
os.path.join(args.smpl_model_path, 'smpl', 'SMPL_NEUTRAL.pkl'),
'rb'),
encoding='latin1')
self.faces = smpl_param_dict['f']
verts_mean = smpl_param_dict['v_template']
self.mesh = o3d.geometry.TriangleMesh()
self.mesh.triangles = o3d.utility.Vector3iVector(self.faces)
self.mesh.vertices = o3d.utility.Vector3dVector(
np.matmul(self.view_mat, verts_mean.T).T * 1000)
self.mesh.compute_vertex_normals()
self.viewer = o3d.visualization.Visualizer()
self.viewer.create_window(width=self.window_size + 1,
height=self.window_size + 1,
window_name='CenterHMR - output')
self.viewer.add_geometry(self.mesh)
view_control = self.viewer.get_view_control()
cam_params = view_control.convert_to_pinhole_camera_parameters()
extrinsic = cam_params.extrinsic.copy()
extrinsic[0:3, 3] = 0
cam_params.extrinsic = extrinsic
#cam_params.intrinsic.set_intrinsics(
# self.window_size, self.window_size, 620.744, 621.151,
# self.window_size//2, self.window_size//2
#)
view_control.convert_from_pinhole_camera_parameters(cam_params)
view_control.set_constant_z_far(1000)
render_option = self.viewer.get_render_option()
render_option.load_from_json('utils/render_option.json')
self.viewer.update_renderer()
self.mesh_smoother = OneEuroFilter(4.0, 0.0)
############ input visualization ############
pygame.init()
self.display = pygame.display.set_mode(
(self.window_size, self.window_size))
pygame.display.set_caption('CenterHMR - input')
def process_photon(self, dir, pos, energy, polerization):
intersect, h_intersect, loc_inter = self.intersect(dir, pos)
distance = distance_point_point(h_intersect,
self.geometry['center'][np.newaxis, :])
if distance == 0.:
# No change of direction for rays through the origin.
# Need to special case this, because rotation axis is not defined
# in this case.
new_ray_dir = h2e(dir)
else:
delta_angle = distance / self.focallength
e_rotation_axis = np.cross(dir[:3], (h_intersect - self.geometry['center'])[:3])
# This is the first step that cannot be done on a stack of photons
# Could have a for "i in photons", but might come up with better way
rot = axangle2mat(e_rotation_axis, delta_angle)
new_ray_dir = np.dot(rot, dir[:3])
return e2h(new_ray_dir, 0), h_intersect, energy, polerization, 1.
def process_photon(self, dir, pos, energy, polerization):
intersect, h_intersect, loc_inter = self.intersect(dir, pos)
distance = distance_point_point(h_intersect,
self.geometry['center'][np.newaxis, :])
if distance == 0.:
# No change of direction for rays through the origin.
# Need to special case this, because rotation axis is not defined
# in this case.
new_ray_dir = h2e(dir)
else:
delta_angle = distance / self.focallength
e_rotation_axis = np.cross(dir[:3], (h_intersect -
self.geometry['center'])[:3])
# This is the first step that cannot be done on a stack of photons
# Could have a for "i in photons", but might come up with better way
rot = axangle2mat(e_rotation_axis, delta_angle)
new_ray_dir = np.dot(rot, dir[:3])
return e2h(new_ray_dir, 0), h_intersect, energy, polerization, 1.
def test_axangle2mat_torch():
B = 8
device = "cuda"
def to_tensor(a):
return torch.tensor(a, dtype=torch.float32, device=device)
np.random.seed(1)
axis = np.random.rand(B, 3)
angle = np.random.rand(B)
axis_tensor = to_tensor(axis)
angle_tensor = to_tensor(angle)
mat_torch = axangle2mat_torch(axis_tensor, angle_tensor, is_normalized=False)
mat_np = []
for i in range(B):
mat_np.append(axangle2mat(axis[i], angle[i]))
mat_np = np.array(mat_np)
print(mat_np)
print(mat_torch)
print(np.allclose(mat_np, mat_torch.cpu().numpy()))
def test_ego_to_allo_v2():
ego_pose = np.zeros((3, 4), dtype=np.float32)
ego_pose[:3, :3] = axangle2mat((1, 2, 3), 1)
ego_pose[:3, 3] = np.array([0.4, 0.5, 0.6])
ego_pose_q = np.zeros((7, ), dtype=np.float32)
ego_pose_q[:4] = mat2quat(ego_pose[:3, :3])
ego_pose_q[4:7] = ego_pose[:3, 3]
ego_poses = {"mat": ego_pose, "quat": ego_pose_q}
rot_types = ["mat", "quat"]
for src_type in rot_types:
dst_type = src_type
allo_pose = egocentric_to_allocentric(ego_poses[src_type], src_type,
dst_type)
ego_pose_1 = allocentric_to_egocentric(allo_pose, dst_type, src_type)
if src_type == "mat":
allo_pose_v2 = ego_to_allo_v2(ego_poses[src_type][:3, :3],
ego_poses[src_type][:3, 3],
rot_type=src_type)
ego_pose_1_v2 = allocentric_to_egocentric(
np.concatenate(
[allo_pose_v2[0], allo_pose_v2[1].reshape(3, 1)], axis=1),
dst_type, src_type)
else:
allo_pose_v2 = ego_to_allo_v2(ego_poses[src_type][:4],
ego_poses[src_type][4:7],
rot_type=src_type)
ego_pose_1_v2 = allocentric_to_egocentric(
np.concatenate(allo_pose_v2, axis=0), dst_type, src_type)
print(src_type, dst_type)
print("ego_pose: ", ego_poses[src_type])
print("allo_pose from ego_pose: ", allo_pose)
print("ego_pose from allo_pose: ", ego_pose_1)
print(np.allclose(ego_poses[src_type], ego_pose_1))
print()
print("allo_pose from ego_pose (v2): ", allo_pose_v2)
print("ego_pose from allo_pose (v2): ", ego_pose_1_v2)
print(np.allclose(ego_poses[src_type], ego_pose_1_v2))
print()
print("************************")
def test_ego_allo():
ego_pose = np.zeros((3, 4), dtype=np.float32)
ego_pose[:3, :3] = axangle2mat((1, 2, 3), 1)
ego_pose[:3, 3] = np.array([0.4, 0.5, 0.6])
ego_pose_q = np.zeros((7, ), dtype=np.float32)
ego_pose_q[:4] = mat2quat(ego_pose[:3, :3])
ego_pose_q[4:7] = ego_pose[:3, 3]
ego_poses = {"mat": ego_pose, "quat": ego_pose_q}
rot_types = ["mat", "quat"]
for src_type in rot_types:
for dst_type in rot_types:
allo_pose = egocentric_to_allocentric(ego_poses[src_type],
src_type, dst_type)
ego_pose_1 = allocentric_to_egocentric(allo_pose, dst_type,
src_type)
print(src_type, dst_type)
print("ego_pose: ", ego_poses[src_type])
print("allo_pose from ego_pose: ", allo_pose)
print("ego_pose from allo_pose: ", ego_pose_1)
print(np.allclose(ego_poses[src_type], ego_pose_1))
print("************************")
def test_groove_direction():
'''Direction of grooves may not be parallel to z axis.'''
photons = generate_test_photons(5)
order1 = OrderSelector([1])
g = FlatGrating(d=1. / 500, order_selector=order1)
e_groove, e_perp_groove, n = g.e_groove_coos(np.ones((2, 1)))
assert np.isclose(np.dot(e_groove[0, :], e_perp_groove[0, :]), 0.)
p = g(photons.copy())
g1 = FlatGrating(d=1. / 500, order_selector=order1, groove_angle=.3)
p1 = g1(photons.copy())
pos3d = axangles.axangle2mat([1, 0, 0], .3)
g2 = FlatGrating(d=1. / 500, order_selector=order1, orientation=pos3d)
p2 = g2(photons.copy())
pos4d = axangles.axangle2aff([1, 0, 0], .1)
g3 = FlatGrating(d=1. / 500,
order_selector=order1,
groove_angle=.2,
pos4d=pos4d)
p3 = g3(photons.copy())
def angle_in_yz(vec1, vec2):
'''project in the y,z plane (the plane of the grating) and calculate angle.'''
v1 = vec1[1:3]
v2 = vec2[1:3]
arccosalpha = np.dot(v1, v2) / np.sqrt(np.dot(v1, v1) * np.dot(v2, v2))
return np.arccos(arccosalpha)
assert np.allclose(angle_in_yz(p1['dir'][0, :], p2['dir'][0, :]),
0,
atol=5e-8)
assert np.allclose(angle_in_yz(p3['dir'][0, :], p2['dir'][0, :]),
0,
atol=5e-8)
for px in [p1, p2, p3]:
assert np.allclose(angle_in_yz(p['dir'][0, :], px['dir'][0, :]), 0.3)
def test_groove_direction():
'''Direction of grooves may not be parallel to z axis.'''
photons = generate_test_photons(5)
g = FlatGrating(d=1. / 500, order_selector=constant_order_factory(1))
assert np.allclose(
np.dot(g.geometry['e_groove'], g.geometry['e_perp_groove']), 0.)
p = g.process_photons(photons.copy())
g1 = FlatGrating(d=1. / 500,
order_selector=constant_order_factory(1),
groove_angle=.3)
p1 = g1.process_photons(photons.copy())
pos3d = axangles.axangle2mat([1, 0, 0], .3)
g2 = FlatGrating(d=1. / 500,
order_selector=constant_order_factory(1),
orientation=pos3d)
p2 = g2.process_photons(photons.copy())
pos4d = axangles.axangle2aff([1, 0, 0], .1)
g3 = FlatGrating(d=1. / 500,
order_selector=constant_order_factory(1),
groove_angle=.2,
pos4d=pos4d)
p3 = g3.process_photons(photons.copy())
def angle_in_yz(vec1, vec2):
'''project in the y,z plane (the plane of the grating) and calculate angle.'''
v1 = vec1[1:3]
v2 = vec2[1:3]
arccosalpha = np.dot(v1, v2) / np.sqrt(np.dot(v1, v1) * np.dot(v2, v2))
return np.arccos(arccosalpha)
assert np.allclose(angle_in_yz(p1['dir'][0, :], p2['dir'][0, :]), 0)
assert np.allclose(angle_in_yz(p3['dir'][0, :], p2['dir'][0, :]), 0)
for px in [p1, p2, p3]:
assert np.allclose(angle_in_yz(p['dir'][0, :], px['dir'][0, :]), 0.3)
