def make_calibration_vector(self):
head_ori = qa.norm(self.store_head_ori.mean(axis=0))
Ni = self.store_head_pos.mean(axis=0)
self.Qi = qa.norm(self.store_glasses_ori.mean(axis=0))
Ti = self.store_glasses_pos.mean(axis=0)
self.tp = TP(self.Qi, Ni, Ti)
if ((np.any(np.isnan(head_ori))) == True or (np.any(np.isnan(Ni))) == True
or (np.any(np.isnan(self.Qi))) == True or (np.any(np.isnan(Ti))) == True):
sys.stdout.write("!!!!!!!! Calibration complete! !!!!!!!!!!!\n")
self.user_start_calibrate()
return
msg_str_pos = "%.5e, " * 3
msg_str_ori = "%.5e, " * 4
sys.stdout.write('Cz orientation: ')
sys.stdout.write(msg_str_ori % (head_ori[0], head_ori[1], head_ori[2],\
head_ori[3]) + "\n")
sys.stdout.write('Cz position: ')
sys.stdout.write(msg_str_pos % (Ni[0], Ni[1], Ni[2]) + "\n")
sys.stdout.write('Glasses orientation: ')
sys.stdout.write(msg_str_ori % (self.Qi[0], self.Qi[1], self.Qi[2], self.Qi[3]) + "\n")
sys.stdout.write('Glasses position: ')
sys.stdout.write(msg_str_pos % (Ti[0], Ti[1], Ti[2]) + "\n")
sys.stdout.write("********** Calibration complete! ***********\n")
sys.stdout.flush()
self.calibrated = True
self.got_coil = False
self.got_head = False
def test_slerp(self):
q = qarray.norm(np.array([[2., 3, 4, 5],
[6, 7, 8, 9]]))
time = [0, 9]
targettime = [0, 3, 4.5, 9]
q_interp = qarray.slerp(
targettime,
time,
q)
self.assertEquals(len(q_interp), 4)
np.testing.assert_array_almost_equal(q_interp[0], q[0])
np.testing.assert_array_almost_equal(q_interp[-1], q[-1])
np.testing.assert_array_almost_equal(q_interp[1], qarray.norm(q[0] * 2/3 + q[1]/3),decimal=4)
np.testing.assert_array_almost_equal(q_interp[2], qarray.norm((q[0] + q[1])/2),decimal=4)
def load_config(self, config_file):
cfg = PyFileConfigLoader(config_file)
cfg.load_config()
self.config = cfg.config
# special casing for SAMPLE_GENERATED
if (self.config.trigger == 'SAMPLE_GENERATED'):
self.config.trigger_msg = rc.MT_SAMPLE_GENERATED
self.config.trigger_mdf = rc.MDF_SAMPLE_GENERATED
else:
self.config.trigger_msg = \
eval('rc.MT_' + self.config.trigger)
self.config.trigger_mdf = \
eval('rc.MDF_' + self.config.trigger)
print "Triggering with", self.config.trigger
print "CoilPlotter: loading config"
#Getting tool message number
self.plate = self.config.tools.index('CB609')
self.marker = self.config.tools.index('CT315')
self.glasses = self.config.tools.index('ST568')
self.pointer = self.config.tools.index('P717')
#Get tool P717 calibration values for pointer calculation
self.pointer_Ti = np.array(self.config.tool_list[self.pointer].Ti)
self.pointer_Qi = qa.norm(np.array(self.config.tool_list[self.pointer].Qi))
self.pointer_Ni = np.array(self.config.tool_list[self.pointer].Ni)
self.pointer_tp = TP(self.pointer_Qi, self.pointer_Ni, self.pointer_Ti)
def quaternion_mean(q):
average = np.array([0, 0, 0, 0])
for i in q:
if check_same(q, i) == False:
i = change_sign(i)
average = average + i
return qa.norm(average / q.shape[0])
def process_message(self, msg):
# read a Dragonfly message
msg_type = msg.GetHeader().msg_type
dest_mod_id = msg.GetHeader().dest_mod_id
if msg_type == MT_EXIT:
if (dest_mod_id == 0) or (dest_mod_id == self.mod.GetModuleID()):
print 'Received MT_EXIT, disconnecting...'
self.mod.SendSignal(rc.MT_EXIT_ACK)
self.mod.DisconnectFromMMM()
return
elif msg_type == rc.MT_PING:
respond_to_ping(self.mod, msg, 'PlotHead')
elif msg_type == rc.MT_POLARIS_POSITION:
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, msg)
positions = np.asarray(in_mdf.xyz[:])
orientations = self.shuffle_q(np.asarray(in_mdf.ori[:]))
if in_mdf.tool_id == (self.pointer + 1):
Qf = qa.norm(orientations)
Qr = qa.mult(Qf, qa.inv(self.pointer_Qi)).flatten()
#find_nans(self.store_head, Qr, 'Qr')
Tk = positions
#find_nans(self.store_head, Tk, 'Tk')
tip_pos = (qa.rotate(Qr, self.pointer_Xi) + Tk).flatten()
self.pointer_position = np.append(self.pointer_position,
(tip_pos[np.newaxis, :]),
axis=0)
#self.pl.reset(x=self.pointer_position[:,0], y=self.pointer_position[:,1], z=self.pointer_position[:,2])
print("old=", tip_pos)
print("new=", self.tp.get_pos(orientations, positions)[0])
def get_4piconv(self, ch, theta, phi, psi, horn_pointing=False):
l.info('Computing dipole temperature with 4pi convolver')
vel = qarray.amplitude(self.satellite_v).flatten()
beta = vel / physcon.c
gamma = 1./np.sqrt(1-beta**2)
unit_vel = self.satellite_v/vel[:,None]
if horn_pointing: # psi comes from the S channel, so there is no need
# to remove psi_pol
psi_nopol = psi
else:
# remove psi_pol
psi_nopol = psi - np.radians(ch.get_instrument_db_field("psi_pol"))
# rotate vel to ecliptic
# phi around z
#ecl_rotation = qarray.rotation([0,0,1], -phi)
# theta around y
ecl_rotation = qarray.norm( qarray.mult(
qarray.rotation([0,0,1], -psi_nopol) ,
qarray.mult(
qarray.rotation([0,1,0], -theta) ,
qarray.rotation([0,0,1], -phi)
)
))
# psi around z
#ecl_rotation = qarray.mult(qarray.rotation([0,0,1], -psi) , ecl_rotation)
# vel in beam ref frame
vel_rad = qarray.rotate(ecl_rotation, unit_vel)
cosdir = qarray.arraylist_dot(vel_rad, self.beam_sum[ch.tag]).flatten()
#return beta * cosdir * T_CMB
return (1. / ( gamma * (1 - beta * cosdir ) ) - 1) * T_CMB
def compute_psi_dx8(self, theta, phi, rad):
z = np.dot(self.siam.get(rad),[0, 0, 1])
vecz = qarray.norm(qarray.rotate(self.qsatgal_interp, z))
e_phi = np.hstack([-np.sin(phi)[:,np.newaxis], np.cos(phi)[:,np.newaxis], np.zeros([len(phi),1])])
e_theta = np.hstack([(np.cos(theta)*np.cos(phi))[:,np.newaxis], (np.cos(theta)*np.sin(phi))[:,np.newaxis], -np.sin(theta)[:,np.newaxis]])
psi = np.arctan2(-qarray.arraylist_dot(vecz, e_phi), -qarray.arraylist_dot(vecz, e_theta))
return psi.flatten()
def process_message(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.array(in_mdf.xyz[:])
orientations = self.shuffle_q(np.array(in_mdf.ori[:]))
# np.testing.assert_array_equal(positions[:,0], orientations[:,0], err_msg='Samples are not aligned')
if self.calibrated:
if in_mdf.tool_id == (self.marker + 1):
# calculating output
self.Qk = qa.norm(
orientations
) # need to find a way to discriminate the tools files in the messages???
Qr = qa.mult(self.Qk, qa.inv(self.Qi)).flatten()
Tk = positions
hotspot_position = (qa.rotate(Qr, self.Xi) + Tk).flatten()
hotspot_vector_head = qa.rotate(Qr, plate_vector)
if np.any(np.isnan(hotspot_position)) == True:
print "x",
# print ' *****nan present, check coil is within frame!*****'
# creating output message
out_mdf = rc.MDF_HOTSPOT_POSITION()
out_mdf.xyz[:] = hotspot_position
out_mdf.ori[:3] = hotspot_vector_head # Qk - coil active orientation
out_mdf.sample_header = in_mdf.sample_header
msg = CMessage(rc.MT_HOTSPOT_POSITION)
copy_to_msg(out_mdf, msg)
self.mod.SendMessage(msg)
sys.stdout.write("o")
else:
if np.any(np.isnan(positions)) == True:
raise Exception, "nan present"
if np.any(np.isnan(orientations)) == True:
raise Exception, "nan present"
if (
(self.store_plate >= self.store_plate_pos.shape[0])
& (self.store_plate >= self.store_plate_ori.shape[0])
& (self.store_coil >= self.store_coil_pos.shape[0])
& (self.store_coil >= self.store_coil_ori.shape[0])
):
self.calibrating = False
self.make_calibration_vector()
elif in_mdf.tool_id == (self.marker + 1):
self.store_coil_pos[self.store_coil, :] = positions
self.store_coil_ori[self.store_coil, :] = orientations
self.store_coil += 1
elif in_mdf.tool_id == (self.plate + 1):
self.store_plate_pos[self.store_plate, :] = positions
self.store_plate_ori[self.store_plate, :] = orientations
self.store_plate += 1
def process_message(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
#print('? %d STATUS=%s TESTING=%s' % (msg_type, str(self.status), str(self.testing)))
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
if in_mdf.tool_id == (self.glasses + 1):
positions = np.array(in_mdf.xyz[:])
orientations = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:])))
self.find_pos_to_glasses(positions, orientations)
def make_calibration_vector(self):
plate_ori = qa.norm(self.store_plate_ori.mean(axis=0))
Ni = self.store_plate_pos.mean(axis=0)
self.Qi = qa.norm(self.store_coil_ori.mean(axis=0))
Ti = self.store_coil_pos.mean(axis=0)
msg_str_pos = "%.5e, " * 3
msg_str_ori = "%.5e, " * 4
sys.stdout.write("Plate orientation: ")
sys.stdout.write(msg_str_ori % (plate_ori[0], plate_ori[1], plate_ori[2], plate_ori[3]) + "\n")
sys.stdout.write("Plate position: ")
sys.stdout.write(msg_str_pos % (Ni[0], Ni[1], Ni[2]) + "\n")
sys.stdout.write("Coil orientation: ")
sys.stdout.write(msg_str_ori % (self.Qi[0], self.Qi[1], self.Qi[2], self.Qi[3]) + "\n")
sys.stdout.write("Coil position: ")
sys.stdout.write(msg_str_pos % (Ti[0], Ti[1], Ti[2]) + "\n")
self.Xi = Ni - Ti
sys.stdout.write("Vector: ")
sys.stdout.write(msg_str_pos % (self.Xi[0], self.Xi[1], self.Xi[2]) + "\n")
sys.stdout.write("********** Calibration complete! ***********\n")
sys.stdout.flush()
self.calibrated = True
def process_message(self, in_msg):
# read a Dragonfly message
msg_type = in_msg.GetHeader().msg_type
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.array(in_mdf.xyz[:])
orientations = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:])))
if in_mdf.tool_id == (self.pointer + 1):
pointer_pos, Qr = self.pointer_tp.get_pos(
orientations, positions)
print pointer_pos
def load_config(self, config_file):
cfg = PyFileConfigLoader(config_file)
cfg.load_config()
self.config = cfg.config
print "HotspotLocator: loading config"
#self.ntools = len(self.config.tool_list)
self.plate = self.config.tools.index('CB609')
self.marker = self.config.tools.index('CT315')
self.glasses = self.config.tools.index('ST568')
self.pointer = self.config.tools.index('P717')
self.pointer_Ti = np.array(self.config.tool_list[self.pointer].Ti)
self.pointer_Qi = qa.norm(
np.array(self.config.tool_list[self.pointer].Qi))
self.pointer_Ni = np.array(self.config.tool_list[self.pointer].Ni)
self.pointer_tp = TP(self.pointer_Qi, self.pointer_Ni, self.pointer_Ti)
def load_config(self, config_file):
cfg = PyFileConfigLoader(config_file)
cfg.load_config()
self.config = cfg.config
self.filename = self.config.head_model
self.plate = self.config.tools.index('CB609')
self.marker = self.config.tools.index('CT315')
self.glasses = self.config.tools.index('ST568')
self.pointer = self.config.tools.index('P717')
self.pointer_Ti = np.array(self.config.tool_list[self.pointer].Ti)
self.pointer_Qi = qa.norm(
np.array(self.config.tool_list[self.pointer].Qi))
self.pointer_Ni = np.array(self.config.tool_list[self.pointer].Ni)
self.pointer_Xi = self.pointer_Ni - self.pointer_Ti
self.tp = TP(self.pointer_Qi, self.pointer_Ni, self.pointer_Ti)
def get(self, ch, vec, maximum=False):
l.info('Computing dipole temperature')
#T_dipole_CMB = doppler_factor(qarray.arraylist_dot(self.satellite_v,vec).flatten()) * T_CMB
vel = qarray.amplitude(self.satellite_v).flatten()
beta = vel / physcon.c
gamma=1/np.sqrt(1-beta**2)
if maximum:
cosdir = 1
else:
cosdir = qarray.arraylist_dot(qarray.norm(self.satellite_v), vec).flatten()
T_dipole_CMB = T_CMB / (gamma * ( 1 - beta * cosdir ))
#T_dipole_CMB = T_CMB * (1 - beta * cosdir )
if self.K_CMB:
return T_dipole_CMB - T_CMB
else:
T_dipole_RJ = ch.Planck_to_RJ( T_dipole_CMB ) - ch.Planck_to_RJ(T_CMB)
return T_dipole_RJ
def calibrate_head(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.asarray(in_mdf.xyz[:])
orientations = self.shuffle_q(np.asarray(in_mdf.ori[:]))
#When arrays have been filled the calibration vector is generated
if (
(self.store_glasses >= self.store_glasses_pos.shape[0]) &
(self.store_glasses >= self.store_glasses_ori.shape[0]) &
(self.store_head >= self.store_head_pos.shape[0]) &
(self.store_head >= self.store_head_ori.shape[0])
):
self.calibrating = False
self.make_calibration_vector()
#Pointer is measured from ball 1, pointer end must be calculated
elif in_mdf.tool_id == (self.pointer + 1):
if self.store_head < self.store_head_pos.shape[0]:
if np.any(np.isnan(positions)) == True:
raise Exception, 'nan present'
elif np.any(np.isnan(orientations)) == True:
raise Exception, 'nan present'
Qf = qa.norm(orientations) #Sometimes gets nan, source unknown
if np.any(np.isnan(Qf)):
print(self.store_head, 'Qf', orientations)
#find_nans(self.store_head, Tk, 'Tk')
Cz_pos, Qr = self.pointer_tp.get_pos(orientations, positions)
#find_nans(self.store_head, Cz_pos, 'Cz')
self.store_head_pos[self.store_head, :] = Cz_pos
self.store_head_ori[self.store_head, :] = orientations
self.store_head += 1
elif in_mdf.tool_id == (self.glasses + 1):
if self.store_glasses < self.store_glasses_pos.shape[0]:
if np.any(np.isnan(positions)) == True:
raise Exception, 'nan present'
if np.any(np.isnan(orientations)) == True:
raise Exception, 'nan present'
self.store_glasses_pos[self.store_glasses, :] = positions
self.store_glasses_ori[self.store_glasses, :] = orientations
self.store_glasses += 1
def get_4piconv_dx10(self, ch, theta, phi, psi):
l.info('Computing dipole temperature with 4pi convolver')
rel_vel = self.satellite_v/physcon.c
# remove psi_pol
psi_nopol = psi - np.radians(ch.get_instrument_db_field("psi_pol"))
# rotate vel to horn reference frame
tohorn_rotation = qarray.norm( qarray.mult(
qarray.rotation([0,0,1], -psi_nopol) ,
qarray.mult(
qarray.rotation([0,1,0], -theta) ,
qarray.rotation([0,0,1], -phi)
)
))
# vel in beam ref frame
vel_rad = qarray.rotate(tohorn_rotation, rel_vel)
dipole_amplitude = self.get_fourpi_prod(vel_rad, ["S100", "S010", "S001"], ch)
# relative corrections
dipole_amplitude += vel_rad[:,0] * self.get_fourpi_prod(vel_rad, ["S200", "S110", "S101"], ch)/2
dipole_amplitude += vel_rad[:,1] * self.get_fourpi_prod(vel_rad, ["S110", "S020", "S011"], ch)/2
dipole_amplitude += vel_rad[:,2] * self.get_fourpi_prod(vel_rad, ["S101", "S011", "S002"], ch)/2
return dipole_amplitude * T_CMB
def process_message(self, in_msg):
msg_type = in_msg.GetHeader().msg_type
#print('? %d STATUS=%s TESTING=%s' % (msg_type, str(self.status), str(self.testing)))
if self.status == False:
pass
elif self.testing:
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.array(in_mdf.xyz[:])
orientations = qa.norm(self.shuffle_q(np.array(in_mdf.ori[:])))
if self.Ptest_count == 250 & self.Gtest_count == 250:
self.testing = False
self.status = False
print 'done'
else:
if in_mdf.tool_id == (self.pointer + 1):
self.pointer_test[self.Ptest_count, :] = positions
if np.any(np.isnan(positions)) == True:
sys.stdout.write('\n Pointer: NaN present')
self.Gtest_count -= 1
elif np.any(np.isnan(orientations)) == True:
sys.stdout.write('\n Pointer: NaN present')
self.Gtest_count -= 1
elif np.any(np.isinf(positions)) == True:
sys.stdout.write('\n Pointer: inf present')
self.Gtest_count -= 1
elif np.any(np.isinf(orientations)) == True:
sys.stdout.write('\n Pointer: inf present')
self.Gtest_count -= 1
elif np.all(positions) == positions[0]:
sys.stdout.write('\n Pointer: Zeros present')
self.Gtest_count -= 1
elif np.all(orientations) == orientations[0]:
sys.stdout.write('\n Pointer: Zeros present')
self.Gtest_count -= 1
else:
self.Ptest_count += 1
if in_mdf.tool_id == (self.glasses + 1):
self.headP_test[self.Gtest_count, :] = positions
self.headQ_test[self.Gtest_count, :] = orientations
if np.any(np.isnan(positions)) == True:
sys.stdout.write('\n Glasses: NaN present')
self.Ptest_count -= 1
elif np.any(np.isnan(orientations)) == True:
sys.stdout.write('\n Glasses: NaN present')
self.Ptest_count -= 1
elif np.any(np.isinf(positions)) == True:
sys.stdout.write('\n Glasses: inf present')
self.Ptest_count -= 1
elif np.any(np.isinf(orientations)) == True:
sys.stdout.write('\n Glasses: inf present')
self.Ptest_count -= 1
elif np.all(positions) == positions[0]:
sys.stdout.write('\n Glasses: Zeros present')
self.Ptest_count -= 1
elif np.all(orientations) == orientations[0]:
sys.stdout.write('\n Glasses: Zeros present')
self.Ptest_count -= 1
else:
self.Gtest_count += 1
print orientations
if (self.Ptest_count < 0) or (self.Gtest_count < 0):
self.Gtest_count = 0
else:
if msg_type == rc.MT_POLARIS_POSITION:
# handling input message
in_mdf = rc.MDF_POLARIS_POSITION()
copy_from_msg(in_mdf, in_msg)
if in_mdf.tool_id == (self.glasses + 1):
positions = np.array(in_mdf.xyz[:])
orientations = qa.norm(
self.shuffle_q(np.array(in_mdf.ori[:])))
self.glasses_pos_buffer = positions
self.glasses_orientation_buffer = orientations
else:
pass
elif msg_type == rc.MT_HOTSPOT_POSITION:
in_mdf = rc.MDF_HOTSPOT_POSITION()
copy_from_msg(in_mdf, in_msg)
positions = np.array(in_mdf.xyz[:])
vector = np.array(in_mdf.ori[:3])
self.coil_pos_buffer = positions
self.coil_vector_buffer = vector
elif msg_type == rc.MT_TMS_TRIGGER:
self.find_hotspot_to_cz(self.coil_pos_buffer,
self.glasses_orientation_buffer,
self.glasses_pos_buffer,
self.coil_vector_buffer)
# In[13]:
q = np.empty([len(vec), 4], dtype=np.float)
# In[14]:
n = len(vec)
for i in range(n):
rotmat = np.hstack([ vec_north[i,:][:,np.newaxis],
np.cross(vec[i,:], vec_north[i,:])[:,np.newaxis],
vec[i,:][:,np.newaxis]
])
q[i] = qa.norm(qa.from_rotmat(rotmat))
if (i > 0):
if np.dot(q[i], q[i-1]) < 0:
q[i] *= -1
# `rotmat` / `q` are the rotation matrix / quaternion between the local system and Equatorial coordinates
# ### Interpolation
# In[18]:
jd = pointing.index.to_julian_date()
# In[19]:
prec_ang_speed = 2 * np.pi / prec_period_seconds
rot_prec_opening = qa.rotation(z_axis, -np.radians(40))
prec_angles = (timestamps * prec_ang_speed) % (2 * np.pi)
rot_prec = qa.mult(qa.rotation(x_axis, prec_angles), rot_prec_opening)
# Spin
spin_period_seconds = 60
spin_ang_speed = 2 * np.pi / spin_period_seconds
spin_angles = (timestamps * spin_ang_speed) % (2 * np.pi)
rot_opening = qa.rotation(z_axis, -np.radians(10))
rot_spin = qa.mult(qa.rotation(x_axis, spin_angles), rot_opening)
# Total quaternions to boresight
bore_quat = qa.norm(qa.mult(rot_earth_orbit, qa.mult(rot_prec, rot_spin)))
bore_v = qa.rotate(bore_quat, x_axis)
pix_1det = hp.vec2pix(nside,
bore_v[:, 0],
bore_v[:, 1],
bore_v[:, 2],
nest=True)
pixels = np.tile(pix_1det, ndet)
# polarization weights
bore_v_proj_ortog = np.hstack([
-bore_v[:, [1]], bore_v[:, [0]],
np.zeros(len(bore_v))[:, np.newaxis]
def get_pos(self, Qk, Tk):
Qk = qa.norm(Qk)
Qr = (qa.mult(Qk, qa.inv(self.Qi))).flatten()
pos = (qa.rotate(Qr, self.Xi)).flatten() + Tk
return pos, Qr
def test_norm(self):
np.testing.assert_array_almost_equal(qarray.norm(self.q1), self.q1/np.linalg.norm(self.q1))
np.testing.assert_array_almost_equal(qarray.norm(self.qtonormalize) , self.qnormalized)
