Exemplo n.º 1
0
 def testGetCamDir2Plane2(self):
     planes = [self.plane1, self.plane2]
     mic_loc = np.array([0, 0, 0])
     cam_loc = np.array([1, 1, 1])
     space = SearchSpace(mic_loc, cam_loc, planes)
     direction = np.array([-1, 1, 1])
     camdir = space.get_camera_dir(direction)
     self.assertListEqual(list(camdir), list(np.array([-6, 4, 4]) / (68 ** .5)))
Exemplo n.º 2
0
 def testGetSourceLoc1Plane(self):
     planes = [self.plane1]
     mic_loc = np.array([0, 0, 0])
     cam_loc = np.array([1, 1, 1])
     space = SearchSpace(mic_loc, cam_loc, planes)
     direction = np.array([-1, 1, 1])
     loc = space.get_source_loc(direction)
     self.assertListEqual(list(loc), [-5, 5, 5])
Exemplo n.º 3
0
 def testGetCameraDir3Plane(self):
     planes = [self.plane1, self.plane2, self.plane3]
     mic_loc = np.array([0, 0, 0])
     cam_loc = np.array([1, 1, 1])
     space = SearchSpace(mic_loc, cam_loc, planes)
     direction = np.array([-1, 1, 1])
     camdir = space.get_camera_dir(direction)
     self.assertListEqual(list(camdir), [-1, 0, 0])
Exemplo n.º 4
0
 def testGetCameraDir3Plane(self):
     planes = [self.plane1, self.plane2, self.plane3]
     mic_loc = np.array([0, 0, 0])
     cam_loc = np.array([1, 1, 1])
     space = SearchSpace(mic_loc, cam_loc, planes)
     direction = np.array([-1, 1, 1])
     camdir = space.get_camera_dir(direction)
     self.assertListEqual(list(camdir), [-1, 0, 0])
Exemplo n.º 5
0
 def testGetSourceLoc1Plane(self):
     planes = [self.plane1]
     mic_loc = np.array([0, 0, 0])
     cam_loc = np.array([1, 1, 1])
     space = SearchSpace(mic_loc, cam_loc, planes)
     direction = np.array([-1, 1, 1])
     loc = space.get_source_loc(direction)
     self.assertListEqual(list(loc), [-5, 5, 5])
Exemplo n.º 6
0
 def testGetCamDir2Plane2(self):
     planes = [self.plane1, self.plane2]
     mic_loc = np.array([0, 0, 0])
     cam_loc = np.array([1, 1, 1])
     space = SearchSpace(mic_loc, cam_loc, planes)
     direction = np.array([-1, 1, 1])
     camdir = space.get_camera_dir(direction)
     self.assertListEqual(list(camdir),
                          list(np.array([-6, 4, 4]) / (68**.5)))
Exemplo n.º 7
0
 def testGetSourceLoc2PlaneBehind(self):
     normal2 = np.array([0, 0, 1])
     offset2 = np.array([0, 0, -1])
     planes = [self.plane1, SourcePlane(normal2, offset2)]
     mic_loc = np.array([0, 0, 0])
     cam_loc = np.array([1, 1, 1])
     space = SearchSpace(mic_loc, cam_loc, planes)
     direction = np.array([-1, 1, 1])
     loc = space.get_source_loc(direction)
     self.assertListEqual(list(loc), [-5, 5, 5])
Exemplo n.º 8
0
 def testGetSourceLoc2PlaneBehind(self):
     normal2 = np.array([0, 0, 1])
     offset2 = np.array([0, 0, -1])
     planes = [self.plane1, SourcePlane(normal2, offset2)]
     mic_loc = np.array([0, 0, 0])
     cam_loc = np.array([1, 1, 1])
     space = SearchSpace(mic_loc, cam_loc, planes)
     direction = np.array([-1, 1, 1])
     loc = space.get_source_loc(direction)
     self.assertListEqual(list(loc), [-5, 5, 5])
Exemplo n.º 9
0
 def testInit(self):
     normal = np.array([0, 1, 0])
     offset = np.array([0, 5, 0])
     planes = [SourcePlane(offset, normal)]
     mic_loc = np.array([0, 0, 0])
     cam_loc = np.array([1, 1, 1])
     space = SearchSpace(mic_loc, cam_loc, planes)
Exemplo n.º 10
0
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"
Exemplo n.º 11
0
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"
Exemplo n.º 12
0
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"
Exemplo n.º 13
0
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"
Exemplo n.º 14
0
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"
Exemplo n.º 15
0
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"