def setUp(self):
self.data_type = np.float32
self.window_len = 16
self.dft_len = 16
self.hop_len = self.window_len # No overlap
self.use_window = False # No hann
self.n_channels = 1
self.stft_dft = StftManager(dft_length=self.dft_len,
window_length=self.window_len,
hop_length=self.hop_len,
use_window_fcn=self.use_window,
n_channels=self.n_channels,
dtype=self.data_type)
print "Beginning"
def testInvalidHopLength(self):
caught = False
try:
pa_tools = StftManager(hop_length=-1)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testHopGreaterThanWindow(self):
caught = False
try:
pa_tools = StftManager(hop_length=1024, window_length=512)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testInvalidNChannels(self):
caught = False
try:
pa_tools = StftManager(n_channels=-1)
except ValueError:
caught = True
self.assertEquals(caught, True)
def setUp(self):
self.data_type = np.float32
self.window_len = 16
self.dft_len = 16
self.hop_len = self.window_len # No overlap
self.use_window = False # No hann
self.n_channels = 1
self.stft_dft = StftManager(
dft_length=self.dft_len,
window_length=self.window_len,
hop_length=self.hop_len,
use_window_fcn=self.use_window,
n_channels=self.n_channels,
dtype=self.data_type,
)
print "Beginning"
def setUp(self):
self.sampling_rate = 44100
self.dirloc = DirectionLocalizer(mic_layout=None, sample_rate=44100)
self.dft_len = 8
self.stft = StftManager(dft_length=self.dft_len,
window_length=self.dft_len,
hop_length=self.dft_len,
use_window_fcn=False)
mic_positions = np.array([[1, 1, 0], [-1, 1, 0], [-1, -1, 0],
[1, -1, 0], [0, 0, 1]])
self._n_mics = mic_positions.shape[0]
self._n_theta = 4
self._n_phi = 4
self.distrloc = DistributionLocalizer(mic_positions=mic_positions,
dft_len=self.dft_len,
sample_rate=44100,
n_theta=self._n_theta,
n_phi=self._n_phi)
pass
def localize():
global switch_beamforming
global DO_BEAMFORM
# Setup search space
source_plane = OrientedSourcePlane(SOURCE_PLANE_NORMAL, SOURCE_PLANE_UP,
SOURCE_PLANE_OFFSET)
space = SearchSpace(MIC_LOC, CAMERA_LOC, [source_plane], MIC_FORWARD,
MIC_ABOVE)
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
localizer = GridTrackingLocalizer(mic_positions=mic_layout,
search_space=space,
source_cov=SOURCE_LOCATION_COV,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
beamformer = BeamFormer(mic_layout, SAMPLE_RATE)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
quit_thread = threading.Thread(target=check_for_quit)
quit_thread.start()
# Setup directions and alignment matrices
direcs = localizer.get_directions()
align_mats = localizer.get_pos_align_mat()
# Plotting setup
if PLOT_POLAR:
fig = plt.figure(facecolor='white')
ax = fig.add_subplot(111, projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
#post_plot, = plt. plot(theta, np.ones(theta.shape), 'green')
ax.set_ylim(0, 1)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_CARTES:
fig = plt.figure(facecolor='white')
ax = fig.add_subplot(111)
ax.set_ylim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
#post_plot, = plt. plot(theta, np.ones(theta.shape), 'green')
ax.set_ylim(0, 1)
ax.set_xlim(0, np.pi)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
try:
global done
while in_stream.is_active() or out_stream.is_active():
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
if switch_beamforming:
DO_BEAMFORM = not DO_BEAMFORM
switch_beamforming = False
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
rffts = mat.to_all_real_matlab_format(dfts)
d, energy = localizer.get_distribution_real(
rffts[:, :, 0], 'gcc') # Use first hop
post = localizer.get_distribution(rffts[:, :, 0])
ind = np.argmax(d)
u = 1.5 * direcs[:, ind] # Direction of arrival
# Do beam forming
if DO_BEAMFORM:
align_mat = align_mats[:, :, ind]
filtered = beamformer.filter_real(rffts, align_mat)
mat.set_dfts_real(dfts, filtered, n_channels=2)
# Take care of plotting
if count % 1 == 0:
if PLOT_POLAR or PLOT_CARTES:
#d -= np.min(d)
d = localizer.to_spher_grid(d)
post = localizer.to_spher_grid(post) * 50
#d /= np.max(d)
if np.max(d) > 1:
d /= np.max(d)
if np.max(post) > 1:
post /= np.max(post)
pol_plot.set_ydata(d[0, :])
#post_plot.set_ydata(post[0, :])
if DO_BEAMFORM:
# Get beam plot
freq = 1900. # Hz
response = beamformer.get_beam(
align_mat, align_mats, rffts, freq)
response = localizer.to_spher_grid(response)
if np.max(response) > 1:
response /= np.max(response)
pol_beam_plot.set_ydata(response[-1, :])
plt.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO or RECORD_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data,
NUM_CHANNELS_IN,
NUM_CHANNELS_OUT)
# Write out the new, altered data
if PLAY_AUDIO:
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
if RECORD_AUDIO:
if record_buf.get_available_write() >= WINDOW_LENGTH:
record_buf.write_samples(new_data)
except KeyboardInterrupt:
print "Program interrupted"
done = True
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
# Take care of output file
if RECORD_AUDIO:
print "Writing output file"
make_wav()
print "Done"
def localize():
# Global variables that may be set in this function
global switch_beamforming
global DO_BEAMFORM
global done
# Setup search space
source_plane = OrientedSourcePlane(SOURCE_PLANE_NORMAL, SOURCE_PLANE_UP,
SOURCE_PLANE_OFFSET)
space = SearchSpace(MIC_LOC, CAMERA_LOC, [source_plane], MIC_FORWARD,
MIC_ABOVE)
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
listener = CommandListener()
plot_manager = PlotManager()
#localizer = GridTrackingLocalizer(mic_positions=mic_layout,
# search_space=space,
# source_cov=SOURCE_LOCATION_COV,
# dft_len=FFT_LENGTH,
# sample_rate=SAMPLE_RATE,
# n_theta=N_THETA,
# n_phi=N_PHI)
localizer = KalmanTrackingLocalizer(mic_positions=mic_layout,
search_space=space,
mic_forward=MIC_FORWARD,
mic_above=MIC_ABOVE,
trans_mat=STATE_TRANSITION_MAT,
state_cov=STATE_TRANSITION_MAT,
emission_mat=EMISSION_MAT,
emission_cov=EMISSION_COV,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
beamformer = BeamFormer(mic_layout, SAMPLE_RATE)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
listener.start_polling()
# Setup directions and alignment matrices
direcs = localizer.get_directions()
align_mats = localizer.get_pos_align_mat()
# Plotting setup
if PLOT_POLAR:
fig = plt.figure()
ax = fig.add_subplot(111, projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.zeros(theta.shape))
post_plot, = plt.plot(theta, np.zeros(theta.shape), 'green')
ax.set_ylim(0, 1)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_CARTES:
fig = plt.figure()
ax = plotting.get_halfpage_axis(fig)
#ax = fig.add_subplot(111)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
theta = np.linspace(0, 1, theta.shape[0])
gcc_plots = []
gcc_shaping_vals = [1, 2, 3, 4, 5]
for i in gcc_shaping_vals:
plot, = plt.plot(theta, np.zeros(theta.shape))
gcc_plots.append(plot)
pol_plot, = plt.plot(theta, np.zeros(theta.shape), 'r--')
post_plot, = plt.plot(theta, np.zeros(theta.shape), 'b')
ax.set_ylim(0, 1.2)
ax.set_xlim(0, 1) # Normalized
#ax.set_xlabel('Angle $\left(\\frac{1}{\pi}\\right)$')
#ax.set_ylabel('Normalized GCC')
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_2D:
n_past_samples = 200
filter_plot = FilterPlot(N_THETA, n_past_samples, 2)
save_plot = filter_plot # For saving figures
if VIDEO_OVERLAY:
vc = cv2.VideoCapture(0)
video_handle, video_plot = setup_video_handle(720, 1280)
plt.show(block=False)
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
try:
while in_stream.is_active() or out_stream.is_active():
done = listener.quit()
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
if listener.switch_beamforming():
DO_BEAMFORM = not DO_BEAMFORM
if listener.savefig():
plot_manager.savefig(save_plot.get_figure())
# Get data from circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
rffts = mat.to_all_real_matlab_format(dfts)
gccs = []
#for k in gcc_shaping_vals:
# d, energy = localizer.get_distribution_real(
# rffts[:, :, 0], 'mcc', k) # Use first hop
# gccs.append(d)
d, energy = localizer.get_distribution_real(
rffts[:, :, 0], 'beam') # Use first hop
def w(cpmat):
cpmat /= (np.abs(cpmat + consts.EPS))
return cpmat
post = localizer.get_distribution(rffts[:, :, 0], 'beam')
#post, bla = localizer.get_distribution_real(rffts[:, :, 0], 'mcc')
#post = localizer.get_distribution(rffts[:, :, 0])
ind = np.argmax(d)
u = 1.5 * direcs[:, ind] # Direction of arrival
#if energy < 500:
#continue
# Do beam forming
if DO_BEAMFORM:
align_mat = align_mats[:, :, ind]
filtered = beamformer.filter_real(rffts, align_mat)
mat.set_dfts_real(dfts, filtered, n_channels=2)
# Take care of plotting
if count % 1 == 0:
if PLOT_POLAR or PLOT_CARTES:
dist = d
#dist -= np.min(dist)
dist = localizer.to_spher_grid(dist)
print post.shape
post = localizer.to_spher_grid(post) * 50
dist /= np.max(dist)
if np.max(dist) > 1:
dist /= np.max(dist)
if np.max(post) > 1:
post /= np.max(post)
pol_plot.set_ydata(dist[0, :])
post_plot.set_ydata(post[0, :])
#for i, plot in enumerate(gcc_plots):
# gcc = gccs[i]
# gcc /= (np.max(gcc) + consts.EPS)
# plot.set_ydata(gccs[i])
if DO_BEAMFORM:
# Get beam plot
freq = 2500. # Hz
response = beamformer.get_beam(
align_mat, align_mats, rffts, freq)
response = localizer.to_spher_grid(response)
if np.max(response) > 1:
response /= np.max(response)
pol_beam_plot.set_ydata(response[-1, :])
plt.draw()
if PLOT_2D:
dist = localizer.to_spher_grid(d)
p = localizer.to_spher_grid(post)
est1 = THETA_SPACE[np.argmax(p)]
est2 = THETA_SPACE[np.argmax(dist)]
filter_plot.update(dist, [est1, est2])
if VIDEO_OVERLAY:
post /= np.max(post + consts.EPS)
dist = d - np.min(d)
dist = dist / np.max(dist + consts.EPS)
_, cvimage = vc.read()
overlay_distribution(video_handle, video_plot, cvimage,
dist[::-1])
plt.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO or RECORD_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data,
NUM_CHANNELS_IN,
NUM_CHANNELS_OUT)
# Write out the new, altered data
if PLAY_AUDIO:
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
if RECORD_AUDIO:
if record_buf.get_available_write() >= WINDOW_LENGTH:
record_buf.write_samples(new_data)
except KeyboardInterrupt:
print "Program interrupted"
listener.set_quit(True)
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
# Take care of output file
if RECORD_AUDIO:
print "Writing output file"
make_wav()
print "Done"
def localize():
global switch_beamforming
global DO_BEAMFORM
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
localizer = DistributionLocalizer(mic_positions=mic_layout,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
beamformer = BeamFormer(mic_layout, SAMPLE_RATE)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
quit_thread = threading.Thread(target=check_for_quit)
quit_thread.start()
# Setup directions and alignment matrices
direcs = localizer.get_directions()
align_mats = localizer.get_pos_align_mat()
# Plotting setup
if PLOT_CARTES:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plt.show(block=False)
x = localizer.to_spher_grid(direcs[0, :])
y = localizer.to_spher_grid(direcs[1, :])
z = localizer.to_spher_grid(direcs[2, :])
#scat = ax.scatter(x, y, z, s=100)
if PLOT_POLAR:
fig = plt.figure()
ax = fig.add_axes([.1, .1, .8, .8], projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
pol = localizer.to_spher_grid(spher_coords[2, :])
weight = 1. - .3 * np.sin(
2 * pol) # Used to pull visualization off edges
r = np.sin(pol) * weight
theta = localizer.to_spher_grid(spher_coords[1, :])
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
try:
global done
while in_stream.is_active() or out_stream.is_active():
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
if switch_beamforming:
DO_BEAMFORM = not DO_BEAMFORM
switch_beamforming = False
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
rffts = mat.to_all_real_matlab_format(dfts)
d, energy = localizer.get_distribution_real(rffts[:, :, 0])
ind = np.argmax(d)
u = 1.5 * direcs[:, ind] # Direction of arrival
# Do beam forming
if DO_BEAMFORM:
align_mat = align_mats[:, :, ind]
filtered = beamformer.filter_real(rffts, align_mat)
mat.set_dfts_real(dfts, filtered, n_channels=2)
# Get beam plot
freq = 1500. # Hz
response = beamformer.get_beam(align_mat, align_mats,
rffts, freq)
response = localizer.to_spher_grid(response)
# Take car of plotting
if count % 1 == 0:
if PLOT_CARTES:
ax.cla()
ax.grid(False)
d = localizer.to_spher_grid(d /
(np.max(d) + consts.EPS))
ax.scatter(x, y, z, c=d, s=40)
#ax.plot_surface(x, y, z, rstride=1, cstride=1, facecolor=plt.cm.gist_heat(d))
ax.plot([0, u[0]], [0, u[1]], [0, u[2]],
c='black',
linewidth=3)
if DO_BEAMFORM:
if np.max(np.abs(response)) > 1:
response /= np.max(np.abs(response))
X = response * x
Y = response * y
Z = response * z
ax.plot_surface(X,
Y,
Z,
rstride=1,
cstride=1,
color='white')
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(0, 1)
#ax.view_init(90, -90)
fig.canvas.draw()
if PLOT_POLAR:
plt.cla()
d = localizer.to_spher_grid(d)
con = ax.contourf(theta, r, d, vmin=0, vmax=40)
con.set_cmap('gist_heat')
if DO_BEAMFORM:
response = response[
-1, :] # Pick which polar angle sample to use
ax.plot(theta[0, :], response, 'cyan', linewidth=4)
ax.set_rlim(0, 1)
fig.canvas.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO or RECORD_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data,
NUM_CHANNELS_IN,
NUM_CHANNELS_OUT)
# Write out the new, altered data
if PLAY_AUDIO:
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
if RECORD_AUDIO:
if record_buf.get_available_write() >= WINDOW_LENGTH:
record_buf.write_samples(new_data)
except KeyboardInterrupt:
print "Program interrupted"
done = True
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
# Take care of output file
if RECORD_AUDIO:
print "Writing output file"
make_wav()
print "Done"
def localize():
global switch_beamforming
global DO_BEAMFORM
# Setup search space
source_plane = OrientedSourcePlane(SOURCE_PLANE_NORMAL,
SOURCE_PLANE_UP,
SOURCE_PLANE_OFFSET)
space = SearchSpace(MIC_LOC, CAMERA_LOC, [source_plane])
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
localizer = KalmanTrackingLocalizer(mic_positions=mic_layout,
search_space=space,
mic_forward=MIC_FORWARD,
mic_above=MIC_ABOVE,
trans_mat=STATE_TRANSITION_MAT,
state_cov=STATE_TRANSITION_MAT,
emission_mat=EMISSION_MAT,
emission_cov=EMISSION_COV,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
beamformer = BeamFormer(mic_layout, SAMPLE_RATE)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
quit_thread = threading.Thread(target=check_for_quit)
quit_thread.start()
# Setup directions and alignment matrices
direcs = localizer.get_directions()
align_mats = localizer.get_pos_align_mat()
# Plotting setup
if PLOT_POLAR:
fig = plt.figure()
ax = fig.add_subplot(111, projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
post_plot, = plt. plot(theta, np.ones(theta.shape), 'green')
ax.set_ylim(0, 1)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_CARTES:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plt.show(block=False)
x = localizer.to_spher_grid(direcs[0, :])
y = localizer.to_spher_grid(direcs[1, :])
z = localizer.to_spher_grid(direcs[2, :])
#scat = ax.scatter(x, y, z, s=100)
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
try:
global done
while in_stream.is_active() or out_stream.is_active():
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
if switch_beamforming:
DO_BEAMFORM = not DO_BEAMFORM
switch_beamforming = False
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
rffts = mat.to_all_real_matlab_format(dfts)
d, energy = localizer.get_distribution_real(rffts[:, :, 0], 'gcc') # Use first hop
post = localizer.get_distribution(rffts[:, :, 0])
ind = np.argmax(post)
u = 1.5 * direcs[:, ind] # Direction of arrival
#if energy < 500:
# continue
# Do beam forming
if DO_BEAMFORM:
align_mat = align_mats[:, :, ind]
filtered = beamformer.filter_real(rffts, align_mat)
mat.set_dfts_real(dfts, filtered, n_channels=2)
# Take care of plotting
if count % 1 == 0:
if PLOT_CARTES:
ax.cla()
ax.grid(False)
d = localizer.to_spher_grid(post / (np.max(post) + consts.EPS))
#d = localizer.to_spher_grid(d / (np.max(d) + consts.EPS))
ax.scatter(x, y, z, c=d, s=40)
#ax.plot_surface(x, y, z, rstride=1, cstride=1, facecolor=plt.cm.gist_heat(d))
ax.plot([0, u[0]], [0, u[1]], [0, u[2]], c='black', linewidth=3)
if DO_BEAMFORM:
if np.max(np.abs(response)) > 1:
response /= np.max(np.abs(response))
X = response * x
Y = response * y
Z = response * z
ax.plot_surface(X, Y, Z, rstride=1, cstride=1, color='white')
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(0, 1)
#ax.view_init(90, -90)
fig.canvas.draw()
if PLOT_2D:
# Get unconditional distribution
dist = localizer.to_spher_grid(d)
dist -= np.min(dist)
dist /= (np.sum(dist) + consts.EPS)
sample_mat[:, :-1] = sample_mat[:, 1:]
sample_mat[:, -1] = dist
# Get kalman estimate
maxind = np.argmax(post)
estimate_mat[:-1] = estimate_mat[1:]
estimate_mat[-1] = maxind
plot_2d.set_array(sample_mat)
state_est_plot.set_ydata(estimate_mat)
plt.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO or RECORD_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data, NUM_CHANNELS_IN, NUM_CHANNELS_OUT)
# Write out the new, altered data
if PLAY_AUDIO:
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
if RECORD_AUDIO:
if record_buf.get_available_write() >= WINDOW_LENGTH:
record_buf.write_samples(new_data)
except KeyboardInterrupt:
print "Program interrupted"
done = True
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
# Take care of output file
if RECORD_AUDIO:
print "Writing output file"
make_wav()
print "Done"
while True:
read_in = raw_input()
if read_in == "q":
done = True
break
if __name__ == '__main__':
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Get devices
in_device = helper.get_input_device_from_user()
out_device = helper.get_output_device_from_user()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
__author__ = 'adamjmiller'
from pa_tools.stftmanager import StftManager
import numpy as np
import scipy.fftpack as dft
dft_len = 8
window_len = 8
hop_len = 4
n_channels = 1
stft = StftManager(dft_length=dft_len, window_length=window_len,
hop_length=window_len, # No overlap -- easy to check accuracy
use_window_fcn=False,
n_channels=n_channels)
data = np.array([1, 0, 1, 0, 1, 0, 1, 0], dtype=np.float32)
#data = np.ndarray([window_len, 1], dtype=np.float32)
#data[:, 0] = np.array([1, 0, 1, 0, 1, 0, 1, 0])
stft.performStft(data)
print data.shape
npdft = dft.rfft(data)
print "npdft: " + str(npdft)
dfts = stft.getDFTs() # List of dfts for each channel
reals, imags = dfts[0]
real_part = reals[0]
imag_part = imags[0]
real_str = ""
def localize():
global switch_beamforming
global DO_BEAMFORM
# Setup search space
source_plane = OrientedSourcePlane(SOURCE_PLANE_NORMAL, SOURCE_PLANE_UP,
SOURCE_PLANE_OFFSET)
space = SearchSpace(MIC_LOC, CAMERA_LOC, [source_plane])
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
localizer = KalmanTrackingLocalizer(mic_positions=mic_layout,
search_space=space,
mic_forward=MIC_FORWARD,
mic_above=MIC_ABOVE,
trans_mat=STATE_TRANSITION_MAT,
state_cov=STATE_TRANSITION_MAT,
emission_mat=EMISSION_MAT,
emission_cov=EMISSION_COV,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
beamformer = BeamFormer(mic_layout, SAMPLE_RATE)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
quit_thread = threading.Thread(target=check_for_quit)
quit_thread.start()
# Setup directions and alignment matrices
direcs = localizer.get_directions()
align_mats = localizer.get_pos_align_mat()
# Plotting setup
if PLOT_POLAR:
fig = plt.figure()
ax = fig.add_subplot(111, projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
post_plot, = plt.plot(theta, np.ones(theta.shape), 'green')
ax.set_ylim(0, 1)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_CARTES:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plt.show(block=False)
x = localizer.to_spher_grid(direcs[0, :])
y = localizer.to_spher_grid(direcs[1, :])
z = localizer.to_spher_grid(direcs[2, :])
#scat = ax.scatter(x, y, z, s=100)
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
try:
global done
while in_stream.is_active() or out_stream.is_active():
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
if switch_beamforming:
DO_BEAMFORM = not DO_BEAMFORM
switch_beamforming = False
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
rffts = mat.to_all_real_matlab_format(dfts)
d, energy = localizer.get_distribution_real(
rffts[:, :, 0], 'gcc') # Use first hop
post = localizer.get_distribution(rffts[:, :, 0])
ind = np.argmax(post)
u = 1.5 * direcs[:, ind] # Direction of arrival
#if energy < 500:
# continue
# Do beam forming
if DO_BEAMFORM:
align_mat = align_mats[:, :, ind]
filtered = beamformer.filter_real(rffts, align_mat)
mat.set_dfts_real(dfts, filtered, n_channels=2)
# Take care of plotting
if count % 1 == 0:
if PLOT_CARTES:
ax.cla()
ax.grid(False)
d = localizer.to_spher_grid(
post / (np.max(post) + consts.EPS))
#d = localizer.to_spher_grid(d / (np.max(d) + consts.EPS))
ax.scatter(x, y, z, c=d, s=40)
#ax.plot_surface(x, y, z, rstride=1, cstride=1, facecolor=plt.cm.gist_heat(d))
ax.plot([0, u[0]], [0, u[1]], [0, u[2]],
c='black',
linewidth=3)
if DO_BEAMFORM:
if np.max(np.abs(response)) > 1:
response /= np.max(np.abs(response))
X = response * x
Y = response * y
Z = response * z
ax.plot_surface(X,
Y,
Z,
rstride=1,
cstride=1,
color='white')
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(0, 1)
#ax.view_init(90, -90)
fig.canvas.draw()
if PLOT_2D:
# Get unconditional distribution
dist = localizer.to_spher_grid(d)
dist -= np.min(dist)
dist /= (np.sum(dist) + consts.EPS)
sample_mat[:, :-1] = sample_mat[:, 1:]
sample_mat[:, -1] = dist
# Get kalman estimate
maxind = np.argmax(post)
estimate_mat[:-1] = estimate_mat[1:]
estimate_mat[-1] = maxind
plot_2d.set_array(sample_mat)
state_est_plot.set_ydata(estimate_mat)
plt.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO or RECORD_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data,
NUM_CHANNELS_IN,
NUM_CHANNELS_OUT)
# Write out the new, altered data
if PLAY_AUDIO:
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
if RECORD_AUDIO:
if record_buf.get_available_write() >= WINDOW_LENGTH:
record_buf.write_samples(new_data)
except KeyboardInterrupt:
print "Program interrupted"
done = True
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
# Take care of output file
if RECORD_AUDIO:
print "Writing output file"
make_wav()
print "Done"
def localize():
# Setup search space
# x vector points to front of class, -z vector points to floor
teacher_plane = SourcePlane(TEACHER_NORMAL, TEACHER_OFFSET)
student_plane = SourcePlane(STUDENT_NORMAL, STUDENT_OFFSET)
space = SearchSpace(MIC_LOC, CAMERA_LOC, [teacher_plane, student_plane])
# Setup camera
forward = np.array([1, 0, 0])
above = np.array([0, 0, 1])
camera = SonyCamera(URL, forward, above)
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
localizer = DistributionLocalizer(mic_positions=mic_layout,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
quit_thread = threading.Thread(target=check_for_quit)
quit_thread.start()
# Plotting setup
if PLOT_CARTES:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plt.show(block=False)
scat = []
if PLOT_SPACE:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# Setup bounds
xlo, xhi = (-5, DISTANCE_TO_TEACHER + 5)
ylo, yhi = (-15, 15)
zlo, zhi = (-15, 5)
# Setup grid
nx, ny = (200, 100)
x = np.linspace(xlo, xhi, nx)
y = np.linspace(ylo, yhi, ny)
X, Y = np.meshgrid(x, y)
n, m = (STUDENT_NORMAL, STUDENT_OFFSET)
TP = (n.dot(m) - n[0] * X - n[1] * Y) / n[2] - 2
# Plot markers for mic
m = MIC_LOC
ax.plot([MIC_LOC[0]], [MIC_LOC[1]], [MIC_LOC[2]], 'r.', markersize=10.)
# Plot marker for camera
c = CAMERA_LOC
ax.plot([CAMERA_LOC[0]], [CAMERA_LOC[1]], [CAMERA_LOC[2]], 'b.', markersize=10.)
# Draw lines from camera and mic to source
source_loc = np.array([10, 0, 0])
source_point, = ax.plot([source_loc[0]], [source_loc[1]], [source_loc[2]], 'black', marker='.', markersize=10.)
s = source_loc
camera_dir, = ax.plot([c[0], m[0]], [c[1], m[1]], [c[2], m[2]], 'blue')
mic_dir, = ax.plot([m[0], m[0]], [m[1], m[1]], [m[2], m[2]], 'red')
#ax.plot_surface(X, Y, TP)
ax.set_xlim(xlo, xhi)
ax.set_ylim(ylo, yhi)
ax.set_zlim(zlo, zhi)
ax.view_init(elev=25, azim=-120)
plt.show(block=False)
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
prev_direc = np.array([0, 0, 0])
direcs = localizer.get_directions()
try:
global done
while in_stream.is_active() or out_stream.is_active():
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
d = localizer.get_3d_real_distribution(dfts)
ind = np.argmax(d)
u = 1.5 * direcs[:, ind] # Direction of arrival
if DO_TRACK and count % TRACKING_FREQ == 0:
#v = np.array([1, 0, 1])
v = u
direc = space.get_camera_dir(v)
if not direc.any():
direc = prev_direc
else:
prev_direc = direc
# Send camera new direction
camera.face_direction(direc)
if PLOT_SPACE:
if direc.any():
src = space.get_source_loc(u)
source_point.set_xdata([src[0]])
source_point.set_ydata([src[1]])
source_point.set_3d_properties(zs=[src[2]])
cam_src = CAMERA_LOC + 30 * direc
mic_src = MIC_LOC + 30 * u
# Update camera line
camera_dir.set_xdata([CAMERA_LOC[0], cam_src[0]])
camera_dir.set_ydata([CAMERA_LOC[1], cam_src[1]])
camera_dir.set_3d_properties(zs=[CAMERA_LOC[2], cam_src[2]])
# Update mic line
mic_dir.set_xdata([MIC_LOC[0], mic_src[0]])
mic_dir.set_ydata([MIC_LOC[1], mic_src[1]])
mic_dir.set_3d_properties(zs=[MIC_LOC[2], mic_src[2]])
plt.draw()
# Take care of plotting
if count % 1 == 0:
if PLOT_CARTES:
plt.cla()
ax.scatter(direcs[0, :], direcs[1, :], direcs[2, :], s=30, c=d[:])
ax.plot([0, u[0]], [0, u[1]], [0, u[2]], c='blue')
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(0, 1)
plt.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data, NUM_CHANNELS_IN, NUM_CHANNELS_OUT)
# Write out the new, altered data
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
#time.sleep(.05)
except KeyboardInterrupt:
print "Program interrupted"
done = True
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
print "Done"
def localize():
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
localizer = DistributionLocalizer(mic_positions=mic_layout,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
quit_thread = threading.Thread(target=check_for_quit)
quit_thread.start()
# Plotting setup
if PLOT_POLAR:
fig = plt.figure()
ax = fig.add_subplot(111, projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
ax.set_ylim(0, 1)
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
direcs = localizer.get_directions()
try:
global done
while in_stream.is_active() or out_stream.is_active():
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
d = localizer.get_3d_real_distribution(dfts)
ind = np.argmax(d)
u = 1.5 * direcs[:, ind] # Direction of arrival
# Take car of plotting
if count % 1 == 0:
if PLOT_POLAR:
d = localizer.to_spher_grid(d)
d /= np.max(d)
pol_plot.set_ydata(d[0, :])
plt.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data,
NUM_CHANNELS_IN,
NUM_CHANNELS_OUT)
# Write out the new, altered data
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
#time.sleep(.05)
except KeyboardInterrupt:
print "Program interrupted"
done = True
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
print "Done"
def testConstructor(self):
StftManager()
def setUp(self):
self.dft_len = 8
self.stft = StftManager(dft_length=self.dft_len,
window_length=self.dft_len,
hop_length=self.dft_len,
use_window_fcn=False)
class MatToolsTest(unittest.TestCase):
def setUp(self):
self.dft_len = 8
self.stft = StftManager(dft_length=self.dft_len,
window_length=self.dft_len,
hop_length=self.dft_len,
use_window_fcn=False)
def testZipFFT(self):
reals = [4, 0, 1, 0]
imags = [2, 1, 0, 0]
zipped = mat.zip_fft(reals, imags)
self.assertListFloatEqual(zipped, [4, 0, 1, 1, 0, 0, 0, 2])
def testToNumpyFormat(self):
dfts = []
dfts.append(([[4, 0, 1, 0]], [[2, 1, 0, 0]]))
dfts.append(([[1, 0, 0, 1]], [[0, 1, 1, 0]]))
arr = mat.to_numpy_format(dfts)
self.assertListFloatEqual(arr[:, 0], [4, 0, 1, 1, 0, 0, 0, 2])
self.assertListFloatEqual(arr[:, 1], [1, 0, 1, 0, 1, 1, 0, 0])
def testFFTComp(self):
data = np.array([1, 0, 0, 1, 1, 0, 0, 1], dtype=np.float32)
self.stft.performStft(data)
dfts = self.stft.getDFTs()
transformed = mat.to_numpy_format(dfts)
ifftout = fftp.irfft(transformed[:, 0] / 2)
print ifftout
self.assertListFloatEqual(data, ifftout)
def testFullFft(self):
data = np.array([1, 2, 1, 3, 1, 4, 1, 2], dtype=np.float32)
dft = np.array([
15, -2.1213 + 0.7071j, -1j, 2.1213 + 0.7071j, -7, 2.1213 - 0.7071j,
1j, -2.1213 - 0.7071j
])
self.stft.performStft(data)
dfts = self.stft.getDFTs()
reals = dfts[0][0][0]
imags = dfts[0][1][0]
full_fft = mat.to_full_fft(reals, imags)
print full_fft / 2
self.assertListFloatEqual(dft, full_fft / 2)
def testRIFFT(self):
data = np.array([1, 2, 1, 3, 1, 4, 1, 2], dtype=np.float32)
dft = np.array([15, -2.1213 + 0.7071j, -1j, 2.1213 + 0.7071j, -7])
inver = np.fft.irfft(dft)
self.assertListFloatEqual(data, inver)
def testFullFfft2(self):
reals = [4, 0, 1, 0]
imags = [1, 2, 0, 0]
full_fft = mat.to_full_fft(reals, imags)
self.assertListFloatEqual(np.array([4, 2j, 1, 0, 1, 0, 1, -2j]),
full_fft)
def testRealFFT(self):
reals = [4, 0, 1, 0]
imags = [1, 2, 0, 0]
half_fft = mat.to_real_fft(reals, imags)
self.assertListFloatEqual(np.array([4, 2j, 1, 0, 1]), half_fft)
def testToMatlab(self):
dfts = []
dfts.append(([[4, 0, 1, 0]], [[2, 1, 0, 0]]))
dfts.append(([[1, 0, 0, 1]], [[0, 1, 1, 0]]))
dft_arr = mat.to_matlab_format(dfts)
chan1 = np.array([4, 1j, 1, 0, 2, 0, 1, -1j], dtype=np.complex64)
chan2 = np.array([1, 1j, 1j, 1, 0, 1, -1j, -1j], dtype=np.complex64)
print dft_arr
self.assertListFloatEqual(chan1, dft_arr[0, :])
self.assertListFloatEqual(chan2, dft_arr[1, :])
def testToRealMatlab(self):
dfts = []
dfts.append(([[4, 0, 1, 0]], [[2, 1, 0, 0]]))
dfts.append(([[1, 0, 0, 1]], [[0, 1, 1, 0]]))
dft_arr = mat.to_real_matlab_format(dfts)
chan1 = np.array([4, 1j, 1, 0, 2], dtype=np.complex64)
chan2 = np.array([1, 1j, 1j, 1, 0], dtype=np.complex64)
print dft_arr
self.assertListFloatEqual(chan1, dft_arr[0, :])
self.assertListFloatEqual(chan2, dft_arr[1, :])
def testAllToRealMatlab(self):
dfts = []
dfts.append(([[4, 0, 1, 0], [1, 0, 0, 1]], [[2, 1, 0, 0], [0, 1, 1,
0]]))
dfts.append(([[1, 0, 0, 1], [4, 0, 1, 0]], [[0, 1, 1, 0], [2, 1, 0,
0]]))
dft_arr = mat.to_all_real_matlab_format(dfts)
chan1 = np.array([4, 1j, 1, 0, 2], dtype=np.complex64)
chan2 = np.array([1, 1j, 1j, 1, 0], dtype=np.complex64)
print dft_arr
self.assertListFloatEqual(chan1, dft_arr[0, :, 0])
self.assertListFloatEqual(chan2, dft_arr[0, :, 1])
self.assertListFloatEqual(chan2, dft_arr[1, :, 0])
self.assertListFloatEqual(chan1, dft_arr[1, :, 1])
def testNormalizeRows(self):
data = np.array([[1, 0, 3], [0, 0, 0], [2, 2, 4]], dtype=np.float32)
a = mat.normalize_rows(data.copy())
correct = np.array([[.25, 0., 0.75], [0., 0., 0.], [0.25, 0.25, 0.5]])
self.assertEquals((a - correct).any(), False)
# Test normalization of only submatrix
mat.normalize_rows(data[1:, :])
self.assertEquals((data[1:, :] - correct[1:, :]).any(), False)
self.assertEquals((data[0] - np.array([1, 0, 3])).any(), False)
def testSetDftReal(self):
dfts = []
list1 = list(np.array([4, 0, 1, 0], dtype=np.float32))
dfts.append(([list1], [[2., 1, 0, 0]]))
dfts.append(([[1., 0, 0, 1]], [[0., 1, 1, 0]]))
rfft = np.array([1, 2j, 3, 4j, 5], dtype=np.complex64)
mat.set_dft_real(dfts[0][0][0], dfts[0][1][0], rfft)
mat.set_dft_real(dfts[1][0][0], dfts[1][1][0], rfft)
print dfts[0][0][0]
self.assertListFloatEqual(dfts[0][0][0], [1, 0, 3, 0])
self.assertListFloatEqual(dfts[0][1][0], [5, 2, 0, 4])
self.assertListFloatEqual(dfts[1][0][0], [1, 0, 3, 0])
self.assertListFloatEqual(dfts[1][1][0], [5, 2, 0, 4])
def testSetDftsReal(self):
dfts = []
dfts.append(([[4, 0, 1, 0], [1, 0, 0, 1]], [[2, 1, 0, 0], [0, 1, 1,
0]]))
dfts.append(([[1, 0, 0, 1], [4, 0, 1, 0]], [[0, 1, 1, 0], [2, 1, 0,
0]]))
rffts = np.array([[1, 2j, 3, 4j, 5], [10, 20j, 30, 40j, 50]],
dtype=np.complex64)
mat.set_dfts_real(dfts, rffts)
for n in range(2):
reals = dfts[n][0]
imags = dfts[n][1]
self.assertListEqual(reals[0], [1, 0, 3, 0])
self.assertListEqual(imags[0], [5, 2, 0, 4])
self.assertListEqual(reals[1], [10, 0, 30, 0])
self.assertListEqual(imags[1], [50, 20, 0, 40])
def testSetDftsReal1Chan(self):
dfts = []
dfts.append(([[4, 0, 1, 0], [1, 0, 0, 1]], [[2, 1, 0, 0], [0, 1, 1,
0]]))
dfts.append(([[1, 0, 0, 1], [4, 0, 1, 0]], [[0, 1, 1, 0], [2, 1, 0,
0]]))
rffts = np.array([[1, 2j, 3, 4j, 5], [10, 20j, 30, 40j, 50]],
dtype=np.complex64)
mat.set_dfts_real(dfts, rffts, n_channels=1)
reals = dfts[0][0]
imags = dfts[0][1]
self.assertListEqual(reals[0], [1, 0, 3, 0])
self.assertListEqual(imags[0], [5, 2, 0, 4])
self.assertListEqual(reals[1], [10, 0, 30, 0])
self.assertListEqual(imags[1], [50, 20, 0, 40])
reals = dfts[1][0]
imags = dfts[1][1]
self.assertListEqual(reals[0], [1, 0, 0, 1])
self.assertListEqual(imags[0], [0, 1, 1, 0])
self.assertListEqual(reals[1], [4, 0, 1, 0])
self.assertListEqual(imags[1], [2, 1, 0, 0])
def testCheckVecEmpty(self):
vec = np.array([])
mat.check_vec(vec)
vec = np.array([[]])
self.assertRaises(ValueError, mat.check_vec, vec)
def testCheckVecMat(self):
vec = np.array([[1]])
self.assertRaises(ValueError, mat.check_vec, vec)
vec = np.array([[2, 4, 5], [2, 3, 3]])
self.assertRaises(ValueError, mat.check_vec, vec)
def testCheck3dVecMat(self):
vec = np.array([[1, 1, 1]])
self.assertRaises(ValueError, mat.check_3d_vec, vec)
def testCheck3dVec2(self):
vec = np.array([1, 1])
self.assertRaises(ValueError, mat.check_3d_vec, vec)
def testToFloat(self):
vec = np.array([1, 1], dtype=np.int32)
vec = mat.to_float(vec)
self.assertEquals(vec.dtype, np.float)
def assertListFloatEqual(self, list1, list2):
if not len(list1) == len(list2):
raise AssertionError("Lists differ in lenght. Cannot be equal")
for i in range(len(list1)):
try:
self.assertLessEqual(abs(list1[i] - list2[i]), 1e-4)
except AssertionError:
err_str = "Lists differ on element " + str(i) + ": " + \
str(list1[i]) + " vs. " + str(list2[i])
raise AssertionError(err_str)
def tearDown(self):
pass
def localize():
global switch_beamforming
global DO_BEAMFORM
global done
# Setup search space
source_plane = OrientedSourcePlane(SOURCE_PLANE_NORMAL,
SOURCE_PLANE_UP,
SOURCE_PLANE_OFFSET)
space = SearchSpace(MIC_LOC, CAMERA_LOC, [source_plane], MIC_FORWARD, MIC_ABOVE)
# Setup camera
camera = SonyCamera(URL, CAM_FORWARD, CAM_ABOVE)
prev_direc = np.array([1., 0., 0.])
if DO_TRACK:
camera.face_direction(prev_direc) # Will force login
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
listener = CommandListener()
plot_manager = PlotManager('3d_vm_srp_')
localizer = VonMisesTrackingLocalizer(mic_positions=mic_layout,
search_space=space,
n_particles=N_PARTICLES,
state_kappa=STATE_KAPPA,
#observation_kappa=OBS_KAPPA,
observation_kappa=5,
outlier_prob=.5,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
localizer2 = VonMisesTrackingLocalizer(mic_positions=mic_layout,
search_space=space,
n_particles=N_PARTICLES,
state_kappa=STATE_KAPPA,
#observation_kappa=OBS_KAPPA,
observation_kappa=25,
outlier_prob=0,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
localizer3 = VonMisesTrackingLocalizer(mic_positions=mic_layout,
search_space=space,
n_particles=N_PARTICLES,
state_kappa=STATE_KAPPA,
observation_kappa=OBS_KAPPA,
outlier_prob=.6,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
beamformer = BeamFormer(mic_layout, SAMPLE_RATE)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
listener.start_polling()
# Setup directions and alignment matrices
direcs = localizer.get_directions()
align_mats = localizer.get_pos_align_mat()
# Plotting setup
if PLOT_PARTICLES:
ml_color = 'r'
color = 'b';
particle_plot = ParticleHemispherePlot(
N_PARTICLES, color, n_estimates=2, n_past_estimates=100,
plot_lines=[False, True], elev=60, azim=45, estim_colors=[ml_color, color])
#color = 'b'
#particle_plot2 = ParticleHemispherePlot(
# N_PARTICLES, color, n_estimates=2, n_past_estimates=100,
# plot_lines=[False, True], elev=60, azim=45, estim_colors=[ml_color, color])
#color = 'r'
#particle_plot3 = ParticleHemispherePlot(
# N_PARTICLES, color, n_estimates=2, n_past_estimates=100,
# plot_lines=[False, True], elev=60, azim=45, estim_colors=[ml_color, color])
if PLOT_POLAR:
fig = plt.figure()
ax = fig.add_subplot(111, projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
post_plot, = plt. plot(theta, np.ones(theta.shape), 'green')
ax.set_ylim(0, 1)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_CARTES:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plt.show(block=False)
x = localizer.to_spher_grid(direcs[0, :])
y = localizer.to_spher_grid(direcs[1, :])
z = localizer.to_spher_grid(direcs[2, :])
#scat = ax.scatter(x, y, z, s=100)
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
estimate = np.array([1., 0., 0.])
estimate2 = np.array([1., 0., 0.])
try:
while in_stream.is_active() or out_stream.is_active():
done = listener.quit()
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
if switch_beamforming:
DO_BEAMFORM = not DO_BEAMFORM
switch_beamforming = False
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
rffts = mat.to_all_real_matlab_format(dfts)
d, energy = localizer.get_distribution_real(rffts[:, :, 0], 'gcc') # Use first hop
# Find ml_est
ml_est = direcs[:, np.argmax(d)]
#print energy
#if energy < 1500:
# continue
post = localizer.get_distribution(rffts[:, :, 0]) # PyBayes EmpPdf
post2 = localizer2.get_distribution(rffts[:, :, 0])
post3 = localizer3.get_distribution(rffts[:, :, 0])
# Get estimate from particles
w = np.asarray(post.weights)
ps = np.asarray(post.particles)
w2 = np.asarray(post2.weights)
ps2 = np.asarray(post2.particles)
w3 = np.asarray(post3.weights)
ps3 = np.asarray(post3.particles)
#estimate2 = w2.dot(ps2)
if DO_TRACK and count % TRACKING_FREQ == 0:
#v = np.array([1, 0, 1])
v = estimate
direc = space.get_camera_dir(v)
if direc is None or not direc.any():
direc = prev_direc
else:
direc[2] = -.5
prev_direc = direc
# Send camera new direction
camera.face_direction(direc)
# Do beam forming
if DO_BEAMFORM:
align_mat = align_mats[:, :, ind]
filtered = beamformer.filter_real(rffts, align_mat)
mat.set_dfts_real(dfts, filtered, n_channels=2)
# Take care of plotting
if count % 1 == 0:
if PLOT_PARTICLES:
estimate = w.dot(ps)
estimate /= (mat.norm2(estimate) + consts.EPS)
particle_plot.update(ps, w, [ml_est, estimate])
#estimate2 = w2.dot(ps2)
#estimate2 /= (mat.norm2(estimate2) + consts.EPS)
#particle_plot2.update(ps2, w2, [ml_est, estimate2])
#estimate3 = w3.dot(ps3)
#estimate3 /= (mat.norm2(estimate3) + consts.EPS)
#particle_plot3.update(ps3, w3, [ml_est, estimate3])
if listener.savefig():
plot_manager.savefig(particle_plot.get_figure())
#plot_manager.savefig(particle_plot2.get_figure())
#plot_manager.savefig(particle_plot3.get_figure())
if PLOT_CARTES:
ax.cla()
ax.grid(False)
#d = localizer.to_spher_grid(post / (np.max(post) + consts.EPS))
#d = localizer.to_spher_grid(d / (np.max(d) + consts.EPS))
ax.scatter(x, y, z, c=d, s=40)
#ax.plot_surface(x, y, z, rstride=1, cstride=1, facecolor=plt.cm.gist_heat(d))
u = estimate
ax.plot([0, u[0]], [0, u[1]], [0, u[2]], c='black', linewidth=3)
if DO_BEAMFORM:
if np.max(np.abs(response)) > 1:
response /= np.max(np.abs(response))
X = response * x
Y = response * y
Z = response * z
ax.plot_surface(X, Y, Z, rstride=1, cstride=1, color='white')
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(0, 1)
#ax.view_init(90, -90)
fig.canvas.draw()
if PLOT_2D:
# Get unconditional distribution
dist = localizer.to_spher_grid(d)
dist -= np.min(dist)
dist /= (np.sum(dist) + consts.EPS)
sample_mat[:, :-1] = sample_mat[:, 1:]
sample_mat[:, -1] = dist
# Get kalman estimate
maxind = np.argmax(post)
estimate_mat[:-1] = estimate_mat[1:]
estimate_mat[-1] = maxind
plot_2d.set_array(sample_mat)
state_est_plot.set_ydata(estimate_mat)
plt.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO or RECORD_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data, NUM_CHANNELS_IN, NUM_CHANNELS_OUT)
# Write out the new, altered data
if PLAY_AUDIO:
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
if RECORD_AUDIO:
if record_buf.get_available_write() >= WINDOW_LENGTH:
record_buf.write_samples(new_data)
except KeyboardInterrupt:
print "Program interrupted"
listener.set_quit(True)
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
# Take care of output file
if RECORD_AUDIO:
print "Writing output file"
make_wav()
print "Done"
def localize():
# Global variables that may be set in this function
global DO_BEAMFORM
global done
# Setup search space
source_plane = OrientedSourcePlane(SOURCE_PLANE_NORMAL, SOURCE_PLANE_UP,
SOURCE_PLANE_OFFSET)
space = SearchSpace(MIC_LOC, CAMERA_LOC, [source_plane], MIC_FORWARD,
MIC_ABOVE)
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
listener = CommandListener()
plot_manager = PlotManager('vmpf_2d_weightings_')
localizer = SRPPFTrackingLocalizer(mic_positions=mic_layout,
search_space=space,
n_particles=N_PARTICLES,
state_kappa=STATE_KAPPA,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
beamformer = BeamFormer(mic_layout, SAMPLE_RATE)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
listener.start_polling()
# Setup directions and alignment matrices
direcs = localizer.get_directions()
align_mats = localizer.get_pos_align_mat()
# Plotting setup
if PLOT_PARTICLES:
ptools.setup_halfpage_figsize()
fig = plt.figure()
ax = ptools.get_halfpage_axis(fig)
#ax = fig.add_subplot(111)
#particle_plots, estimate_plot = setup_particle_plot(ax, 'b', 'r', .4)
particle_plots2, estimate_plot2 = setup_particle_plot(ax, 'k', 'r', .3)
#particle_plots3, estimate_plot3 = setup_particle_plot(ax, 'g', 'r', .8)
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
lhood_plot, = ax.plot(theta, np.ones(theta.shape), 'b')
ax.set_ylim(0, 1.2)
plt.show(block=False)
if PLOT_POLAR:
fig = plt.figure()
ax = fig.add_subplot(111, projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
post_plot, = plt.plot(theta, np.ones(theta.shape), 'green')
ax.set_ylim(0, 1)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_CARTES:
fig = plt.figure()
ax = fig.add_subplot(111)
ax.set_ylim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
post_plot, = plt.plot(theta, np.ones(theta.shape), 'green')
ax.set_ylim(0, 1)
ax.set_xlim(0, np.pi)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_2D:
n_past_samples = 100
noise_color = 'r'
color = 'b'
particle_plot = ParticleFilterPlot(
N_PARTICLES,
n_space=N_THETA,
n_past_samples=n_past_samples,
n_estimates=2,
particle_color=color,
distr_cmap='bone',
estimate_colors=[noise_color, color])
if VIDEO_OVERLAY:
fig = plt.figure()
ax = fig.add_subplot(111)
vc = cv2.VideoCapture(0)
video_handle, vid_part_plots, vid_estim_plot = setup_video_handle(
ax, 720, 1280)
plt.show(block=False)
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
try:
while in_stream.is_active() or out_stream.is_active():
done = listener.quit()
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
if listener.switch_beamforming():
DO_BEAMFORM = not DO_BEAMFORM
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
rffts = mat.to_all_real_matlab_format(dfts)
d, energy = localizer.get_distribution_real(
rffts[:, :, 0], 'gcc') # Use first hop
post = localizer.get_distribution(rffts[:, :, 0])
w = np.asarray(post.weights)
ps = np.asarray(post.particles)
ps[:, 1] = np.abs(ps[:, 1]) # Ensure remain positive
estimate = w.dot(ps)
#if energy < 1000:
# continue
# Do beam forming
if DO_BEAMFORM:
align_mat = align_mats[:, :, ind]
filtered = beamformer.filter_real(rffts, align_mat)
mat.set_dfts_real(dfts, filtered, n_channels=2)
# Take care of plotting
if count % 1 == 0:
if PLOT_PARTICLES:
#plot_particles(particle_plots, estimate_plot, ps, w, estimate)
plot_particles(particle_plots2, estimate_plot2, ps2,
w2, estimate2)
#plot_particles(particle_plots3, estimate_plot3, ps3, w3, estimate3)
dist = localizer.to_spher_grid(d)
dist /= (np.max(dist) + consts.EPS)
lhood_plot.set_ydata(dist)
plt.draw()
if listener.savefig():
plot_manager.savefig(fig)
if PLOT_POLAR or PLOT_CARTES:
dist = d
#dist -= np.min(dist)
dist = localizer.to_spher_grid(dist)
post = localizer.to_spher_grid(post) * 50
#dist /= np.max(dist)
if np.max(dist) > 1:
dist /= np.max(dist)
if np.max(post) > 1:
post /= np.max(post)
pol_plot.set_ydata(dist[0, :])
post_plot.set_ydata(post[0, :])
if DO_BEAMFORM:
# Get beam plot
freq = 1900. # Hz
response = beamformer.get_beam(
align_mat, align_mats, rffts, freq)
response = localizer.to_spher_grid(response)
if np.max(response) > 1:
response /= np.max(response)
pol_beam_plot.set_ydata(response[-1, :])
plt.draw()
if PLOT_2D:
dist = localizer.to_spher_grid(d)
noisy = THETA_SPACE[np.argmax(dist)]
theta_parts = np.arctan2(ps[:, 1], ps[:, 0])
estimate = w.dot(ps)
estimate = np.arctan2(estimate[1], estimate[0])
particle_plot.update(dist, theta_parts, w,
[noisy, estimate])
if listener.savefig():
plot_manager.savefig(particle_plot.get_figure())
if VIDEO_OVERLAY:
_, cvimage = vc.read()
overlay_particles(video_handle, vid_part_plots, vid_estim_plot, \
cvimage, ps, w, estimate)
plt.draw()
if SAVE_FRAMES:
fig.canvas.print_rgba('out/out' + str(count) + '.mat')
count += 1
# Get the istft of the processed data
if PLAY_AUDIO or RECORD_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data,
NUM_CHANNELS_IN,
NUM_CHANNELS_OUT)
# Write out the new, altered data
if PLAY_AUDIO:
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
if RECORD_AUDIO:
if record_buf.get_available_write() >= WINDOW_LENGTH:
record_buf.write_samples(new_data)
except KeyboardInterrupt:
print "Program interrupted"
listener.set_quit(True)
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
# Take care of output file
if RECORD_AUDIO:
print "Writing output file"
make_wav()
print "Done"
def localize():
# Global variables that may be set in this function
global switch_beamforming
global DO_BEAMFORM
global done
# Setup search space
source_plane = OrientedSourcePlane(SOURCE_PLANE_NORMAL,
SOURCE_PLANE_UP,
SOURCE_PLANE_OFFSET)
space = SearchSpace(MIC_LOC, CAMERA_LOC, [source_plane], MIC_FORWARD, MIC_ABOVE)
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
listener = CommandListener()
plot_manager = PlotManager()
#localizer = GridTrackingLocalizer(mic_positions=mic_layout,
# search_space=space,
# source_cov=SOURCE_LOCATION_COV,
# dft_len=FFT_LENGTH,
# sample_rate=SAMPLE_RATE,
# n_theta=N_THETA,
# n_phi=N_PHI)
localizer = KalmanTrackingLocalizer(mic_positions=mic_layout,
search_space=space,
mic_forward=MIC_FORWARD,
mic_above=MIC_ABOVE,
trans_mat=STATE_TRANSITION_MAT,
state_cov=STATE_TRANSITION_MAT,
emission_mat=EMISSION_MAT,
emission_cov=EMISSION_COV,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
beamformer = BeamFormer(mic_layout, SAMPLE_RATE)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
listener.start_polling()
# Setup directions and alignment matrices
direcs = localizer.get_directions()
align_mats = localizer.get_pos_align_mat()
# Plotting setup
if PLOT_POLAR:
fig = plt.figure()
ax = fig.add_subplot(111, projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.zeros(theta.shape))
post_plot, = plt. plot(theta, np.zeros(theta.shape), 'green')
ax.set_ylim(0, 1)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_CARTES:
fig = plt.figure()
ax = plotting.get_halfpage_axis(fig)
#ax = fig.add_subplot(111)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
theta = np.linspace(0, 1, theta.shape[0])
gcc_plots = []
gcc_shaping_vals = [1, 2, 3, 4, 5]
for i in gcc_shaping_vals:
plot, = plt.plot(theta, np.zeros(theta.shape))
gcc_plots.append(plot)
pol_plot, = plt.plot(theta, np.zeros(theta.shape), 'r--')
post_plot, = plt. plot(theta, np.zeros(theta.shape), 'b')
ax.set_ylim(0, 1.2)
ax.set_xlim(0, 1) # Normalized
#ax.set_xlabel('Angle $\left(\\frac{1}{\pi}\\right)$')
#ax.set_ylabel('Normalized GCC')
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_2D:
n_past_samples = 200
filter_plot = FilterPlot(N_THETA, n_past_samples, 2)
save_plot = filter_plot # For saving figures
if VIDEO_OVERLAY:
vc = cv2.VideoCapture(0)
video_handle, video_plot = setup_video_handle(720, 1280)
plt.show(block=False)
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
try:
while in_stream.is_active() or out_stream.is_active():
done = listener.quit()
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
if listener.switch_beamforming():
DO_BEAMFORM = not DO_BEAMFORM
if listener.savefig():
plot_manager.savefig(save_plot.get_figure())
# Get data from circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
rffts = mat.to_all_real_matlab_format(dfts)
gccs = []
#for k in gcc_shaping_vals:
# d, energy = localizer.get_distribution_real(
# rffts[:, :, 0], 'mcc', k) # Use first hop
# gccs.append(d)
d, energy = localizer.get_distribution_real(rffts[:, :, 0], 'beam') # Use first hop
def w(cpmat):
cpmat /= (np.abs(cpmat + consts.EPS))
return cpmat
post = localizer.get_distribution(rffts[:, :, 0], 'beam')
#post, bla = localizer.get_distribution_real(rffts[:, :, 0], 'mcc')
#post = localizer.get_distribution(rffts[:, :, 0])
ind = np.argmax(d)
u = 1.5 * direcs[:, ind] # Direction of arrival
#if energy < 500:
#continue
# Do beam forming
if DO_BEAMFORM:
align_mat = align_mats[:, :, ind]
filtered = beamformer.filter_real(rffts, align_mat)
mat.set_dfts_real(dfts, filtered, n_channels=2)
# Take care of plotting
if count % 1 == 0:
if PLOT_POLAR or PLOT_CARTES:
dist = d
#dist -= np.min(dist)
dist = localizer.to_spher_grid(dist)
print post.shape
post = localizer.to_spher_grid(post) * 50
dist /= np.max(dist)
if np.max(dist) > 1:
dist /= np.max(dist)
if np.max(post) > 1:
post /= np.max(post)
pol_plot.set_ydata(dist[0, :])
post_plot.set_ydata(post[0, :])
#for i, plot in enumerate(gcc_plots):
# gcc = gccs[i]
# gcc /= (np.max(gcc) + consts.EPS)
# plot.set_ydata(gccs[i])
if DO_BEAMFORM:
# Get beam plot
freq = 2500. # Hz
response = beamformer.get_beam(
align_mat, align_mats, rffts, freq
)
response = localizer.to_spher_grid(response)
if np.max(response) > 1:
response /= np.max(response)
pol_beam_plot.set_ydata(response[-1, :])
plt.draw()
if PLOT_2D:
dist = localizer.to_spher_grid(d)
p = localizer.to_spher_grid(post)
est1 = THETA_SPACE[np.argmax(p)]
est2 = THETA_SPACE[np.argmax(dist)]
filter_plot.update(dist, [est1, est2])
if VIDEO_OVERLAY:
post /= np.max(post + consts.EPS)
dist = d - np.min(d)
dist = dist / np.max(dist + consts.EPS)
_, cvimage = vc.read()
overlay_distribution(video_handle, video_plot, cvimage, dist[::-1])
plt.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO or RECORD_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data, NUM_CHANNELS_IN, NUM_CHANNELS_OUT)
# Write out the new, altered data
if PLAY_AUDIO:
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
if RECORD_AUDIO:
if record_buf.get_available_write() >= WINDOW_LENGTH:
record_buf.write_samples(new_data)
except KeyboardInterrupt:
print "Program interrupted"
listener.set_quit(True)
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
# Take care of output file
if RECORD_AUDIO:
print "Writing output file"
make_wav()
print "Done"
class StftManagerTest(unittest.TestCase):
"""
Tester for StftManager class
"""
def setUp(self):
self.data_type = np.float32
self.window_len = 16
self.dft_len = 16
self.hop_len = self.window_len # No overlap
self.use_window = False # No hann
self.n_channels = 1
self.stft_dft = StftManager(
dft_length=self.dft_len,
window_length=self.window_len,
hop_length=self.hop_len,
use_window_fcn=self.use_window,
n_channels=self.n_channels,
dtype=self.data_type,
)
print "Beginning"
def testConstructor(self):
StftManager()
def testData(self):
data = np.array([1, 0, 1, 0], dtype=np.float32)
self.stft_dft.performStft(data)
def testOutData(self):
data = np.array(np.ones(self.window_len), dtype=np.float32)
self.stft_dft.performStft(data)
data1 = np.array(np.zeros(self.window_len), dtype=np.float32)
data1 = self.stft_dft.performIStft()
# self.stft.performIStft(data1)
for i in range(len(data)):
print i
print "data: " + str(data[i])
print "data1:" + str(data1[i])
self.assertEquals(data[i], data1[i])
def testInvalidDftLength(self):
caught = False
try:
pa_tools = StftManager(dft_length=-1)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testInvalidWindowLength(self):
caught = False
try:
pa_tools = StftManager(window_length=-1)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testInvalidNChannels(self):
caught = False
try:
pa_tools = StftManager(n_channels=-1)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testInvalidHopLength(self):
caught = False
try:
pa_tools = StftManager(hop_length=-1)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testHopGreaterThanWindow(self):
caught = False
try:
pa_tools = StftManager(hop_length=1024, window_length=512)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testGetDFT(self):
dfts = self.stft_dft.getDFTs()
(reals, imags) = dfts[0]
print reals
print imags
class MatToolsTest(unittest.TestCase):
def setUp(self):
self.dft_len = 8
self.stft = StftManager(dft_length=self.dft_len,
window_length=self.dft_len,
hop_length=self.dft_len,
use_window_fcn=False)
def testZipFFT(self):
reals = [4, 0, 1, 0]
imags = [2, 1, 0, 0]
zipped = mat.zip_fft(reals, imags)
self.assertListFloatEqual(zipped, [4, 0, 1, 1, 0, 0, 0, 2])
def testToNumpyFormat(self):
dfts = []
dfts.append(([[4, 0, 1, 0]], [[2, 1, 0, 0]]))
dfts.append(([[1, 0, 0, 1]], [[0, 1, 1, 0]]))
arr = mat.to_numpy_format(dfts)
self.assertListFloatEqual(arr[:, 0], [4, 0, 1, 1, 0, 0, 0, 2])
self.assertListFloatEqual(arr[:, 1], [1, 0, 1, 0, 1, 1, 0, 0])
def testFFTComp(self):
data = np.array([1, 0, 0, 1, 1, 0, 0, 1], dtype=np.float32)
self.stft.performStft(data)
dfts = self.stft.getDFTs()
transformed = mat.to_numpy_format(dfts)
ifftout = fftp.irfft(transformed[:, 0] / 2)
print ifftout
self.assertListFloatEqual(data, ifftout)
def testFullFft(self):
data = np.array([1, 2, 1, 3, 1, 4, 1, 2], dtype=np.float32)
dft = np.array([15, -2.1213 + 0.7071j, -1j, 2.1213 + 0.7071j,
-7, 2.1213 - 0.7071j, 1j, -2.1213 - 0.7071j])
self.stft.performStft(data)
dfts = self.stft.getDFTs()
reals = dfts[0][0][0]
imags = dfts[0][1][0]
full_fft = mat.to_full_fft(reals, imags)
print full_fft / 2
self.assertListFloatEqual(dft, full_fft / 2)
def testRIFFT(self):
data = np.array([1, 2, 1, 3, 1, 4, 1, 2], dtype=np.float32)
dft = np.array([15, -2.1213 + 0.7071j, -1j, 2.1213 + 0.7071j, -7])
inver = np.fft.irfft(dft)
self.assertListFloatEqual(data, inver)
def testFullFfft2(self):
reals = [4, 0, 1, 0]
imags = [1, 2, 0, 0]
full_fft = mat.to_full_fft(reals, imags)
self.assertListFloatEqual(
np.array([4, 2j, 1, 0, 1, 0, 1, -2j]), full_fft)
def testRealFFT(self):
reals = [4, 0, 1, 0]
imags = [1, 2, 0, 0]
half_fft = mat.to_real_fft(reals, imags)
self.assertListFloatEqual(np.array([4, 2j, 1, 0, 1]), half_fft)
def testToMatlab(self):
dfts = []
dfts.append(([[4, 0, 1, 0]], [[2, 1, 0, 0]]))
dfts.append(([[1, 0, 0, 1]], [[0, 1, 1, 0]]))
dft_arr = mat.to_matlab_format(dfts)
chan1 = np.array([4, 1j, 1, 0, 2, 0, 1, -1j], dtype=np.complex64)
chan2 = np.array([1, 1j, 1j, 1, 0, 1, -1j, -1j], dtype=np.complex64)
print dft_arr
self.assertListFloatEqual(chan1, dft_arr[0, :])
self.assertListFloatEqual(chan2, dft_arr[1, :])
def testToRealMatlab(self):
dfts = []
dfts.append(([[4, 0, 1, 0]], [[2, 1, 0, 0]]))
dfts.append(([[1, 0, 0, 1]], [[0, 1, 1, 0]]))
dft_arr = mat.to_real_matlab_format(dfts)
chan1 = np.array([4, 1j, 1, 0, 2], dtype=np.complex64)
chan2 = np.array([1, 1j, 1j, 1, 0], dtype=np.complex64)
print dft_arr
self.assertListFloatEqual(chan1, dft_arr[0, :])
self.assertListFloatEqual(chan2, dft_arr[1, :])
def testAllToRealMatlab(self):
dfts = []
dfts.append(([[4, 0, 1, 0], [1, 0, 0, 1]], [[2, 1, 0, 0], [0, 1, 1, 0]]))
dfts.append(([[1, 0, 0, 1], [4, 0, 1, 0]], [[0, 1, 1, 0], [2, 1, 0, 0]]))
dft_arr = mat.to_all_real_matlab_format(dfts)
chan1 = np.array([4, 1j, 1, 0, 2], dtype=np.complex64)
chan2 = np.array([1, 1j, 1j, 1, 0], dtype=np.complex64)
print dft_arr
self.assertListFloatEqual(chan1, dft_arr[0, :, 0])
self.assertListFloatEqual(chan2, dft_arr[0, :, 1])
self.assertListFloatEqual(chan2, dft_arr[1, :, 0])
self.assertListFloatEqual(chan1, dft_arr[1, :, 1])
def testNormalizeRows(self):
data = np.array([[1, 0, 3],
[0, 0, 0],
[2, 2, 4]], dtype=np.float32)
a = mat.normalize_rows(data.copy())
correct = np.array([[.25, 0., 0.75],
[0., 0., 0.],
[0.25, 0.25, 0.5]])
self.assertEquals((a - correct).any(), False)
# Test normalization of only submatrix
mat.normalize_rows(data[1:, :])
self.assertEquals((data[1:, :] - correct[1:, :]).any(), False)
self.assertEquals((data[0] - np.array([1, 0, 3])).any(), False)
def testSetDftReal(self):
dfts = []
list1 = list(np.array([4, 0, 1, 0], dtype=np.float32))
dfts.append(([list1], [[2., 1, 0, 0]]))
dfts.append(([[1., 0, 0, 1]], [[0., 1, 1, 0]]))
rfft = np.array([1, 2j, 3, 4j, 5], dtype=np.complex64)
mat.set_dft_real(dfts[0][0][0], dfts[0][1][0], rfft)
mat.set_dft_real(dfts[1][0][0], dfts[1][1][0], rfft)
print dfts[0][0][0]
self.assertListFloatEqual(dfts[0][0][0], [1, 0, 3, 0])
self.assertListFloatEqual(dfts[0][1][0], [5, 2, 0, 4])
self.assertListFloatEqual(dfts[1][0][0], [1, 0, 3, 0])
self.assertListFloatEqual(dfts[1][1][0], [5, 2, 0, 4])
def testSetDftsReal(self):
dfts = []
dfts.append(([[4, 0, 1, 0], [1, 0, 0, 1]], [[2, 1, 0, 0], [0, 1, 1, 0]]))
dfts.append(([[1, 0, 0, 1], [4, 0, 1, 0]], [[0, 1, 1, 0], [2, 1, 0, 0]]))
rffts = np.array([[1, 2j, 3, 4j, 5], [10, 20j, 30, 40j, 50]], dtype=np.complex64)
mat.set_dfts_real(dfts, rffts)
for n in range(2):
reals = dfts[n][0]
imags = dfts[n][1]
self.assertListEqual(reals[0], [1, 0, 3, 0])
self.assertListEqual(imags[0], [5, 2, 0, 4])
self.assertListEqual(reals[1], [10, 0, 30, 0])
self.assertListEqual(imags[1], [50, 20, 0, 40])
def testSetDftsReal1Chan(self):
dfts = []
dfts.append(([[4, 0, 1, 0], [1, 0, 0, 1]], [[2, 1, 0, 0], [0, 1, 1, 0]]))
dfts.append(([[1, 0, 0, 1], [4, 0, 1, 0]], [[0, 1, 1, 0], [2, 1, 0, 0]]))
rffts = np.array([[1, 2j, 3, 4j, 5], [10, 20j, 30, 40j, 50]], dtype=np.complex64)
mat.set_dfts_real(dfts, rffts, n_channels=1)
reals = dfts[0][0]
imags = dfts[0][1]
self.assertListEqual(reals[0], [1, 0, 3, 0])
self.assertListEqual(imags[0], [5, 2, 0, 4])
self.assertListEqual(reals[1], [10, 0, 30, 0])
self.assertListEqual(imags[1], [50, 20, 0, 40])
reals = dfts[1][0]
imags = dfts[1][1]
self.assertListEqual(reals[0], [1, 0, 0, 1])
self.assertListEqual(imags[0], [0, 1, 1, 0])
self.assertListEqual(reals[1], [4, 0, 1, 0])
self.assertListEqual(imags[1], [2, 1, 0, 0])
def testCheckVecEmpty(self):
vec = np.array([])
mat.check_vec(vec)
vec = np.array([[]])
self.assertRaises(ValueError, mat.check_vec, vec)
def testCheckVecMat(self):
vec = np.array([[1]])
self.assertRaises(ValueError, mat.check_vec, vec)
vec = np.array([[2, 4, 5], [2, 3, 3]])
self.assertRaises(ValueError, mat.check_vec, vec)
def testCheck3dVecMat(self):
vec = np.array([[1, 1, 1]])
self.assertRaises(ValueError, mat.check_3d_vec, vec)
def testCheck3dVec2(self):
vec = np.array([1, 1])
self.assertRaises(ValueError, mat.check_3d_vec, vec)
def testToFloat(self):
vec = np.array([1, 1], dtype=np.int32)
vec = mat.to_float(vec)
self.assertEquals(vec.dtype, np.float)
def assertListFloatEqual(self, list1, list2):
if not len(list1) == len(list2):
raise AssertionError("Lists differ in lenght. Cannot be equal")
for i in range(len(list1)):
try:
self.assertLessEqual(abs(list1[i] - list2[i]), 1e-4)
except AssertionError:
err_str = "Lists differ on element " + str(i) + ": " + \
str(list1[i]) + " vs. " + str(list2[i])
raise AssertionError(err_str)
def tearDown(self):
pass
def localize():
global switch_beamforming
global DO_BEAMFORM
# Setup search space
source_plane = OrientedSourcePlane(SOURCE_PLANE_NORMAL,
SOURCE_PLANE_UP,
SOURCE_PLANE_OFFSET)
space = SearchSpace(MIC_LOC, CAMERA_LOC, [source_plane], MIC_FORWARD, MIC_ABOVE)
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
localizer = GridTrackingLocalizer(mic_positions=mic_layout,
search_space=space,
source_cov=SOURCE_LOCATION_COV,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
beamformer = BeamFormer(mic_layout, SAMPLE_RATE)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
quit_thread = threading.Thread(target=check_for_quit)
quit_thread.start()
# Setup directions and alignment matrices
direcs = localizer.get_directions()
align_mats = localizer.get_pos_align_mat()
# Plotting setup
if PLOT_POLAR:
fig = plt.figure(facecolor='white')
ax = fig.add_subplot(111, projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
#post_plot, = plt. plot(theta, np.ones(theta.shape), 'green')
ax.set_ylim(0, 1)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_CARTES:
fig = plt.figure(facecolor='white')
ax = fig.add_subplot(111)
ax.set_ylim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
#post_plot, = plt. plot(theta, np.ones(theta.shape), 'green')
ax.set_ylim(0, 1)
ax.set_xlim(0, np.pi)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
try:
global done
while in_stream.is_active() or out_stream.is_active():
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
if switch_beamforming:
DO_BEAMFORM = not DO_BEAMFORM
switch_beamforming = False
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
rffts = mat.to_all_real_matlab_format(dfts)
d, energy = localizer.get_distribution_real(rffts[:, :, 0], 'gcc') # Use first hop
post = localizer.get_distribution(rffts[:, :, 0])
ind = np.argmax(d)
u = 1.5 * direcs[:, ind] # Direction of arrival
# Do beam forming
if DO_BEAMFORM:
align_mat = align_mats[:, :, ind]
filtered = beamformer.filter_real(rffts, align_mat)
mat.set_dfts_real(dfts, filtered, n_channels=2)
# Take care of plotting
if count % 1 == 0:
if PLOT_POLAR or PLOT_CARTES:
#d -= np.min(d)
d = localizer.to_spher_grid(d)
post = localizer.to_spher_grid(post) * 50
#d /= np.max(d)
if np.max(d) > 1:
d /= np.max(d)
if np.max(post) > 1:
post /= np.max(post)
pol_plot.set_ydata(d[0, :])
#post_plot.set_ydata(post[0, :])
if DO_BEAMFORM:
# Get beam plot
freq = 1900. # Hz
response = beamformer.get_beam(align_mat, align_mats, rffts, freq)
response = localizer.to_spher_grid(response)
if np.max(response) > 1:
response /= np.max(response)
pol_beam_plot.set_ydata(response[-1, :])
plt.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO or RECORD_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data, NUM_CHANNELS_IN, NUM_CHANNELS_OUT)
# Write out the new, altered data
if PLAY_AUDIO:
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
if RECORD_AUDIO:
if record_buf.get_available_write() >= WINDOW_LENGTH:
record_buf.write_samples(new_data)
except KeyboardInterrupt:
print "Program interrupted"
done = True
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
# Take care of output file
if RECORD_AUDIO:
print "Writing output file"
make_wav()
print "Done"
def setUp(self):
self.dft_len = 8
self.stft = StftManager(dft_length=self.dft_len,
window_length=self.dft_len,
hop_length=self.dft_len,
use_window_fcn=False)
class StftManagerTest(unittest.TestCase):
"""
Tester for StftManager class
"""
def setUp(self):
self.data_type = np.float32
self.window_len = 16
self.dft_len = 16
self.hop_len = self.window_len # No overlap
self.use_window = False # No hann
self.n_channels = 1
self.stft_dft = StftManager(dft_length=self.dft_len,
window_length=self.window_len,
hop_length=self.hop_len,
use_window_fcn=self.use_window,
n_channels=self.n_channels,
dtype=self.data_type)
print "Beginning"
def testConstructor(self):
StftManager()
def testData(self):
data = np.array([1, 0, 1, 0], dtype=np.float32)
self.stft_dft.performStft(data)
def testOutData(self):
data = np.array(np.ones(self.window_len), dtype=np.float32)
self.stft_dft.performStft(data)
data1 = np.array(np.zeros(self.window_len), dtype=np.float32)
data1 = self.stft_dft.performIStft()
#self.stft.performIStft(data1)
for i in range(len(data)):
print i
print "data: " + str(data[i])
print "data1:" + str(data1[i])
self.assertEquals(data[i], data1[i])
def testInvalidDftLength(self):
caught = False
try:
pa_tools = StftManager(dft_length=-1)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testInvalidWindowLength(self):
caught = False
try:
pa_tools = StftManager(window_length=-1)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testInvalidNChannels(self):
caught = False
try:
pa_tools = StftManager(n_channels=-1)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testInvalidHopLength(self):
caught = False
try:
pa_tools = StftManager(hop_length=-1)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testHopGreaterThanWindow(self):
caught = False
try:
pa_tools = StftManager(hop_length=1024, window_length=512)
except ValueError:
caught = True
self.assertEquals(caught, True)
def testGetDFT(self):
dfts = self.stft_dft.getDFTs()
(reals, imags) = dfts[0]
print reals
print imags
def localize():
# Global variables that may be set in this function
global DO_BEAMFORM
global done
# Setup search space
source_plane = OrientedSourcePlane(SOURCE_PLANE_NORMAL,
SOURCE_PLANE_UP,
SOURCE_PLANE_OFFSET)
space = SearchSpace(MIC_LOC, CAMERA_LOC, [source_plane], MIC_FORWARD, MIC_ABOVE)
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
listener = CommandListener()
plot_manager = PlotManager('vmpf_2d_weightings_')
localizer = SRPPFTrackingLocalizer(mic_positions=mic_layout,
search_space=space,
n_particles=N_PARTICLES,
state_kappa=STATE_KAPPA,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
beamformer = BeamFormer(mic_layout, SAMPLE_RATE)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
listener.start_polling()
# Setup directions and alignment matrices
direcs = localizer.get_directions()
align_mats = localizer.get_pos_align_mat()
# Plotting setup
if PLOT_PARTICLES:
ptools.setup_halfpage_figsize()
fig = plt.figure()
ax = ptools.get_halfpage_axis(fig)
#ax = fig.add_subplot(111)
#particle_plots, estimate_plot = setup_particle_plot(ax, 'b', 'r', .4)
particle_plots2, estimate_plot2 = setup_particle_plot(ax, 'k', 'r', .3)
#particle_plots3, estimate_plot3 = setup_particle_plot(ax, 'g', 'r', .8)
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
lhood_plot, = ax.plot(theta, np.ones(theta.shape), 'b')
ax.set_ylim(0, 1.2)
plt.show(block=False)
if PLOT_POLAR:
fig = plt.figure()
ax = fig.add_subplot(111, projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
post_plot, = plt.plot(theta, np.ones(theta.shape), 'green')
ax.set_ylim(0, 1)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_CARTES:
fig = plt.figure()
ax = fig.add_subplot(111)
ax.set_ylim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
theta = spher_coords[1, :]
pol_plot, = plt.plot(theta, np.ones(theta.shape))
post_plot, = plt.plot(theta, np.ones(theta.shape), 'green')
ax.set_ylim(0, 1)
ax.set_xlim(0, np.pi)
if DO_BEAMFORM:
pol_beam_plot, = plt.plot(theta, np.ones(theta.shape), 'red')
if PLOT_2D:
n_past_samples = 100
noise_color = 'r'
color = 'b'
particle_plot = ParticleFilterPlot(N_PARTICLES,
n_space=N_THETA, n_past_samples=n_past_samples,
n_estimates=2, particle_color=color, distr_cmap='bone',
estimate_colors=[noise_color, color])
if VIDEO_OVERLAY:
fig = plt.figure()
ax = fig.add_subplot(111)
vc = cv2.VideoCapture(0)
video_handle, vid_part_plots, vid_estim_plot = setup_video_handle(ax, 720, 1280)
plt.show(block=False)
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
try:
while in_stream.is_active() or out_stream.is_active():
done = listener.quit()
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
if listener.switch_beamforming():
DO_BEAMFORM = not DO_BEAMFORM
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
rffts = mat.to_all_real_matlab_format(dfts)
d, energy = localizer.get_distribution_real(rffts[:, :, 0], 'gcc') # Use first hop
post = localizer.get_distribution(rffts[:, :, 0])
w = np.asarray(post.weights)
ps = np.asarray(post.particles)
ps[:, 1] = np.abs(ps[:, 1]) # Ensure remain positive
estimate = w.dot(ps)
#if energy < 1000:
# continue
# Do beam forming
if DO_BEAMFORM:
align_mat = align_mats[:, :, ind]
filtered = beamformer.filter_real(rffts, align_mat)
mat.set_dfts_real(dfts, filtered, n_channels=2)
# Take care of plotting
if count % 1 == 0:
if PLOT_PARTICLES:
#plot_particles(particle_plots, estimate_plot, ps, w, estimate)
plot_particles(particle_plots2, estimate_plot2, ps2, w2, estimate2)
#plot_particles(particle_plots3, estimate_plot3, ps3, w3, estimate3)
dist = localizer.to_spher_grid(d)
dist /= (np.max(dist) + consts.EPS)
lhood_plot.set_ydata(dist)
plt.draw()
if listener.savefig():
plot_manager.savefig(fig)
if PLOT_POLAR or PLOT_CARTES:
dist = d
#dist -= np.min(dist)
dist = localizer.to_spher_grid(dist)
post = localizer.to_spher_grid(post) * 50
#dist /= np.max(dist)
if np.max(dist) > 1:
dist /= np.max(dist)
if np.max(post) > 1:
post /= np.max(post)
pol_plot.set_ydata(dist[0, :])
post_plot.set_ydata(post[0, :])
if DO_BEAMFORM:
# Get beam plot
freq = 1900. # Hz
response = beamformer.get_beam(align_mat, align_mats, rffts, freq)
response = localizer.to_spher_grid(response)
if np.max(response) > 1:
response /= np.max(response)
pol_beam_plot.set_ydata(response[-1, :])
plt.draw()
if PLOT_2D:
dist = localizer.to_spher_grid(d)
noisy = THETA_SPACE[np.argmax(dist)]
theta_parts = np.arctan2(ps[:, 1], ps[:, 0])
estimate = w.dot(ps)
estimate = np.arctan2(estimate[1], estimate[0])
particle_plot.update(dist, theta_parts, w, [noisy, estimate])
if listener.savefig():
plot_manager.savefig(particle_plot.get_figure())
if VIDEO_OVERLAY:
_, cvimage = vc.read()
overlay_particles(video_handle, vid_part_plots, vid_estim_plot, \
cvimage, ps, w, estimate)
plt.draw()
if SAVE_FRAMES:
fig.canvas.print_rgba('out/out' + str(count) + '.mat')
count += 1
# Get the istft of the processed data
if PLAY_AUDIO or RECORD_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data, NUM_CHANNELS_IN, NUM_CHANNELS_OUT)
# Write out the new, altered data
if PLAY_AUDIO:
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
if RECORD_AUDIO:
if record_buf.get_available_write() >= WINDOW_LENGTH:
record_buf.write_samples(new_data)
except KeyboardInterrupt:
print "Program interrupted"
listener.set_quit(True)
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
# Take care of output file
if RECORD_AUDIO:
print "Writing output file"
make_wav()
print "Done"
def localize():
global switch_beamforming
global DO_BEAMFORM
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
localizer = DistributionLocalizer(mic_positions=mic_layout,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
beamformer = BeamFormer(mic_layout, SAMPLE_RATE)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
quit_thread = threading.Thread(target=check_for_quit)
quit_thread.start()
# Setup directions and alignment matrices
direcs = localizer.get_directions()
align_mats = localizer.get_pos_align_mat()
# Plotting setup
if PLOT_CARTES:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plt.show(block=False)
x = localizer.to_spher_grid(direcs[0, :])
y = localizer.to_spher_grid(direcs[1, :])
z = localizer.to_spher_grid(direcs[2, :])
#scat = ax.scatter(x, y, z, s=100)
if PLOT_POLAR:
fig = plt.figure()
ax = fig.add_axes([.1, .1, .8, .8], projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
pol = localizer.to_spher_grid(spher_coords[2, :])
weight = 1. - .3 * np.sin(2 * pol) # Used to pull visualization off edges
r = np.sin(pol) * weight
theta = localizer.to_spher_grid(spher_coords[1, :])
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
try:
global done
while in_stream.is_active() or out_stream.is_active():
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
if switch_beamforming:
DO_BEAMFORM = not DO_BEAMFORM
switch_beamforming = False
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
rffts = mat.to_all_real_matlab_format(dfts)
d, energy = localizer.get_distribution_real(rffts[:, :, 0])
ind = np.argmax(d)
u = 1.5 * direcs[:, ind] # Direction of arrival
# Do beam forming
if DO_BEAMFORM:
align_mat = align_mats[:, :, ind]
filtered = beamformer.filter_real(rffts, align_mat)
mat.set_dfts_real(dfts, filtered, n_channels=2)
# Get beam plot
freq = 1500. # Hz
response = beamformer.get_beam(align_mat, align_mats, rffts, freq)
response = localizer.to_spher_grid(response)
# Take car of plotting
if count % 1 == 0:
if PLOT_CARTES:
ax.cla()
ax.grid(False)
d = localizer.to_spher_grid(d / (np.max(d) + consts.EPS))
ax.scatter(x, y, z, c=d, s=40)
#ax.plot_surface(x, y, z, rstride=1, cstride=1, facecolor=plt.cm.gist_heat(d))
ax.plot([0, u[0]], [0, u[1]], [0, u[2]], c='black', linewidth=3)
if DO_BEAMFORM:
if np.max(np.abs(response)) > 1:
response /= np.max(np.abs(response))
X = response * x
Y = response * y
Z = response * z
ax.plot_surface(X, Y, Z, rstride=1, cstride=1, color='white')
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(0, 1)
#ax.view_init(90, -90)
fig.canvas.draw()
if PLOT_POLAR:
plt.cla()
d = localizer.to_spher_grid(d)
con = ax.contourf(theta, r, d, vmin=0, vmax=40)
con.set_cmap('gist_heat')
if DO_BEAMFORM:
response = response[-1, :] # Pick which polar angle sample to use
ax.plot(theta[0, :], response, 'cyan', linewidth=4)
ax.set_rlim(0, 1)
fig.canvas.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO or RECORD_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data, NUM_CHANNELS_IN, NUM_CHANNELS_OUT)
# Write out the new, altered data
if PLAY_AUDIO:
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
if RECORD_AUDIO:
if record_buf.get_available_write() >= WINDOW_LENGTH:
record_buf.write_samples(new_data)
except KeyboardInterrupt:
print "Program interrupted"
done = True
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
# Take care of output file
if RECORD_AUDIO:
print "Writing output file"
make_wav()
print "Done"
def localize():
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
localizer = DistributionLocalizer(mic_positions=mic_layout,
dft_len=FFT_LENGTH,
sample_rate=SAMPLE_RATE,
n_theta=N_THETA,
n_phi=N_PHI)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Setup devices
in_device = helper.get_input_device_from_user()
if PLAY_AUDIO:
out_device = helper.get_output_device_from_user()
else:
out_device = helper.get_default_output_device_info()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
out_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_OUT,
format=SAMPLE_TYPE,
output=True,
frames_per_buffer=FRAMES_PER_BUF,
output_device_index=int(out_device['index']),
stream_callback=write_out_data)
# Start recording/playing back
in_stream.start_stream()
out_stream.start_stream()
# Start thread to check for user quit
quit_thread = threading.Thread(target=check_for_quit)
quit_thread.start()
# Plotting setup
if PLOT_CARTES:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plt.show(block=False)
scat = []
if PLOT_POLAR:
fig = plt.figure()
ax = fig.add_axes([.1, .1, .8, .8], projection='polar')
ax.set_rlim(0, 1)
plt.show(block=False)
# Setup space for plotting in new coordinates
spher_coords = localizer.get_spher_directions()
pol = localizer.to_spher_grid(spher_coords[2, :])
weight = 1. - .3 * np.sin(2 * pol) # Used to pull visualization off edges
r = np.sin(pol) * weight
theta = localizer.to_spher_grid(spher_coords[1, :])
if EXTERNAL_PLOT:
fig = plt.figure()
ax = fig.add_subplot(111)
plt.show(block=False)
count = 0
direcs = localizer.get_directions()
try:
global done
while in_stream.is_active() or out_stream.is_active():
data_available = in_buf.wait_for_read(WINDOW_LENGTH, TIMEOUT)
if data_available:
# Get data from the circular buffer
data = in_buf.read_samples(WINDOW_LENGTH)
# Perform an stft
stft.performStft(data)
# Process dfts from windowed segments of input
dfts = stft.getDFTs()
d = localizer.get_3d_real_distribution(dfts)
ind = np.argmax(d)
u = 1.5 * direcs[:, ind] # Direction of arrival
# Take car of plotting
if count % 1 == 0:
if PLOT_CARTES:
plt.cla()
ax.scatter(direcs[0, :], direcs[1, :], direcs[2, :], s=30, c=d[3, :])
ax.plot([0, u[0]], [0, u[1]], [0, u[2]], c='blue')
ax.set_xlim(-1, 1)
ax.set_ylim(-1, 1)
ax.set_zlim(0, 1)
plt.draw()
if PLOT_POLAR:
plt.cla()
d = localizer.to_spher_grid(d)
con = ax.contourf(theta, r, d, vmin=0, vmax=40)
con.set_cmap('gist_heat')
fig.canvas.draw()
count += 1
# Get the istft of the processed data
if PLAY_AUDIO:
new_data = stft.performIStft()
new_data = out_buf.reduce_channels(new_data, NUM_CHANNELS_IN, NUM_CHANNELS_OUT)
# Write out the new, altered data
if out_buf.get_available_write() >= WINDOW_LENGTH:
out_buf.write_samples(new_data)
#time.sleep(.05)
except KeyboardInterrupt:
print "Program interrupted"
done = True
print "Cleaning up"
in_stream.stop_stream()
in_stream.close()
out_stream.stop_stream()
out_stream.close()
pa.terminate()
print "Done"
while True:
read_in = raw_input()
if read_in == "q":
done = True
break
if __name__ == '__main__':
# Setup pyaudio instances
pa = pyaudio.PyAudio()
helper = AudioHelper(pa)
# Setup STFT object
stft = StftManager(dft_length=FFT_LENGTH,
window_length=WINDOW_LENGTH,
hop_length=HOP_LENGTH,
use_window_fcn=True,
n_channels=NUM_CHANNELS_IN,
dtype=DATA_TYPE)
# Get devices
in_device = helper.get_input_device_from_user()
out_device = helper.get_output_device_from_user()
# Setup streams
in_stream = pa.open(rate=SAMPLE_RATE,
channels=NUM_CHANNELS_IN,
format=SAMPLE_TYPE,
frames_per_buffer=FRAMES_PER_BUF,
input=True,
input_device_index=int(in_device['index']),
stream_callback=read_in_data)
