def one_window(self):
'''Calculate pos4d matrices for each element on the boom'''
# longeron
zoom = [self.l_longeron / 2, self.d_longeron / 2, self.d_longeron / 2]
trans = [self.l_longeron / 2, self.l_batten / 2, self.l_batten / 2]
rot = np.eye(3)
pos4d = [compose(trans, rot, zoom)]
# batten
zoom = [self.l_batten / 2, self.d_batten / 2, self.d_batten / 2]
trans = [0., self.l_batten / 2, 0]
rot = euler2mat(np.pi / 2, 0, 0, 'syxz')
pos4d.append(compose(trans, rot, zoom))
# diagonal1
zoom = [self.l_diag / 2., self.d_diag / 2., self.d_diag / 2.]
trans = [self.l_longeron / 2., self.l_batten / 2, 0. ]
ang = np.arcsin(self.l_batten / self.l_diag)
rot = euler2mat(ang, 0, 0, 'syxz')
pos4d.append(compose(trans, rot, zoom))
# diagonal2
rot = euler2mat(- ang, 0, 0, 'syxz')
pos4d.append(compose(trans, rot, zoom))
return pos4d
def test_euler_axes():
# Test there and back with all axis specs
aba_perms = [(v[0], v[1], v[0]) for v in permutations('xyz', 2)]
axis_perms = list(permutations('xyz', 3)) + aba_perms
for (a, b, c), mat in zip(euler_tuples, euler_mats):
for rs, axes in product('rs', axis_perms):
ax_spec = rs + ''.join(axes)
conventioned = [EulerFuncs(ax_spec)]
if ax_spec in euler.__dict__:
conventioned.append(euler.__dict__[ax_spec])
mat = euler2mat(a, b, c, ax_spec)
a1, b1, c1 = mat2euler(mat, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
quat = euler2quat(a, b, c, ax_spec)
a1, b1, c1 = quat2euler(quat, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
ax, angle = euler2axangle(a, b, c, ax_spec)
a1, b1, c1 = axangle2euler(ax, angle, ax_spec)
mat_again = euler2mat(a1, b1, c1, ax_spec)
assert_almost_equal(mat, mat_again)
for obj in conventioned:
mat = obj.euler2mat(a, b, c)
a1, b1, c1 = obj.mat2euler(mat)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
quat = obj.euler2quat(a, b, c)
a1, b1, c1 = obj.quat2euler(quat)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
ax, angle = obj.euler2axangle(a, b, c)
a1, b1, c1 = obj.axangle2euler(ax, angle)
mat_again = obj.euler2mat(a1, b1, c1)
assert_almost_equal(mat, mat_again)
def one_window(self):
'''Positions for rods on one side of one bay
Returns
-------
pos4d : list
List of pos4d matrices
'''
# longeron
zoom = [self.l_longeron / 2, self.d_longeron / 2, self.d_longeron / 2]
trans = [self.l_longeron / 2, self.l_batten / 3**0.5, 0.]
rot = np.eye(3)
pos4d = [compose(trans, rot, zoom)]
# batten
zoom = [self.l_batten / 2, self.d_batten / 2, self.d_batten / 2]
trans = [0., self.l_batten / 4. / 3**0.5, self.l_batten / 4]
rot = euler2mat(np.pi / 2, - np.pi / 6, 0, 'szxz')
pos4d.append(compose(trans, rot, zoom))
# diagonal1
zoom = [self.l_diag / 2., self.d_diag / 2., self.d_diag / 2.]
trans[0] = self.l_longeron / 2.
rot = euler2mat(np.deg2rad(90. - 34.03), -np.pi / 6, 0, 'szxz')
pos4d.append(compose(trans, rot, zoom))
# diagonal2
rot = euler2mat(np.deg2rad(90. + 34.03), -np.pi / 6, 0, 'szxz')
pos4d.append(compose(trans, rot, zoom))
return pos4d
def test_mat2euler():
# Test mat2euler function
angles = (4 * math.pi) * (np.random.random(3) - 0.5)
for axes in euler._AXES2TUPLE.keys():
R0 = euler2mat(axes=axes, *angles)
R1 = euler2mat(axes=axes, *mat2euler(R0, axes))
assert_almost_equal(R0, R1)
def test_euler2mat():
# Test mat creation from random angles and round trip
ai, aj, ak = (4 * math.pi) * (np.random.random(3) - 0.5)
for ax_specs in euler._AXES2TUPLE.items():
for ax_spec in ax_specs:
R = euler2mat(ai, aj, ak, ax_spec)
bi, bj, bk = mat2euler(R, ax_spec)
R2 = euler2mat(bi, bj, bk, ax_spec)
assert_almost_equal(R, R2)
def defaultDetector(self, detectorDistance): self.defaultDetectorOrientation = euler2mat(0, 0 , -np.pi/2, 'syxz') self.defaultDetectorOrientation = np.dot(euler2mat(np.pi/2, 0, 0, 'syxz'), self.defaultDetectorOrientation) self.defaultDetectorPosition = np.array([0, detectorDistance, 0]) self.defaultDetectorPos4d = compose(self.defaultDetectorPosition, self.defaultDetectorOrientation, [1, 12.288*3, 12.288*3], np.zeros(3)) # True zoom is [1,12.288,12.288] detector = FlatDetector(pixsize=24.576e-3, pos4d = self.defaultDetectorPos4d) return detector
def defaultMirror(self,reflFile, testedPolarization):
#mirrorData Defaults - reference files
self.reflFile = reflFile
self.testedPolarization = testedPolarization
#mirror Defauls
self.defaultMirrorOrientation = euler2mat(-np.pi/4, 0, 0, 'syxz')
self.defaultMirrorOrientation = np.dot(euler2mat(0,-np.pi/2,0,'syxz'),self.defaultMirrorOrientation)
self.defaultMirrorPosition = np.array([0,0,0])
self.defaultMirrorPos4d = compose(self.defaultMirrorPosition, self.defaultMirrorOrientation, np.array([1, 24.5, 12]), np.zeros(3))
mirror = MultiLayerMirror(self.reflFile, self.testedPolarization,
pos4d = self.defaultMirrorPos4d); return mirror
def photons_dir(self, coos, time):
'''Calculate direction on photons in homogeneous coordinates.
Parameters
----------
coos : `astropy.coordiantes.SkyCoord`
Origin of each photon on the sky
time : np.array
Time for each photons in sec
Returns
-------
photons_dir : np.array of shape (n, 4)
Homogeneous direction vector for each photon
'''
ra = coos.ra.rad
dec = coos.dec.rad
# Minus sign here because photons start at +inf and move towards origin
pointing = self.pointing(time)
photons_dir = np.zeros((len(ra), 4))
photons_dir[:, 0] = - np.cos(dec) * np.cos(ra)
photons_dir[:, 1] = - np.cos(dec) * np.sin(ra)
photons_dir[:, 2] = - np.sin(dec)
for i in range(len(ra)):
mat3d = euler2mat(pointing[i, 0],
- pointing[i, 1],
- pointing[i, 2], 'rzyx')
photons_dir[i, :3] = np.dot(mat3d.T, photons_dir[i, :3])
return photons_dir
def test_Parallel_moving():
'''Test moving center of structure after generation.
'''
pos = [[0, -10.1, -10.1],[0, .1, -10.1],[0, -10.1, .1],[0, .1, .1]]
detkwargs = {'elem_class': CCD,
'elem_args': {'pixsize': 0.01, 'zoom': 5},
'elem_pos': {'position': pos},
'id_col': 'CCD_ID',
}
detect = Parallel(**detkwargs)
detect1 = Parallel(**detkwargs)
move = np.eye(4)
move[2, 3] = 15
detect1.move_center(move)
# Check that the default positions are the same
for i in range(len(detect.elements)):
assert np.allclose(detect.elements[i].pos4d,
detect1.elements[i].pos4d)
# but rotations are applied around a different point
rot = np.eye(4)
rot[:3, :3] = euler2mat(.3, .2, 1.2)
detect.uncertainty = rot
detect.generate_elements()
detect1.uncertainty = rot
detect1.generate_elements()
for i in range(len(detect.elements)):
assert ~np.allclose(detect.elements[i].pos4d,
detect1.elements[i].pos4d)
def write_asol(self, photons, asolfile, timestep=0.256):
time = np.arange(0, photons.meta['EXPOSURE'][0], timestep)
pointing = np.rad2deg(self.pointing(time))
asol = Table([time, pointing[:, 0], pointing[:, 1], pointing[:, 2]],
names=['time', 'ra', 'dec', 'roll'],
)
asol['time'].unit = 's'
# The following columns represent measured offsets in Chandra
# They are not part of this simulation. Simply set them to 0
for col in [ 'ra_err', 'dec_err', 'roll_err',
'dy', 'dz', 'dtheta', 'dy_err', 'dz_err', 'dtheta_err',
'roll_bias', 'pitch_bias', 'yaw_bias', 'roll_bias_err', 'pitch_bias_err', 'yaw_bias_err']:
asol[col] = np.zeros_like(time)
if 'bias' in col:
asol[col].unit = 'deg / s'
elif ('dy' in col) or ('dz' in col):
asol[col].unit = 'mm'
else:
asol[col].unit = 'deg'
asol['q_att'] = [mat2quat(euler2mat(np.deg2rad(p[0]),
np.deg2rad(-p[1]),
np.deg2rad(-p[2]), 'rzyx'))
for p in pointing]
# Copy info like the exposure time from the photons list meta to asol,
# but not column specific keywords like TTYPEn, TCTYPn, MTYPEn, MFORMn, etc:
for k in photons.meta:
if (('TYP' not in k) and ('FORM' not in k) and
('TCR' not in k) and ('TCDEL' not in k) and (k not in asol.meta)):
asol.meta[k] = photons.meta[k]
asol.meta['EXTNAME'] = 'ASPECT'
complete_header(asol.meta, None, 'ACASOL', ['OGIP', 'TEMPORALDATA', 'ASPECT'])
# In MARXS t=0 is the start of the observation, but for Chandra we need to make that
# consistent with the value of the TSTART keyword.
asol['time'] += asol.meta['TSTART'][0]
asol.write(asolfile, format='fits') # , checksum=True) - works only with fits interface
def photons_dir(self, ra, dec, time):
'''Calculate direction on photons in homogeneous coordinates.
Parameters
----------
ra : np.array
RA for each photon in rad
dec : np.array
DEC or each photon in rad
time : np.array
Time for each photons in sec
Returns
-------
photons_dir : np.array of shape (n, 4)
Homogeneous direction vector for each photon
'''
# Minus sign here because photons start at +inf and move towards origin
pointing = self.pointing(time)
photons_dir = np.zeros((len(ra), 4))
photons_dir[:, 0] = - np.cos(dec) * np.cos(ra)
photons_dir[:, 1] = - np.cos(dec) * np.sin(ra)
photons_dir[:, 2] = - np.sin(dec)
for i in range(len(ra)):
mat3d = euler2mat(pointing[i, 0],
- pointing[i, 1],
- pointing[i, 2], 'rzyx')
photons_dir[i, :3] = np.dot(mat3d.T, photons_dir[i, :3])
return photons_dir
def from_euler_rad(x, y, z):
"""
return 3x3 rotation matrix
rotation is intrinsic xyz rotation
(rotations around axes of rotating coordinate system).
"""
return euler.euler2mat(x, y, z, axes="rxyz")
def generate_facet_uncertainty(n, xyz, angle_xyz):
'''Generate 4d matrices that represent facet misalignment.
Positional and rotational uncertainties are input to this function. It then
draws randomnly from Gaussians centered on 0 (the correct position) for the displacement
and rotation, where the :math:`\sigma` of the Gaussian is given by the numbers in the input.
The linear displacements and angles are expressed as (4,4) matrixes suitable for use with
homogeneous coordinates.
Parameters
----------
n : int
Number of 4d matrixes to be calculated
xyz : tuple of 3 floats
accuracy of grating positioning in x, y, z (in mm) - Gaussian sigma, not FWHM!
angle_xyz : tuple of 3 floats
accuracy of grating positioning. Rotation around x, y, z (in rad) - Gaussian sigma, not FWHM!
Returns
-------
pos_uncert : list of n (4,4) np.arrays
Random realizations of the uncertainty
'''
translation = np.random.normal(size=(n, 3)) * np.asanyarray(xyz)[np.newaxis, :]
rotation = np.random.normal(size=(n, 3)) * np.asanyarray(angle_xyz)[np.newaxis, :]
return [affines.compose(t, euler.euler2mat(a[0], a[1], a[2], 'sxyz'), np.ones(3))
for t, a in zip(translation, rotation)]
def __init__(self, **kwargs):
self.coords = kwargs.pop('coords')
self.ra = self.coords[0]
self.dec = self.coords[1]
if 'roll' in kwargs:
self.roll = kwargs.pop('roll')
else:
self.roll = 0.
super(FixedPointing, self).__init__(**kwargs)
'''Transform spacecraft to fixed sky system.
This matrix transforms from the spacecraft system to a
right-handed Cartesian system that is defined in the following
way: the (x,y) plane is defined by the celestial equator, and
the x-axis points to :math:`(\alpha, \delta) = (0,0)`.
Implementation notes
--------------------
Negative sign for dec and roll, because these are defined
opposite to the right hand rule.
Note: I hope I figured it out right. If not, I can always bail
and use astropy.coordinates for this.
'''
self.mat3d = euler2mat(np.deg2rad(self.ra),
np.deg2rad(-self.dec),
np.deg2rad(-self.roll), 'rzyx')
def _rotate_vector(self, vector, roll, pitch, yaw, degrees=True):
if degrees:
pitch = utilities.degrees_to_radians(pitch)
roll = utilities.degrees_to_radians(roll)
yaw = utilities.degrees_to_radians(yaw)
rotation_matrix = euler.euler2mat(roll, pitch, yaw, 'sxyz')
rotated_vector = rotation_matrix.dot(vector)
return(rotated_vector)
def __init__(self, firstMirrorREFL = './mirror_files/A12113.txt', firstMirrorPOL = './mirror_files/ALSPolarization.txt', secondMirrorREFL= './mirror_files/A12113.txt', secondMirrorPOL = './mirror_files/ALSPolarization.txt'): self.first = SourceMLMirror(firstMirrorREFL, firstMirrorPOL) self.second = MLMirrorDetector(secondMirrorREFL, secondMirrorPOL) self.distanceBetweenHalves = 500 self.angleOffset = 0 Rotation = euler2mat(0,0,0,'syxz') FirstHalfPlacement = compose([-self.distanceBetweenHalves,0,0], Rotation, [1,1,1], [0,0,0]) #This will move the first half away from the second by 50 mm self.first.offset_apparatus(FirstHalfPlacement)
def pos_spec(self):
'''Calculate pos4d matrices for each element on the boom'''
fullboompos4d = []
for h in range(self.start_bay, self.n_bays):
trans = [h * self.l_longeron, 0, 0]
for i in range(self.n_sides):
rot = euler2mat((i * 2) * np.pi / self.n_sides, 0, 0, 'sxyz')
affmat = compose(trans, rot, np.ones(3))
for p in self.one_window():
fullboompos4d.append(np.dot(affmat, p))
return fullboompos4d
def write_asol(self, photons, asolfile, timestep=0.256):
'''Write an aspect solution (asol) file
Chandra analysis scripts often require an aspect solution, which is essientiall
a list of pointing directions in the dither pattern vs. time.
This method write such a list to a file.
Parameters
----------
photons : `astropy.table.Table` or `astropy.table.Row`
Table with photon properties. Some meta data from the header of this table
is required (e.g. the length of the observation).
asolfile : string
Path and file name where the asol file is saved.
timestamp : float
Time step between entries in the asol file in seconds.
'''
time = np.arange(0, photons.meta['EXPOSURE'][0], timestep)
pointing = self.pointing(time).to(u.deg).value
asol = Table([time, pointing[:, 0], pointing[:, 1], pointing[:, 2]],
names=['time', 'ra', 'dec', 'roll'],
)
asol['time'].unit = 's'
# The following columns represent measured offsets in Chandra
# They are not part of this simulation. Simply set them to 0
for col in [ 'ra_err', 'dec_err', 'roll_err',
'dy', 'dz', 'dtheta', 'dy_err', 'dz_err', 'dtheta_err',
'roll_bias', 'pitch_bias', 'yaw_bias', 'roll_bias_err', 'pitch_bias_err', 'yaw_bias_err']:
asol[col] = np.zeros_like(time)
if 'bias' in col:
asol[col].unit = 'deg / s'
elif ('dy' in col) or ('dz' in col):
asol[col].unit = 'mm'
else:
asol[col].unit = 'deg'
asol['q_att'] = [mat2quat(euler2mat(np.deg2rad(p[0]),
np.deg2rad(-p[1]),
np.deg2rad(-p[2]), 'rzyx'))
for p in pointing]
# Copy info like the exposure time from the photons list meta to asol,
# but not column specific keywords like TTYPEn, TCTYPn, MTYPEn, MFORMn, etc:
for k in photons.meta:
if (('TYP' not in k) and ('FORM' not in k) and
('TCR' not in k) and ('TCDEL' not in k) and (k not in asol.meta)):
asol.meta[k] = photons.meta[k]
asol.meta['EXTNAME'] = 'ASPECT'
complete_header(asol.meta, None, 'ACASOL', ['OGIP', 'TEMPORALDATA', 'ASPECT'])
# In MARXS t=0 is the start of the observation, but for Chandra we need to make that
# consistent with the value of the TSTART keyword.
asol['time'] += asol.meta['TSTART'][0]
asol.write(asolfile, format='fits') # , checksum=True) - works only with fits interface
def test_double_reflection(angle, avgpol):
'''Two mirrors
The first mirror polarizes the light and the second
one perfectly reflects that polarized light or perfectly absorbs it.
'''
pos = np.tile([1., 0., 0., 1.], (8, 1))
dir = np.tile([-1., 0., 0., 0.], (8, 1))
ang = np.arange(0, 2. * np.pi, np.pi / 4)
polarization = np.vstack([np.zeros(8), np.sin(ang), np.cos(ang), np.zeros(8)]).T
photons = Table({'pos': pos,
'dir': dir,
'energy': np.ones(8),
'polarization': polarization,
'probability': np.ones(8)})
ml1 = FlatBrewsterMirror(orientation=euler.euler2mat(np.pi / 4, 0, 0, 'szxy'))
ml2 = FlatBrewsterMirror(position=[0, 1, 0],
orientation=euler.euler2mat(-np.pi / 4, angle, 0, 'szyx'))
photons = ml1(photons)
photons = ml2(photons)
assert np.isclose(np.mean(photons['probability']), avgpol)
def __init__(self, **kwargs):
self.coords = kwargs.pop('coords')
self.ra = self.coords[0]
self.dec = self.coords[1]
self.roll = kwargs.pop('roll', 0.)
super(FixedPointing, self).__init__(**kwargs)
'''
Negative sign for dec and roll in the code, because these are defined
opposite to the right hand rule.
'''
self.mat3d = euler2mat(np.deg2rad(self.ra),
np.deg2rad(-self.dec),
np.deg2rad(-self.roll), 'rzyx')
def test_L2_Abs_angle():
'''Test that that shadow area increases with angle.
The comparison number was calculated by H. M. Guenther in response
to Arcus DQ36.'''
l2_dims = {'period': 0.966 * u.mm, 'bardepth': 0.5 * u.mm,
'barwidth': 0.1 * u.mm}
l2 = L2Abs(l2_dims=l2_dims)
p1 = l2(generate_test_photons(1))
l2 = L2Abs(l2_dims=l2_dims, orientation=euler2mat(np.deg2rad(1.8), 0, 0, 'szxy'))
p2 = l2(generate_test_photons(1))
assert np.isclose(p2['probability'][0] / p1['probability'][0], 0.979, rtol=1e-3)
def moveglobal(e, dx=0, dy=0, dz=0, rx=0., ry=0., rz=0.):
'''Move and rotate origin of the whole `marxs.simulator.Parallel` object.
Parameters
----------
e :`marxs.simulator.Parallel` or list of those elements
Elements where uncertainties will be set
dx, dy, dz : float
translation in x, y, z (in mm)
rx, ry, rz : float
Rotation around x, y, z (in rad)
'''
e.uncertainty = affines.compose([dx, dy, dz],
euler.euler2mat(rx, ry, rz, 'sxyz'),
np.ones(3))
e.generate_elements()
def moveindividual(e, dx=0, dy=0, dz=0, rx=0, ry=0, rz=0):
'''Move and rotate all elements of `marxs.simulator.Parallel` object.
Unlike `~marxs.design.tolerancing.moveglobal` this does not move to
center of the `~marxs.simulator.Parallel` object, instead it moves
all elements individually. Unlike `~marxs.design.tolerancing.wiggle`,
each element is moved by the same amount, not a number drawn from from
distribution.
Parameters
----------
e :`marxs.simulator.Parallel` or list of those elements
Elements where uncertainties will be set
dx, dy, dz : float
translation in x, y, z (in mm)
rx, ry, rz : float
Rotation around x, y, z (in rad)
'''
e.elem_uncertainty = [affines.compose((dx, dy, dz),
euler.euler2mat(rx, ry, rz, 'sxyz'),
np.ones(3))] * len(e.elements)
e.generate_elements()
def run(self, exposureTime = 1000, IntermediateDetector=False): # Generating photons that travel down the beamline print "Generating Cross Photons..." cross = self.first.generate_photons(exposureTime) if IntermediateDetector: # place large detector midwat between the mirrors # correct Location mirrorPosition = np.dot(self.first.pos4d, self.first.mirror.geometry['center']) detectorLocation = mirrorPosition/2 detectorLocation = detectorLocation[0:3] # Correct Orientation detectorRotation = euler2mat(0,-np.pi/2,0,'syxz') # Combine detectorPos4d = compose(detectorLocation, detectorRotation, [1, 3*12.288,3*12.288], np.zeros(3)) detector = FlatDetector(pixsize=24.576e-3, pos4d = detectorPos4d) self.intermediateResults = detector.process_photons(cross) self.intermediateResults = self.intermediateResults[self.intermediateResults['probability']>0] # Receive Photons on other side print "Receiving Photons..." self.results = self.second.detect_photons(cross) if IntermediateDetector: return self.results, self.intermediateResults else: return self.results
@pytest.mark.parametrize('structure, rot_list', [(create_Hex(), [(0, 0, 10),
(0, 0, 0)])])
def test_plot_best_matching_results_on_signal_vector(structure, rot_list, edc):
# Just test that the code runs
library, match_results = get_vector_match_results(structure, rot_list, edc)
# Hyperspy can only add markers to square signals
match_results.data = np.vstack((match_results.data, match_results.data))
dp = ElectronDiffraction(2 * [2 * [np.zeros((144, 144))]])
match_results.plot_best_matching_results_on_signal(dp, library=library)
@pytest.mark.parametrize(
"structure, rotation",
[(create_wurtzite(), euler2mat(0, np.pi / 2, 0, 'rzxz'))])
def test_kinematic_intensities_rotation(structure, rotation):
"""Test that kinematically forbidden diffraction spots gets zero intensity also after rotation."""
rotated_lattice = diffpy.structure.lattice.Lattice(structure.lattice)
rotated_lattice.setLatPar(baserot=rotation)
structure.placeInLattice(rotated_lattice)
reciprocal_lattice = structure.lattice.reciprocal()
g_indices = [(0, 0, 1)]
g_hkls = reciprocal_lattice.dist(g_indices, [0, 0, 0])
scattering_params_list = ['lobato', 'xtables']
for scattering_params in scattering_params_list:
intensities = get_kinematical_intensities(
structure,
g_indices,
g_hkls,
###############################################################
### Establish the base coordinate frame for the gesture sample
# We can either use the first of the last frame of the gesture as the
# transformation basis. For the following two lines, comment out the one
# you don't want.
zeroind = 0 #FIRST
# zeroind = np.append(np.argwhere(np.all(sample==0,axis=1)), sample.shape[0])[0] - 1 #LAST
f0 = sample[zeroind] # frame zero
### YAW CORRECTION ONLY (ORIGINAL SOLUTION)
# Create basis homogeneous transformation matrix
t0 = np.eye(4)
t0[:3, :3] = euler.euler2mat(f0[3], 0, 0, 'rzyx')
t0[:3, 3] = f0[:3]
# for each gesture frame
for j, frame in enumerate(sample):
if np.all(frame == 0): break
t1 = np.eye(4)
t1[:3, :3] = euler.euler2mat(frame[3], frame[4], frame[5], axes='rzyx')
t1[:3, -1] = frame[:3]
t2 = np.dot(Tinv(t0), t1)
framep = frame
framep[:3] = t2[:3, -1]
framep[3:6] = euler.mat2euler(t2[:3, :3], axes='rzyx')
X[i, j] = framep
### QUATERNION IMPLEMENTATION
[(0.025, 0.2, np.array([[0, 0], [0, 1], [1, 1]]),
np.array([[0, 0, 1 / 0.025 - 40], [
0, 1, np.sqrt(1 / (0.025**2) - 1) - 40
], [1, 1, np.sqrt(1 / (0.025**2) - 1 - 1) - 40]]))])
def test_detector_to_fourier(wavelength, camera_length, detector_coords,
k_expected):
k = detector_to_fourier(detector_coords, wavelength, camera_length)
np.testing.assert_allclose(k, k_expected)
@pytest.mark.parametrize(
'from_v1, from_v2, to_v1, to_v2, expected_rotation',
[
# v2 from x to y
([0.0, 0.0, 1.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0], [0.0, 1.0, 0.0],
euler2mat(*np.deg2rad([90, 0, 0]), 'rzxz')),
# Degenerate to-vectors gives half-way rotation (about y-axis)
([0.0, 0.0, 1.0], [1.0, 0.0, 0.0], [1.0, 0.0, 0.0], [2.0, 0.0, 0.0],
euler2mat(*np.deg2rad([90, 45, -90]), 'rzxz')),
# Edges to body diagonals
([0.0, 0.0, 1.0], [1.0, 0.0, 0.0], [0.5, -0.5, 1 / np.sqrt(2)], [
1 / np.sqrt(2), 1 / np.sqrt(2), 0
], euler2mat(*np.deg2rad([45, 45, 0]), 'rzxz'))
])
def test_get_rotation_matrix_between_vectors(from_v1, from_v2, to_v1, to_v2,
expected_rotation):
rotation_matrix = get_rotation_matrix_between_vectors(
np.array(from_v1), np.array(from_v2), np.array([to_v1]),
np.array([to_v2]))
np.testing.assert_allclose(rotation_matrix,
np.array([expected_rotation]),
def get_array(self, num_needles, degree, shape, occult=True, std=None, mean=None):
#################
## generate synthetic labelmap
##################
random.shuffle(self.needles)
labelmap_syn = np.zeros(shape,int)## int may need to be replaced by float
count_needle=0
i=-1
while count_needle < num_needles:
i+=1
needle = self.needles[np.random.randint(len(self.needles))]
xs, ys, zs = np.where(needle == 1)
I = np.asarray([[x, y, z] for x, y, z in zip(xs,ys,zs)])
I = np.transpose(I)
x_angle = (random.random()-0.5)*2 *degree*np.pi/180
y_angle = (random.random()-0.5)*2 *degree*np.pi/180
z_angle = (random.random()-0.5)*2 *degree*np.pi/180
R = euler2mat(x_angle, y_angle, z_angle, 'sxyz')
try:
M = np.transpose(np.dot(R,I)).astype(int)
except ValueError:
print("Empty Needle",self.needlepaths[i])
continue
needle_rot = np.zeros(needle.shape,int)
for x,y,z in M:
try:
needle_rot[x][y][z] = 1
except IndexError:
continue
needle_crop = self.labelmap_resize(needle_rot, shape)
if np.sum(needle_crop)>800:
count_needle+=1
labelmap_syn = labelmap_syn + needle_crop ## There will be chance of needle overlap
if occult:
square = np.ones((20,20,20))
squares = np.zeros(shape)
for i in range(20):
sq = np.where(square!=0)
sqx = sq[0] + np.random.randint(0,148)
sqy = sq[1] + np.random.randint(0,148)
sqz = sq[2] + np.random.randint(0,148)
squares[np.clip(sqx,0,shape[0]-1), np.clip(sqy,0,shape[1]-1), np.clip(sqz,0,shape[2]-1)] = 1
#################
## generate synthetic case
##################
if std == None:
std = np.std(self.cases[0])
if mean == None:
mean = np.mean(self.cases[0])
case_syn = np.random.normal(np.log(std), mean, shape)
case_syn = gaussian_filter(case_syn, sigma=6)
case_syn2 = np.random.normal(np.log(std), mean, shape)
case_syn2 = gaussian_filter(case_syn, sigma=3)
case_syn1 = np.random.normal(np.log(std), mean, shape)
case_syn1 = gaussian_filter(case_syn, sigma=2)
case_syn = case_syn1*case_syn2*case_syn
case_syn /= np.mean(case_syn)
case_syn *= mean
case_syn = np.clip(case_syn,0,500)
VAL_NEEDLE = np.random.randint(np.int(np.mean(case_syn)/3))
if occult:
case_syn[(labelmap_syn != 0) & (squares == 0)] = VAL_NEEDLE
else:
case_syn[labelmap_syn != 0] = VAL_NEEDLE
case_syn = gaussian_filter(case_syn, sigma=0.7)
return labelmap_syn,case_syn
def get_diffraction_library(self,
structure_library,
calibration,
reciprocal_radius,
half_shape,
with_direct_beam=True):
"""Calculates a dictionary of diffraction data for a library of crystal
structures and orientations.
Each structure in the structure library is rotated to each associated
orientation and the diffraction pattern is calculated each time.
Angles must be in the Euler representation (Z,X,Z) and in degrees
Parameters
----------
structure_library : pyxem:StructureLibrary Object
Dictionary of structures and associated orientations for which
electron diffraction is to be simulated.
calibration : float
The calibration of experimental data to be correlated with the
library, in reciprocal Angstroms per pixel.
reciprocal_radius : float
The maximum g-vector magnitude to be included in the simulations.
half_shape: tuple
The half shape of the target patterns, for 144x144 use (72,72) etc
Returns
-------
diffraction_library : :class:`DiffractionLibrary`
Mapping of crystal structure and orientation to diffraction data
objects.
"""
# Define DiffractionLibrary object to contain results
diffraction_library = DiffractionLibrary()
# The electron diffraction calculator to do simulations
diffractor = self.electron_diffraction_calculator
# Iterate through phases in library.
for key in structure_library.struct_lib.keys():
phase_diffraction_library = dict()
structure = structure_library.struct_lib[key][0]
a, b, c, alpha, beta, gamma = structure.lattice.abcABG()
orientations = structure_library.struct_lib[key][1]
# Iterate through orientations of each phase.
for orientation in tqdm(orientations, leave=False):
_orientation = np.deg2rad(orientation)
matrix = euler2mat(_orientation[0],
_orientation[1],
_orientation[2], 'rzxz')
latt_rot = diffpy.structure.lattice.Lattice(a, b, c,
alpha, beta, gamma,
baserot=matrix)
# Don't change the original structure
structure_rotated = diffpy.structure.Structure(structure)
structure_rotated.placeInLattice(latt_rot)
# Calculate electron diffraction for rotated structure
data = diffractor.calculate_ed_data(structure_rotated,
reciprocal_radius,
with_direct_beam)
# Calibrate simulation
data.calibration = calibration
pattern_intensities = data.intensities
pixel_coordinates = np.rint(
data.calibrated_coordinates[:, :2] + half_shape).astype(int)
# Construct diffraction simulation library, removing those that
# contain no peaks
if len(pattern_intensities) > 0:
phase_diffraction_library[tuple(orientation)] = \
{'Sim': data, 'intensities': pattern_intensities,
'pixel_coords': pixel_coordinates,
'pattern_norm': np.sqrt(np.dot(pattern_intensities,
pattern_intensities))}
diffraction_library[key] = phase_diffraction_library
# Pass attributes to diffraction library from structure library.
diffraction_library.identifiers = structure_library.identifiers
diffraction_library.structures = structure_library.structures
return diffraction_library
print('Recovered transform = ')
print(result.transformation)
draw_registration_result(source, target, source_mesh, result.transformation)
return result.transformation.copy()
if __name__ == '__main__':
size = 10
source = np.random.uniform(size=(8, 3)) * size
permute_idx = np.random.permutation(len(source))
target = source.copy()[permute_idx]
target = np.vstack((target, np.random.uniform(size=(3,3))*size))
permute_idx = np.random.permutation(len(target))
target = target[permute_idx]
R = txe.euler2mat(*(2*np.pi*np.random.uniform(size=3)))
T = np.eye(4)
T[:3, :3] = R
T[:3, 3] = np.random.uniform(size=3)*size/2
src = o3d.geometry.PointCloud()
src.points = o3d.utility.Vector3dVector(source)
dst = o3d.geometry.PointCloud()
dst.points = o3d.utility.Vector3dVector(target)
dst.transform(T)
register(src, dst, len(source))
print('Original transform = ')
print(T)
def set_cartesian_euler(self, position, euler_angles):
rot_mat = euler2mat(radians(euler_angles[0]), radians(euler_angles[1]),
radians(euler_angles[2]))
orientation = mat2quat(rot_mat).tolist()
pose = [position, orientation]
self.set_cartesian(pose)
def get_joints_data(self, in_worldspace=False, default=False):
"""
:param in_worldspace: Whether to get the position in worldspace.
:type in_worldspace: bool
:param default: Whether to get the default positions of the joints or current frame.
:type default: bool
:return: A dictionary of joint_label: (parent_label, position, rotation, joint type, children).
:rtype: dict
"""
if default:
joint_trans = self._skeleton.get_frame(0)
else:
joint_trans = self._skeleton.get_frame(self.frame)
joint_dict = dict()
def get_pos(joint_name, is_worldspace):
if self.skeleton_dict[joint_name][3] == 'root': # Make sure root is always set to worldpos.
return joint_trans[joint_name][1]
if is_worldspace:
return joint_trans[joint_name][1]
else:
return self._skeleton.get_offsets()[joint_name]
def get_rot_ordered(rot, order='sxyz'):
if order[1:] == 'zyx':
return rot[2], rot[1], rot[0]
elif order[1:] == 'zxy':
return rot[2], rot[0], rot[1]
elif order[1:] == 'yxz':
return rot[1], rot[0], rot[2]
elif order[1:] == 'xzy':
return rot[0], rot[2], rot[1]
elif order[1:] == 'yzx':
return rot[1], rot[2], rot[0]
else:
return rot[0], rot[1], rot[2]
def get_xyz_rot(rot, order='sxyz'):
if order[1:] == 'zyx':
return rot[2], rot[1], rot[0]
elif order[1:] == 'zxy':
return rot[1], rot[2], rot[0]
elif order[1:] == 'yxz':
return rot[1], rot[0], rot[2]
elif order[1:] == 'xzy':
return rot[0], rot[2], rot[1]
elif order[1:] == 'yzx':
return rot[2], rot[0], rot[1]
else:
return rot[0], rot[1], rot[2]
for joint, data in six.iteritems(self.skeleton_dict):
rot = joint_trans[joint][0]
if rot is not None:
# This is for testing different joint rotation orders.
in_order = 'syxz'
out_order = 'sxyz'
rot = np.deg2rad(rot)
m = euler2mat(*get_rot_ordered(rot, in_order), axes=in_order)
rot = np.rad2deg(mat2euler(m, axes=out_order))
rot = get_xyz_rot(rot, out_order)
joint_dict[joint] = (data[0], get_pos(joint, in_worldspace), rot, data[3], data[4])
return joint_dict
angle_lift(sample[:, 3], int(n))
angle_lift(sample[:, 4], int(n))
angle_lift(sample[:, 5], int(n))
###############################################################
### Establish the base coordinate frame for the gesture sample
# We can either use the first of the last frame of the gesture as the
# transformation basis. For the following two lines, comment out the one
# you don't want.
zeroind = 0 #FIRST
# zeroind = np.append(np.argwhere(np.all(sample==0,axis=1)), sample.shape[0])[0] - 1 #LAST
f0 = sample[zeroind].copy() # frame zero
### YAW CORRECTION
r0p = np.matrix(euler.euler2mat(f0[3], 0, 0, 'rzyx'))**-1
p0 = np.matrix(f0[:3].reshape((-1, 1)))
# for each gesture frame
for j, frame in enumerate(sample):
if np.all(frame == 0): break # stop at zero padding start
p1 = np.matrix(frame[:3].reshape((-1, 1)))
p2 = r0p * (p1 - p0)
frame[:3] = np.squeeze(p2)
frame[3] = frame[3] - f0[3]
# Print progress
stdout.write('\r% 5.1f%%' % ((i + 1) / (X.shape[0]) * 100))
stdout.write('\n')
#%% SET SPLITTTING
def rotation_matrix(angle, rotation_axis):
rot_angle = rotation_axis * angle
M = np.eye(4)
M[:3, :3] = euler2mat(*np.deg2rad(rot_angle))
return M
def blender_euler_to_blender_pose(euler):
azi = -euler[0] - 90
ele = euler[1] + 90
theta = euler[2]
pose = euler2mat(azi, ele, theta, axes='szxz')
return pose
def step_controller(controller, robot_config, quat_orientation):
"""Steps the controller forward one timestep
Parameters
----------
controller : Controller
Robot controller object.
"""
if controller.state == BehaviorState.TROT:
controller.foot_locations, controller.contact_modes = step(
controller.ticks,
controller.foot_locations,
controller.swing_params,
controller.stance_params,
controller.gait_params,
controller.movement_reference,
)
# Apply the desired body rotation
# foot_locations = (
# euler2mat(
# controller.movement_reference.roll, controller.movement_reference.pitch, 0.0
# )
# @ controller.foot_locations
# )
# Disable joystick-based pitch and roll for trotting with IMU feedback
foot_locations = controller.foot_locations
# Construct foot rotation matrix to compensate for body tilt
(roll, pitch, yaw) = quat2euler(quat_orientation)
correction_factor = 0.8
max_tilt = 0.4
roll_compensation = correction_factor * np.clip(
roll, -max_tilt, max_tilt)
pitch_compensation = correction_factor * np.clip(
pitch, -max_tilt, max_tilt)
rmat = euler2mat(roll_compensation, pitch_compensation, 0)
foot_locations = rmat.T @ foot_locations
controller.joint_angles = four_legs_inverse_kinematics(
foot_locations, robot_config)
elif controller.state == BehaviorState.HOP:
hop_foot_locations = (controller.stance_params.default_stance +
np.array([0, 0, -0.09])[:, np.newaxis])
controller.joint_angles = four_legs_inverse_kinematics(
hop_foot_locations, robot_config)
elif controller.state == BehaviorState.FINISHHOP:
hop_foot_locations = (controller.stance_params.default_stance +
np.array([0, 0, -.22])[:, np.newaxis])
controller.joint_angles = four_legs_inverse_kinematics(
hop_foot_locations, robot_config)
elif controller.state == BehaviorState.REST:
if controller.previous_state != BehaviorState.REST:
controller.smoothed_yaw = 0
yaw_factor = -0.25
controller.smoothed_yaw += controller.gait_params.dt * clipped_first_order_filter(
controller.smoothed_yaw,
controller.movement_reference.wz_ref * yaw_factor, 1.5, 0.25)
# Set the foot locations to the default stance plus the standard height
controller.foot_locations = (
controller.stance_params.default_stance + np.array(
[0, 0, controller.movement_reference.z_ref])[:, np.newaxis])
# Apply the desired body rotation
rotated_foot_locations = (
euler2mat(controller.movement_reference.roll,
controller.movement_reference.pitch,
controller.smoothed_yaw) @ controller.foot_locations)
controller.joint_angles = four_legs_inverse_kinematics(
rotated_foot_locations, robot_config)
controller.ticks += 1
controller.previous_state = controller.state
def test_detector_to_fourier(wavelength, camera_length, detector_coords,
k_expected):
k = detector_to_fourier(detector_coords, wavelength, camera_length)
np.testing.assert_allclose(k, k_expected)
@pytest.mark.parametrize(
"from_v1, from_v2, to_v1, to_v2, expected_rotation",
[
# v2 from x to y
(
[0.0, 0.0, 1.0],
[1.0, 0.0, 0.0],
[0.0, 0.0, 1.0],
[0.0, 1.0, 0.0],
euler2mat(*np.deg2rad([90, 0, 0]), "rzxz"),
),
# Degenerate to-vectors gives half-way rotation (about y-axis)
(
[0.0, 0.0, 1.0],
[1.0, 0.0, 0.0],
[1.0, 0.0, 0.0],
[2.0, 0.0, 0.0],
euler2mat(*np.deg2rad([90, 45, -90]), "rzxz"),
),
# Edges to body diagonals
(
[0.0, 0.0, 1.0],
[1.0, 0.0, 0.0],
[0.5, -0.5, 1 / np.sqrt(2)],
[1 / np.sqrt(2), 1 / np.sqrt(2), 0],
def unwarp(img, src, dst):
h, w = img.shape[:2]
src1 = []
dst1 = []
print("src", src)
print("src[0]", src[0])
# print("dst:", dst)
# for i in range(0,5):
# cv2.circle(img, tuple(src), 1, (100, 100, 100), thickness=3, lineType=8, shift=0)
src1.append(src[0])
src1.append(src[1])
src1.append(src[2])
src1.append(src[3])
dst1.append(dst[0])
dst1.append(dst[1])
dst1.append(dst[2])
dst1.append(dst[3])
H, _ = cv2.findHomography(np.asarray(src1, dtype=np.float32),
np.asarray(dst1, dtype=np.float32),
method=cv2.RANSAC,
ransacReprojThreshold=5.0)
# print('\nThe homography matrix is: \n', H)
theta = -math.atan2(H[0, 1], H[0, 0]) * 180 / math.pi
u, _, vh = np.linalg.svd(H[0:2, 0:2])
R = u @ vh
angle = math.atan2(R[1, 0], R[0, 0])
print("theta: ", theta)
# print("angle: ", angle)
# p1 = H * src[0]
un_warped = cv2.warpPerspective(img, H, (w, h), flags=cv2.INTER_LINEAR)
pig = cv2.imread("images/pig.png")
# M = cv2.getPerspectiveTransform(src, dst)
# un_warp_pig = cv2.warpPerspective(img, M, (w, h), flags=cv2.INTER_LINEAR)
# print("un_wrap_pig", un_warped)
# result = cv2.perspectiveTransform(src[None, :, :], M)
# print("result", result)
# # cv2.imshow("unwarppig1", un_warp_pig)
# cv2.imshow("what", un_warped)
ang = rot2eul(H)
print("ang", ang)
hello = eul2rot(ang)
# print("hello", hello)
sin_list = np.sin(hello)
cos_list = np.cos(hello)
A = np.eye(3)
c = 0
for i in range(1, 3):
for j in range(i):
ri = np.copy(A[i])
rj = np.copy(A[j])
A[i] = cos_list[c] * ri + sin_list[c] * rj
A[j] = -sin_list[c] * ri + cos_list[c] * rj
c += 1
# print("A.T", A.T)
import transforms3d.euler as eul
ang = eul.mat2euler(H, axes='sxyz')
# print("ang2", ang)
what = eul.euler2mat(ang[0], ang[1], ang[2], axes='sxyz')
# print("what112", what)
# overlay_image(img, pig, 230, 250, theta, calcaulate_dist_diff(img))
# plot
# f, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 8))
# # f.subplots_adjust(hspace=.2, wspace=.05)
# ax1.imshow(img)
# ax1.set_title('Original Image')
#
# x = [src[0][0], src[1][0], src[2][0], src[3][0], src[0][0]]
# y = [src[0][1], src[1][1], src[2][1], src[3][1], src[0][1]]
f, (ax1, ax2) = plt.subplots(1, 2, figsize=(15, 8))
# f.subplots_adjust(hspace=.2, wspace=.05)
ax1.imshow(img)
ax1.set_title('Original Image')
# x = [src[0][0], src[2][0], src[3][0], src[1][0], src[0][0]]
# y = [src[0][1], src[1][1], src[2][1], src[3][1], src[0][1]]
# ax2.imshow(img)
# ax2.plot(x, y, color='yellow', linewidth=3)
# ax2.set_ylim([h, 0])
# ax2.set_xlim([0, w])
# ax2.set_title('Target Area')
#
# plt.show()
return un_warped
[0, 100, 0], [0, 0, 100] ]) Ipv = np.array([1,100,100]) Rf = np.array([-0.3, 0, 0]) Rt = np.array([-0.3, 0, 0]) R0 = 6400000 g0 = -9.81 m = 1 pos0 = euler.euler2mat(0, 0, np.pi/2, axes='sxyz') def Thurst(y, t): thurst = np.array([0,0,0]) if (t < Tburn): thurst = np.array([20, 1, 0]) return thurst def dynamics(y, t): X, Y, Z, Vx, Vy, Vz, gamma, psi, thetta, omega_x, omega_y, omega_z = y
def cam_pose(predicted, name, is_fixated=False):
import json
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure(figsize=(5, 5))
predicted = np.squeeze(predicted)
ax = fig.add_subplot(1, 1, 1, projection='3d')
from transforms3d import euler
ax.set_title("Green:Ref. Camera(img2);Red: Pred")
if is_fixated:
mean = [
10.12015407, 8.1103528, 1.09171896, 1.21579016, 0.26040945,
10.05966329
]
std = [
-2.67375523e-01, -1.19147040e-02, 1.14497274e-02, 1.10903410e-03,
2.10509948e-02, -4.02013549e+00
]
else:
mean = [
-9.53197445e-03, -1.05196691e-03, -1.07545642e-02, 2.08785638e-02,
-9.27858049e-02, -2.58052205e+00
]
std = [
1.02316223, 0.66477511, 1.03806996, 5.75692889, 1.37604962,
7.43157247
]
predicted = predicted * std
predicted = predicted + mean
cam_origin = np.asarray([0, 0, 0])
cam_direction = np.asarray([0, 0, -1])
translation = predicted[3:]
rotation = euler.euler2mat(*predicted[:3], axes='sxyz')
c12_location = np.matmul(rotation, cam_origin) + translation
c12_direction = np.matmul(rotation, cam_direction) + translation
points = np.vstack([
cam_origin, cam_direction, c12_location, c12_direction,
c12_direction - c12_location
])
axis_min = np.amin(points, axis=0)
axis_max = np.amax(points, axis=0)
axis_size = (axis_max - axis_min) / 10.
axis_min = axis_min - axis_size
axis_max = axis_max + axis_size
length = 3.
ratio = 0.5
ax.quiver(cam_origin[2],
cam_origin[0],
cam_origin[1],
cam_direction[2] - cam_origin[2],
cam_direction[0] - cam_origin[0],
cam_direction[1] - cam_origin[1],
pivot='tail',
arrow_length_ratio=ratio,
length=length,
colors=[0, 1, 0])
ax.quiver(c12_location[2],
c12_location[0],
c12_location[1],
c12_direction[2] - c12_location[2],
c12_direction[0] - c12_location[0],
c12_direction[1] - c12_location[1],
pivot='tail',
arrow_length_ratio=ratio,
length=length,
colors=[1, 0, 0])
axis_mid = (axis_min + axis_max) / 2.
axis_length = (axis_max - axis_min)
axis_length[axis_length < 5.] = 5.
axis_min = axis_mid - axis_length / 2.
axis_max = axis_mid + axis_length / 2.
ax.set_xlabel('Z axis')
ax.set_ylabel('X axis')
ax.set_zlabel('Y axis')
ax.set_xlim([axis_min[2], axis_max[2]])
ax.set_ylim([axis_min[0], axis_max[0]])
ax.set_zlim([axis_min[1], axis_max[1]])
theta = np.arctan2(1, 0) * 180 / np.pi
ax.view_init(60, theta - 90)
fig.savefig(name, dpi=256)
def euler2rvec(a0, a1, a2):
return cv2.Rodrigues(euler2mat(a0, a1, a2, axes='rzxy'))[0]
def inverse_kinematics(self, positions, ignore_joints=[]):
N_joints = self.get_N_joints()
assert positions.shape == (N_joints, 3)
# do IK
for root_idx, grp_inds in self.root_n_group.items():
if root_idx in ignore_joints:
continue
GT2 = self.get_node_transforms()
pj = self.parents[root_idx]
# solve root using SVD
ret, apply_R = self.compute_root_joint_rotation(root_idx, GT2[:,:3,3], positions)
assert ret
root_R = np.dot(apply_R, GT2[root_idx,:3,:3])
root_LR = root_R.copy()
if root_idx == 11: # TODO: CONFIG
# for Spine1, slerp rotation between Spine2 and Spine
ppj = self.parents[pj] # Spine
ppj_R = GT2[ppj,:3,:3]
pj_Q = quaternion_slerp(m2q(ppj_R), m2q(root_R), w=0.5) # slerp
pj_R = q2m(pj_Q)
pj_LR = np.dot(inv(ppj_R), pj_R)
if self.USE_CONSTRAINTS:
is_constrained, pj_LR = self.constrain_joint_LR(pj, pj_LR)
if is_constrained:
print("CONSTRAINED %d"%(pj))
pj_R = np.dot(ppj_R, pj_LR)
root_LR = np.dot(inv(pj_R), root_R)
is_constrained, root_LR = self.constrain_joint_LR(root_idx, root_LR)
if is_constrained: # NEED TO ROTATE ALL POSITIONS BASED ON THIS PIVOT
print("CONSTRAINED %d"%(root_idx))
new_root_R = np.dot(pj_R, root_LR)
apply_R = np.dot(new_root_R, inv(root_R))
ppp = positions[self.joint_subchilds[root_idx]] - positions[root_idx]
ppp_rotated = np.dot(apply_R, np.mat(ppp).T).T
positions[self.joint_subchilds[root_idx]] = ppp_rotated + positions[root_idx]
root_R = np.dot(pj_R, root_LR)
# angles_rotated = list(mat2euler(inv(self.local_rotations[root_idx]).dot(root_LR)))
# print(np.degrees(angles_rotated))
# self.set_node_local_pose_by_index(root_idx, LR=root_LR)
# self.visualize_transforms()
# self.set_node_local_pose_by_index(root_idx, LR=np.dot(inv(pj_R), root_R))
# self.visualize_transforms()
# TODO: re-slerp pj_LR?
self.set_node_local_pose_by_index(pj, LR=pj_LR)
root_LR = np.dot(inv(pj_R), root_R)
self.set_node_local_pose_by_index(root_idx, LR=root_LR)
root_pos = self.get_node_transform_by_index(root_idx)[:3,3]
diff = root_pos - positions[root_idx]
if np.sum(diff) != 0:
positions[self.joint_subchilds[root_idx]] += diff
# using lookat function
for inds in grp_inds:
for ix, j in enumerate(inds[:-1]): # don't need to set any rotation for last end node, ignore
next_ind = inds[ix+1]
# CHECK
if next_ind in ignore_joints:
continue
target = positions[next_ind] - positions[j]
rot_matrix, R = self.solve_node_lookat_R(j, target)
is_constrained = False
if self.USE_CONSTRAINTS:
if j in [2, 3, 6, 7]: # TODO: CONFIG
angles = list(mat2euler(rot_matrix))
angles[self.forward_axis] = 0
rot_matrix = euler2mat(*angles)
is_constrained, rot_matrix2 = self.constrain_joint_LR(j, rot_matrix)
if is_constrained:
rot_matrix = rot_matrix2
"""
EXTRA CONSTRAINTS FOR LEGS:
if distance offset is large, try comparing it with the distance offset if there is
a different rotation config of the parent(s)
Here, we simply compare to the rotation where the base parent has forward-axis (x) rotation set to 0
"""
if j in [2, 6]: # TODO: CONFIG
pj = self.parents[j]
pj_R = self.get_node_transform_by_index(pj, global_=True)[:3,:3]
R = np.dot(pj_R, rot_matrix)
tt = np.dot(R, self.local_translations[next_ind])
dist_offset = dist(tt - target)
if dist_offset > 0.2 * self.bone_lengths[next_ind]:
pj_LR = self.get_node_transform_by_index(pj, global_=False)[:3, :3]
angles = list(mat2euler(pj_LR))
angles[self.forward_axis] = 0
pj_LR_x0 = euler2mat(*angles)
ppj_R = self.get_node_transform_by_index(self.parents[pj], global_=True)[:3, :3]
pj_R_x0 = np.dot(ppj_R, pj_LR_x0)
self.set_node_local_pose_by_index(pj, pj_LR_x0) # TEMPORARILY, JUST FOR LOOKAT
rot_matrix_x0, _ = self.solve_node_lookat_R(j, target)
is_constrained1, rot_matrix_x0 = self.constrain_joint_LR(j, rot_matrix_x0)
R = np.dot(pj_R_x0, rot_matrix_x0)
tt = np.dot(R, self.local_translations[next_ind])
dist_offset2 = dist(tt - target)
if dist_offset2 < dist_offset:
print("YES", j)
rot_matrix = rot_matrix_x0
# out_j_t = self.get_node_transform_by_index(j, global_=True)[:3,3]
# positions[self.joint_subchilds[pj]] += out_j_t - positions[j]
else:
self.set_node_local_pose_by_index(pj, pj_LR) # SET BACK TO PREVIOUS
"""END EXTRA CONSTRAINTS FOR LEGS"""
self.set_node_local_pose_by_index(j, rot_matrix)
out = self.get_node_transform_by_index(next_ind, global_=True)
if is_constrained:
# update all child positions by the constrained offset
pos_diff = out[:3,3] - positions[next_ind]
positions[self.joint_subchilds[j]] += pos_diff
else:
assert_same(out[:3,3], positions[next_ind])
# Keep a copy of the initial position for later
m1pos4d = m1.pos4d.copy()
rot2 = np.array([[2.**(-0.5), 0, 2.**(-0.5)],[0, 1., 0],[-2.**(-0.5), 0, 2.**(-0.5)]])
m2 = FlatBrewsterMirror(orientation=rot2,zoom=[1, 10, 30], position=[0, 0, 2e2])
det = FlatDetector(position=[50, 0, 2e2], zoom=[1., 20., 20.])
experiment = Sequence(elements=[m1, m2, det])
angles = np.arange(0, 2 * np.pi, 0.1)
n_detected = np.zeros_like(angles)
for i, angle in enumerate(angles):
rotmat = np.eye(4)
rotmat[:3, :3] = euler.euler2mat(angle, 0, 0, 'szxy')
light.position = np.dot(rotmat, light_pos)
light.dir = np.dot(rotmat, light_dir)
m1.geometry.pos4d = np.dot(rotmat, m1pos4d)
rays = light(10000)
rays = experiment(rays)
n_detected[i] = rays['probability'].sum()
# Per email from Herman Marshal, see
# http://adsabs.harvard.edu/abs/2013SPIE.8861E..1DM
labdata = '''
Angle Rate Uncertainty
# (deg) (cnt/s/.1mA) (cnt/s/.1mA)
89.8900 0.320045 0.00624663
44.8900 0.132169 0.00401622
44.8900 0.133989 0.00402895
def process_event(self):
"""
Handle user interface events: keydown, close, dragging.
"""
for event in pygame.event.get():
if event.type == pygame.QUIT:
self.done = True
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_RETURN: # reset camera
self.translate = self.default_translate
self.global_rx = 0
self.global_ry = 0
elif event.key == pygame.K_SPACE:
self.playing = not self.playing
elif event.type == pygame.MOUSEBUTTONDOWN: # dragging
if event.button == 1:
self.rotate_dragging = True
else:
self.translate_dragging = True
self.old_x, self.old_y = event.pos
elif event.type == pygame.MOUSEBUTTONUP:
if event.button == 1:
self.rotate_dragging = False
else:
self.translate_dragging = False
elif event.type == pygame.MOUSEMOTION:
if self.translate_dragging:
# haven't figure out best way to implement this
pass
elif self.rotate_dragging:
new_x, new_y = event.pos
self.global_ry -= (new_x - self.old_x) / \
self.screen_size[0] * np.pi
self.global_rx -= (new_y - self.old_y) / \
self.screen_size[1] * np.pi
self.old_x, self.old_y = new_x, new_y
pressed = pygame.key.get_pressed()
# rotation
if pressed[pygame.K_DOWN]:
self.global_rx -= self.speed_rx
if pressed[pygame.K_UP]:
self.global_rx += self.speed_rx
if pressed[pygame.K_LEFT]:
self.global_ry += self.speed_ry
if pressed[pygame.K_RIGHT]:
self.global_ry -= self.speed_ry
# moving
if pressed[pygame.K_a]:
self.translate[0] -= self.speed_trans
if pressed[pygame.K_d]:
self.translate[0] += self.speed_trans
if pressed[pygame.K_w]:
self.translate[1] += self.speed_trans
if pressed[pygame.K_s]:
self.translate[1] -= self.speed_trans
if pressed[pygame.K_q]:
self.translate[2] += self.speed_zoom
if pressed[pygame.K_e]:
self.translate[2] -= self.speed_zoom
# forward and rewind
if pressed[pygame.K_COMMA]:
self.frame -= 1
if self.frame < 0:
self.frame = len(self.motions) - 1
if pressed[pygame.K_PERIOD]:
self.frame += 1
if self.frame >= len(self.motions):
self.frame = 0
# global rotation
grx = euler.euler2mat(self.global_rx, 0, 0)
gry = euler.euler2mat(0, self.global_ry, 0)
self.rotation_R = grx.dot(gry)
origin = np.array([0, 0, 0])
stick = np.array([LENGTH, 0, 0])
axes = "rxyz"
# def get_angles_rad(x, y, z, axes):
# l2a = {"x": x, "y": y, "z": z}
# angles_rad = []
# for letter in axes[1:]:
# angles_rad.append(l2a[letter])
# return angles_rad
#
# print map(degrees, get_angles_rad(x1, y1, z1, axes))
#
# R1 = euler.euler2mat(*get_angles_rad(x1, y1, z1, axes), axes=axes)
# R2 = euler.euler2mat(*get_angles_rad(x2, y2, z2, axes), axes=axes)
R1 = euler.euler2mat(x1, y1, z1, axes=axes)
R2 = euler.euler2mat(x2, y2, z2, axes=axes)
show_frame(R1, '-o', 5)
show_frame(R2, '-d', 15)
plt.xlabel('x')
plt.ylabel('y')
plt.show()
def sourceMirrorDetector(mirror='A12113', sourceAngle=0, time=1., sourceOffset=0, detOffset=0, source='C',
apDims=[0.125, 0.125], V=10., I=0.1):
'''Sets up and runs the following simulation.
Layout:
--------------------------------------
Source Detector
| ^
| /|\
\|/ |
v |
Mirror -> (Grating) -> Mirror
--------------------------------------
Parameters
----------
mirror: string
the serial number of the mirror (in the mirror reflectivity file's name)
sourceAngle: float
the angle the source is rotated from the +z axis (counter-clockwise looking from the +x axis)
time: float
the amount of time that is being simulated
sourceOffset: float
the distance between the center of the first mirror and the photons beam axis
(in the direction of the +y axis before rotation)
detOffset: float
the distance between the center of the second mirror and the photons beam axis
(in the direction of the +y axis before rotation)
source: string OR 1D array of length 2
if string: the chemical symbol/formula for the material of the source (ex: 'C' for carbon, or 'AlO' for aluminum oxide)
if array: the mean and std. deviation for a normal energy distribution
apDims: 1D array of length 2 OR None
NOTE: the type of input to this parameter determines whether FarLabPointSource or LabPointSource is used
if array: the y and z dimensions of the aperture for a far lab source
if None: None is the input, this causes the program to use the slightly more realistic, but much slower, lab source
V: float
the voltage of the source in kV
I: float
the current of the source in mA
'''
# paths to mirror files
string1 = './mirror_files/' + mirror + '.txt'
string2 = './mirror_files/ALSpolarization.txt'
# inputs not accounted for by parameters
sourceDist = 8149.7
sourceRadius = 192.
apertureRadius = 20.
sourceAngle *= np.pi/180
detDist = -9532.5
detRadius = 50.
detAngle = 0 * np.pi/180 # from the +z axis, counter-clockwise looking from the +x axis
# calculate rotation matrices
# looking from the axis of rotation the rotation is counter-clockwise
r1 = euler2mat(np.pi/4, sourceAngle, 0, 'syxz')
r2 = euler2mat(-np.pi/4, detAngle, 0, 'syxz')
r3 = euler2mat(np.pi/2, detAngle, 0, 'syxz')
r4 = euler2mat(np.pi/2, sourceAngle, 0, 'syxz')
# choose an energy distribution based on the source
if isinstance(source, basestring):
energies = createEnergyTable(source, V_kV = V, I_mA = I) # photons/sec/steradian
rate = sum(energies[1]) # photons/sec/steradian
elif hasattr(source, 'shape') and (source.shape == np.array([0,0]).shape):
energies = normalEnergyFunc(source[0], source[1])
else:
print 'source must be a string describing the source material or a 1D array of length 2'
# choose a model for the source based on whether aperture dimensions were given
if apDims != None:
solidAngle = apDims[0] * apDims[1] / (sourceRadius - apertureRadius)**2
rate *= solidAngle
source = FarLabPointSource([sourceDist, sourceRadius * np.sin(-sourceAngle), sourceRadius * np.cos(-sourceAngle)],
position = [sourceDist, apertureRadius * np.sin(-sourceAngle), apertureRadius * np.cos(-sourceAngle)],
energy = energies, flux = rate, orientation = r4, zoom = [1, apDims[0], apDims[1]])
else:
source = LabPointSource([sourceDist, sourceRadius * np.sin(sourceAngle), sourceRadius * np.cos(sourceAngle)],
direction='-z', energy = energies, flux = 1e9)
# initialize both mirrors and the detector
mirror1 = MultiLayerMirror(string1, string2,
position=np.array([sourceDist, sourceOffset * np.cos(sourceAngle), sourceOffset * np.sin(sourceAngle)]), orientation=r1)
mirror2 = MultiLayerMirror(string1, string2,
position=np.array([detDist, detOffset * np.cos(detAngle), detOffset * np.cos(detAngle)]), orientation=r2)
detector = FlatDetector(pixsize=24.576e-3,
position = np.array([detDist, detRadius * np.sin(-detAngle), detRadius * np.cos(-detAngle)]),
zoom = np.array([1, 12.288, 12.288]), orientation = r3)
#preDetector = FlatDetector(pixsize=24.576e-3, position = np.array([detDist + 20, 0, 0]), zoom = np.array([1, 100, 100]))
#testDetector = FlatDetector(pixsize=24.576e-3, position = np.array([detDist + 1e4, 0, 0]), zoom = np.array([1, 50, 50]))
'''
photons = source.generate_photons(time)
#photons = generateTestPhotons([sourceDist, sourceRadius * np.sin(sourceAngle), sourceRadius * np.cos(sourceAngle)])
photons = mirror1.process_photons(photons)
photons = photons[photons['probability'] > 0]
photons = mirror2.process_photons(photons)
photons = photons[photons['probability'] > 0]
photons = detector.process_photons(photons)
return photons
'''
# run the simulation
parts = 8
for i in range(0, parts):
photons = source.generate_photons(time / parts)
photons = mirror1.process_photons(photons)
photons = photons[photons['probability'] > 0]
#photons = testDetector.process_photons(photons)
#photons = preDetector.process_photons(photons)
#if i == 0:
# pre_all_photons = photons.copy()
#else:
# pre_all_photons = vstack([pre_all_photons, photons])
photons = mirror2.process_photons(photons)
photons = photons[photons['probability'] > 0]
photons = detector.process_photons(photons)
if i == 0:
all_photons = photons.copy()
else:
all_photons = vstack([all_photons, photons])
#print all_photons
#plotPoints(pre_all_photons, 50, 40, 'path_', angle = sourceAngle, all = False)
return all_photons
def get_random_transformation():
T = [0, np.random.uniform(-8, 8), np.random.uniform(-8, 8)]
R = euler2mat(np.random.uniform(-5, 5) / 180.0 * np.pi, 0, 0, 'sxyz')
Z = [1, np.random.uniform(0.9, 1.1), np.random.uniform(0.9, 1.1)]
A = compose(T, R, Z)
return A
def run(self, state, command):
"""Steps the controller forward one timestep
Parameters
----------
controller : Controller
Robot controller object.
"""
########## Update operating state based on command ######
if command.activate_event:
state.behavior_state = self.activate_transition_mapping[state.behavior_state]
elif command.trot_event:
state.behavior_state = self.trot_transition_mapping[state.behavior_state]
elif command.hop_event:
state.behavior_state = self.hop_transition_mapping[state.behavior_state]
if state.behavior_state == BehaviorState.TROT:
state.foot_locations, contact_modes = self.step_gait(state, command,)
# Apply the desired body rotation
rotated_foot_locations = euler2mat(command.roll, command.pitch, 0.0) @ state.foot_locations
# Construct foot rotation matrix to compensate for body tilt
print(state.quat_orientation)
(roll, pitch, yaw) = quat2euler(state.quat_orientation)
correction_factor = 0.8
max_tilt = 0.4
roll_compensation = correction_factor * np.clip(roll, -max_tilt, max_tilt)
pitch_compensation = correction_factor * np.clip(pitch, -max_tilt, max_tilt)
rmat = euler2mat(roll_compensation, pitch_compensation, 0)
rotated_foot_locations = rmat.T @ rotated_foot_locations
state.joint_angles = self.inverse_kinematics(rotated_foot_locations, self.config)
elif state.behavior_state == BehaviorState.HOP:
state.foot_locations = self.config.default_stance + np.array([0, 0, -0.09])[:, np.newaxis]
state.joint_angles = self.inverse_kinematics(state.foot_locations, self.config)
elif state.behavior_state == BehaviorState.FINISHHOP:
state.foot_locations = self.config.default_stance + np.array([0, 0, -0.22])[:, np.newaxis]
state.joint_angles = self.inverse_kinematics(state.foot_locations, self.config)
elif state.behavior_state == BehaviorState.REST:
yaw_proportion = command.yaw_rate / self.config.max_yaw_rate
self.smoothed_yaw += self.config.dt * clipped_first_order_filter(
self.smoothed_yaw,
yaw_proportion * -self.config.max_stance_yaw,
self.config.max_stance_yaw_rate,
self.config.yaw_time_constant,
)
# Set the foot locations to the default stance plus the standard height
state.foot_locations = self.config.default_stance + np.array([0, 0, command.height])[:, np.newaxis]
# Apply the desired body rotation
rotated_foot_locations = euler2mat(command.roll, command.pitch, self.smoothed_yaw,) @ state.foot_locations
state.joint_angles = self.inverse_kinematics(rotated_foot_locations, self.config)
state.ticks += 1
state.pitch = command.pitch
state.roll = command.roll
state.height = command.height
def ego_motion(predicted, name):
import json
from mpl_toolkits.mplot3d import Axes3D
pose12 = np.squeeze(predicted[0])
pose23 = np.squeeze(predicted[-1])
fig = plt.figure(figsize=(5, 5))
ax = fig.add_subplot(1, 1, 1, projection='3d')
ax.set_title("Green:Ref. Camera(img2);Red--img1;Blue--img3")
from mpl_toolkits.mplot3d import Axes3D
from transforms3d import euler
fixated_std = np.asarray([
10.12015407, 8.1103528, 1.09171896, 1.21579016, 0.26040945, 10.05966329
])
fixated_mean = np.asarray([
-2.67375523e-01, -1.19147040e-02, 1.14497274e-02, 1.10903410e-03,
2.10509948e-02, -4.02013549e+00
])
pose12 = pose12 * fixated_std
pose12 = pose12 + fixated_mean
pose23 = pose23 * fixated_std
pose23 = pose23 + fixated_mean
cam_origin = np.asarray([0, 0, 0])
cam_direction = np.asarray([0, 0, -1])
# Calculating Camera 1's world refernce position
translation_12 = pose12[3:]
rotation_12 = euler.euler2mat(*pose12[:3], axes='sxyz')
c12_location = np.matmul(rotation_12, cam_origin) + translation_12
c12_direction = np.matmul(rotation_12, cam_direction) + translation_12
# Calculating Camera 3's world refernce position
translation_32 = -pose23[3:]
rotation_32 = np.linalg.inv(euler.euler2mat(*pose23[:3], axes='sxyz'))
c32_location = np.matmul(rotation_32, cam_origin + translation_32)
c32_direction = np.matmul(rotation_32, cam_direction + translation_32)
points = np.vstack([
c12_location, c12_direction, c32_location, c32_direction, cam_origin,
cam_direction, c12_direction - c12_location,
c32_direction - c32_location
])
axis_min = np.amin(points, axis=0)
axis_max = np.amax(points, axis=0)
axis_size = (axis_max - axis_min) / 10.
axis_min = axis_min - axis_size
axis_max = axis_max + axis_size
length = 3.
ratio = 0.5
ax.quiver(cam_origin[2],
cam_origin[0],
cam_origin[1],
cam_direction[2] - cam_origin[2],
cam_direction[0] - cam_origin[0],
cam_direction[1] - cam_origin[1],
pivot='tail',
arrow_length_ratio=ratio,
length=length,
colors=[0, 1, 0])
ax.quiver(c12_location[2],
c12_location[0],
c12_location[1],
c12_direction[2] - c12_location[2],
c12_direction[0] - c12_location[0],
c12_direction[1] - c12_location[1],
pivot='tail',
arrow_length_ratio=ratio,
length=length,
colors=[1, 0, 0])
ax.quiver(c32_location[2],
c32_location[0],
c32_location[1],
c32_direction[2] - c32_location[2],
c32_direction[0] - c32_location[0],
c32_direction[1] - c32_location[1],
pivot='tail',
arrow_length_ratio=ratio,
length=length,
colors=[0, 0, 1])
axis_mid = (axis_min + axis_max) / 2.
axis_length = (axis_max - axis_min)
axis_length[axis_length < 5.] = 5.
axis_min = axis_mid - axis_length / 2.
axis_max = axis_mid + axis_length / 2.
ax.set_xlabel('Z axis')
ax.set_ylabel('X axis')
ax.set_zlabel('Y axis')
ax.set_xlim([axis_min[2], axis_max[2]])
ax.set_ylim([axis_min[0], axis_max[0]])
ax.set_zlim([axis_min[1], axis_max[1]])
theta = np.arctan2(1, 0) * 180 / np.pi
ax.view_init(60, theta - 90)
fig.savefig(name, dpi=256)
def get_rpy_quaternion(roll, pitch, yaw):
"""Converts human readable roll (about x-axis), pitch (about y-axis)
and yaw (about z-axis) format to a 4D quaternion"""
return mat2quat(euler2mat(roll, pitch, yaw, 'sxyz'))
def make_optical_rotation_matrix(self):
return euler.euler2mat(-1.579, 0, -1.579)
from transforms3d.quaternions import quat2mat, mat2quat from transforms3d.euler import mat2euler, euler2mat, euler2quat, quat2mat import numpy as np np.set_printoptions(precision=3, suppress=True) # neat printing a = [0, 0, 0, 1] a1 = quat2mat(a) print(a1) a = [0, 0, 1, 0] a1 = quat2mat(a) print(a1) a = [0, 1, 0, 0] a1 = quat2mat(a) print(a1) a = [1, 0, 0, 0] a1 = quat2mat(a) print(a1) x_angle = -0.2 y_angle = -np.pi / 2 z_angle = -0.2 R = euler2mat(x_angle, y_angle, z_angle, axes='sxyz') print(R)
def blender_euler_to_blender_pose(euler):
euler_0 = (-euler[0] + 90) % 360
euler_1 = euler[1] + 90
return euler2mat(euler_0 * np.pi / 180, euler_1 * np.pi / 180, euler[2] * np.pi / 180, axes="szxz")
def on_sensor_frame(self, data):
rgb_image = Image.open(BytesIO(base64.b64decode(data['rgb_image'])))
rgb_image = np.asarray(rgb_image)
if rgb_image.shape != (256, 256, 3):
print('image shape not 256, 256, 3')
return None
if rgb_image.shape[0] != self.image_hw:
rgb_image = misc.imresize(rgb_image, (self.image_hw, self.image_hw, 3))
rgb_image = data_iterator.preprocess_input(rgb_image)
pred = np.squeeze(model.predict(np.expand_dims(rgb_image, 0)))
if self.pred_viz_enabled:
self.queue.put([rgb_image, pred])
target_mask = pred[:, :, 2] > 0.5
# reduce the number of false positives by requiring more pixels to be identified as containing the target
if target_mask.sum() > 10:
centroid = scoring_utils.get_centroid_largest_blob(target_mask)
# scale the centroid from the nn image size to the original image size
centroid = centroid.astype(np.int).tolist()
# Obtain 3D world point from centroid pixel
depth_img = get_depth_image(data['depth_image'])
# Get XYZ coordinates for specific pixel
pixel_depth = depth_img[centroid[0]][centroid[1]][0]*50/255.0
point_3d = get_xyz_from_image(centroid[0], centroid[1], pixel_depth, self.image_hw)
point_3d.append(1)
# Get cam_pose from sensor_frame (ROS convention)
cam_pose = get_ros_pose(data['gimbal_pose'])
# Calculate xyz-world coordinates of the point corresponding to the pixel
# Transformation Matrix
R = euler2mat(math.radians(cam_pose[3]), math.radians(cam_pose[4]), math.radians(cam_pose[5]))
T = np.c_[R, cam_pose[:3]]
T = np.vstack([T, [0,0,0,1]]) # transformation matrix from world to quad
# 3D point in ROS coordinates
ros_point = np.dot(T, point_3d)
sio.emit('object_detected', {'coords': [ros_point[0], ros_point[1], ros_point[2]]})
if not self.target_found:
print('Target found!')
self.target_found = True
self.num_no_see = 0
# Publish Hero Marker
marker_pos = [ros_point[0],ros_point[1], ros_point[2]] + [0, 0, 0]
marker_msg = sio_msgs.create_box_marker_msg(np.random.randint(99999), marker_pos)
sio.emit('create_box_marker', marker_msg)
elif self.target_found:
self.num_no_see += 1
# print(self.num_no_see)
if self.target_found and self.num_no_see > 6:
print('Target lost!')
sio.emit('object_lost', {'data': ''})
self.target_found = False
self.num_no_see = 0
inplanescatter = 10. / 2.3545 / 3600 / 180. * np.pi
perpplanescatter = 1.5 / 2.345 / 3600. / 180. * np.pi
focallength = 12000.
spogeom = load_table('spos', 'petallayout')
spogeom['r_mid'] = (spogeom['outer_radius'] + spogeom['inner_radius']) / 2
spo_pos4d = []
# Convert angle to quantity here to make sure that unit is taken into account
for row, ang in zip(spogeom, u.Quantity(spogeom['clocking_angle']).to(u.rad).value):
spo_pos4d.append(compose([0, # focallength, # - spogeom[i]['d_from_12m']
row['r_mid'] * np.sin(ang),
row['r_mid'] * np.cos(ang)],
euler2mat(-ang, 0., 0.),
# In detail this should be (primary_length + gap + secondary_length) / 2
# but the gap is somewhat complicated and this is only used
# for display, we'll ignore that for now.
[row['primary_length'],
row['azwidth'] / 2.,
(row['outer_radius'] - row['inner_radius']) / 2.]))
spo_pos4d = [np.dot(xyz2zxy, s) for s in spo_pos4d]
reflectivity = load_table2d('spos', 'reflectivity')
reflectivity_interpolator = RectBivariateSpline(reflectivity[1].to(u.keV),
reflectivity[2].to(u.rad),
reflectivity[3][0])
def _refine_orientation(
solution,
k_xy,
structure_library,
accelarating_voltage,
camera_length,
index_error_tol=0.2,
method="leastsq",
vary_angles=True,
vary_center=False,
vary_scale=False,
verbose=False,
):
"""
Refine a single orientation agains the given cartesian vector coordinates.
Parameters
----------
solution : OrientationResult
Namedtuple containing the starting orientation
k_xy : DiffractionVectors
DiffractionVectors (x,y pixel format) to be indexed.
structure_library : :obj:`diffsims:StructureLibrary` Object
Dictionary of structures and associated orientations for which
electron diffraction is to be simulated.
index_error_tol : float
Max allowed error in peak indexation for classifying it as indexed,
calculated as :math:`|hkl_calculated - round(hkl_calculated)|`.
method : str
Minimization algorithm to use, choose from:
'leastsq', 'nelder', 'powell', 'cobyla', 'least-squares'.
See `lmfit` documentation (https://lmfit.github.io/lmfit-py/fitting.html)
for more information.
vary_angles : bool,
Free the euler angles (rotation matrix) during the refinement.
vary_center : bool
Free the center of the diffraction pattern (beam center) during the refinement.
vary_scale : bool
Free the scale (i.e. pixel size) of the diffraction vectors during refinement.
verbose : bool
Be more verbose
Returns
-------
result : OrientationResult
Container for the orientation refinement results
"""
# prepare reciprocal_lattice
structure = structure_library.structures[solution.phase_index]
lattice_recip = structure.lattice.reciprocal()
def objfunc(params, k_xy, lattice_recip, wavelength, camera_length):
cx = params["center_x"].value
cy = params["center_y"].value
ai = params["ai"].value
aj = params["aj"].value
ak = params["ak"].value
scale = params["scale"].value
rotmat = euler2mat(ai, aj, ak)
k_xy = (k_xy + np.array((cx, cy)) * scale)
cart = detector_to_fourier(k_xy, wavelength, camera_length)
intermediate = cart.dot(rotmat.T) # Must use the transpose here
hklss = lattice_recip.fractional(intermediate) * scale
rhklss = np.rint(hklss)
ehklss = np.abs(hklss - rhklss)
return ehklss
ai, aj, ak = mat2euler(solution.rotation_matrix)
params = lmfit.Parameters()
params.add("center_x", value=solution.center_x, vary=vary_center)
params.add("center_y", value=solution.center_y, vary=vary_center)
params.add("ai", value=ai, vary=vary_angles)
params.add("aj", value=aj, vary=vary_angles)
params.add("ak", value=ak, vary=vary_angles)
params.add("scale",
value=solution.scale,
vary=vary_scale,
min=0.8,
max=1.2)
wavelength = get_electron_wavelength(accelarating_voltage)
camera_length = camera_length * 1e10
args = k_xy, lattice_recip, wavelength, camera_length
res = lmfit.minimize(objfunc, params, args=args, method=method)
if verbose: # pragma: no cover
lmfit.report_fit(res)
p = res.params
ai, aj, ak = p["ai"].value, p["aj"].value, p["ak"].value
scale = p["scale"].value
center_x = params["center_x"].value
center_y = params["center_y"].value
rotation_matrix = euler2mat(ai, aj, ak)
k_xy = (k_xy + np.array((center_x, center_y)) * scale)
cart = detector_to_fourier(k_xy,
wavelength=wavelength,
camera_length=camera_length)
intermediate = cart.dot(rotation_matrix.T) # Must use the transpose here
hklss = lattice_recip.fractional(intermediate)
rhklss = np.rint(hklss)
error_hkls = res.residual.reshape(-1, 3)
error_mean = np.mean(error_hkls)
valid_peak_mask = np.max(error_hkls, axis=-1) < index_error_tol
valid_peak_count = np.count_nonzero(valid_peak_mask, axis=-1)
num_peaks = len(k_xy)
match_rate = (valid_peak_count * (1 / num_peaks)) if num_peaks else 0
orientation = OrientationResult(phase_index=solution.phase_index,
rotation_matrix=rotation_matrix,
match_rate=match_rate,
error_hkls=error_hkls,
total_error=error_mean,
scale=scale,
center_x=center_x,
center_y=center_y)
res = np.empty(2, dtype=np.object)
res[0] = orientation
res[1] = rhklss
return res
def test_with_euler2mat():
# Test against Tait-Bryan implementation
for a, b, c in euler_tuples:
tb_mat = tb.euler2mat(a, b, c)
gen_mat = euler2mat(a, b, c, 'szyx')
assert_almost_equal(tb_mat, gen_mat)
def main():
#SETUP
BAGFILE = DIFFICULT_RECORD_FILE #the full path to the bagfile
TRUTH_TF = VICON_FRAME_NAME #the name of the truth transform topic
EST_TF = '/rovio/transform' # the name of the estimated transform (odometry for rovio) topic
POSE_TOPIC = '/rovio/odometry'
#get a chronological list of synced poses from the bag
syncedPoses = buildSyncedPoseList(BAGFILE, 1e7, TRUTH_TF, EST_TF,
POSE_TOPIC)
print("Plotting", len(syncedPoses), "synchronized poses.")
#create an MPL figure
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# extract lists of x,y,z coords for plotting
truth_x = []
truth_y = []
truth_z = []
est_x = []
est_y = []
est_z = []
#store the offsets between IMU frame zero and Vicon frame zero
x_offset = 0
y_offset = 0
z_offset = 0
roll_offset = 0
pitch_offset = 0
yaw_offset = 0
counter = 0
for i in syncedPoses:
xt, yt, zt = i.getTruthXYZ()
xe, ye, ze = i.getEstXYZ()
xyz_est = np.array([xe, ye, ze])
if counter < 1:
# NOTE: Rovio starts at (x0,y0,z0) = (0,0,0)
# determine how to translate the Vicon to (0,0,0) in the Rovio frame
x_truth_offset = xe - xt
y_truth_offset = ye - yt
z_truth_offset = ze - zt
counter += 1
rovio_quat_in_vicon = transformRovioQuaternionToViconCF(
i.getEstQuat())
#print "Rovio eulers in vicon:", [math.degrees(j) for j in quat2euler(rovio_quat_in_vicon, axes='sxyz')]
rovio_quat_offset_to_vicon = getRelativeQuaternionAtoB(
rovio_quat_in_vicon, i.getTruthQuat())
rovio_euler_offset_to_vicon = [
math.degrees(j) for j in quat2euler(rovio_quat_offset_to_vicon)
]
yaw_offset = rovio_euler_offset_to_vicon[2] + 2
print "Imu euler offset to vicon:", rovio_euler_offset_to_vicon
rovio_mat_offset_to_vicon = quat2mat(rovio_quat_offset_to_vicon)
#translate the Vicon truth so that it begins at (0,0,0) in the Rovio frame
translated_xyz_truth = np.array(
[xt + x_truth_offset, yt + y_truth_offset, zt + z_truth_offset])
# apply the transformation from the Imu Frame to the Vicon World Frame
imu_in_vicon = transformRovioImuToVicon(xyz_est)
# apply a small extra rotation to correct for callibration differences between imu and vicon
z_off = euler2mat(0, 0, math.radians(yaw_offset))
#easyrecord z_off: 13.11163 yaw
imu_in_vicon_offset = np.dot(z_off, imu_in_vicon)
est_x.append(imu_in_vicon_offset[0])
est_y.append(imu_in_vicon_offset[1])
est_z.append(imu_in_vicon_offset[2])
truth_x.append(translated_xyz_truth[0])
truth_y.append(translated_xyz_truth[1])
truth_z.append(translated_xyz_truth[2])
#format the plot: truth is RED, estimated is GREEN
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_xlim([-3, 1])
ax.set_ylim([-3, 1])
ax.set_zlim([-2, 2])
ax.set_title('EUROC3: Estimated Pose vs. Truth (meters)')
fig.add_axes(ax)
# print "TX:", np.shape(truth_x)
# print "TY:", np.shape(truth_y)
# print "EX:", np.shape(est_x)
# print "EY:",
ax.plot(truth_x, truth_y, zs=truth_z, zdir='z', color="r")
ax.plot(est_x, est_y, zs=est_z, zdir='z', color="g")
p.show()
def calculate_ed_data(
self,
structure,
reciprocal_radius,
rotation=(0, 0, 0),
with_direct_beam=True,
max_excitation_error=1e-2,
debye_waller_factors={},
):
"""Calculates the Electron Diffraction data for a structure.
Parameters
----------
structure : diffpy.structure.structure.Structure
The structure for which to derive the diffraction pattern.
Note that the structure must be rotated to the appropriate
orientation and that testing is conducted on unit cells
(rather than supercells).
reciprocal_radius : float
The maximum radius of the sphere of reciprocal space to
sample, in reciprocal Angstroms.
rotation : tuple
Euler angles, in degrees, in the rzxz convention. Default is
(0, 0, 0) which aligns 'z' with the electron beam.
with_direct_beam : bool
If True, the direct beam is included in the simulated
diffraction pattern. If False, it is not.
max_excitation_error : float
The extinction distance for reflections, in reciprocal
Angstroms. Roughly equal to 1/thickness.
debye_waller_factors : dict of str:value pairs
Maps element names to their temperature-dependent Debye-Waller factors.
Returns
-------
diffsims.sims.diffraction_simulation.DiffractionSimulation
The data associated with this structure and diffraction setup.
"""
# Specify variables used in calculation
wavelength = self.wavelength
latt = structure.lattice
# Obtain crystallographic reciprocal lattice points within `reciprocal_radius` and
# g-vector magnitudes for intensity calculations.
recip_latt = latt.reciprocal()
spot_indices, cartesian_coordinates, spot_distances = get_points_in_sphere(
recip_latt, reciprocal_radius)
ai, aj, ak = (
np.deg2rad(rotation[0]),
np.deg2rad(rotation[1]),
np.deg2rad(rotation[2]),
)
R = euler2mat(ai, aj, ak, axes="rzxz")
cartesian_coordinates = np.matmul(R, cartesian_coordinates.T).T
# Identify points intersecting the Ewald sphere within maximum
# excitation error and store the magnitude of their excitation error.
r_sphere = 1 / wavelength
r_spot = np.sqrt(
np.sum(np.square(cartesian_coordinates[:, :2]), axis=1))
z_spot = cartesian_coordinates[:, 2]
if self.precession_angle > 0 and not self.approximate_precession:
# We find the average excitation error - this step can be
# quite expensive
excitation_error = _average_excitation_error_precession(
z_spot,
r_spot,
wavelength,
self.precession_angle,
)
else:
z_sphere = -np.sqrt(r_sphere**2 - r_spot**2) + r_sphere
excitation_error = z_sphere - z_spot
# Mask parameters corresponding to excited reflections.
intersection = np.abs(excitation_error) < max_excitation_error
intersection_coordinates = cartesian_coordinates[intersection]
excitation_error = excitation_error[intersection]
r_spot = r_spot[intersection]
g_indices = spot_indices[intersection]
g_hkls = spot_distances[intersection]
# take into consideration rel-rods
if self.precession_angle > 0 and not self.approximate_precession:
shape_factor = _shape_factor_precession(
intersection_coordinates[:, 2],
r_spot,
wavelength,
self.precession_angle,
self.shape_factor_model,
max_excitation_error,
**self.shape_factor_kwargs,
)
elif self.precession_angle > 0 and self.approximate_precession:
shape_factor = lorentzian_precession(
excitation_error,
max_excitation_error,
r_spot,
self.precession_angle,
)
else:
shape_factor = self.shape_factor_model(excitation_error,
max_excitation_error)
# Calculate diffracted intensities based on a kinematical model.
intensities = get_kinematical_intensities(
structure,
g_indices,
g_hkls,
prefactor=shape_factor,
scattering_params=self.scattering_params,
debye_waller_factors=debye_waller_factors,
)
# Threshold peaks included in simulation based on minimum intensity.
peak_mask = intensities > self.minimum_intensity
intensities = intensities[peak_mask]
intersection_coordinates = intersection_coordinates[peak_mask]
g_indices = g_indices[peak_mask]
return DiffractionSimulation(coordinates=intersection_coordinates,
indices=g_indices,
intensities=intensities,
with_direct_beam=with_direct_beam,
is_hex=is_lattice_hexagonal(
structure.lattice))
def euler2mat_deg(ee):
return euler2mat(*np.deg2rad(ee))