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