beamforming.py

import acoular
import numpy as np
# from arlpy import bf
from os import path
from scipy.signal import butter, sosfiltfilt, filtfilt
import time
acoular.config.global_caching = 'none' # this is important for fast processing + does not print out [('_cache.h5', id)] !!!

def high_pass_filter_multichannel(data, cutoff_freq, sample_rate):
    """
    Apply an FFT-based high-pass filter with reflection padding to multi-channel data.
    
    Parameters:
    - data: numpy array of shape (n_samples, n_channels)
    - cutoff_freq: cutoff frequency in Hz
    - sample_rate: sampling rate in Hz
    
    Returns:
    - filtered_data: filtered data with the same shape as input data
    """
    n_samples = data.shape[0]
    
    # Choose a padding length. This can be tuned.
    pad_length = n_samples // 2

    # Reflect the signal at the boundaries
    padded_data = np.pad(data, ((pad_length, pad_length), (0, 0)), mode='reflect')

    # FFT on padded signal (along axis=0)
    fft_data = np.fft.fft(padded_data, axis=0)
    
    # Frequency bins for padded signal
    freqs = np.fft.fftfreq(padded_data.shape[0], d=1/sample_rate)
    
    # Create the high-pass mask
    mask = np.abs(freqs) >= cutoff_freq
    fft_data_filtered = fft_data * mask[:, np.newaxis]
    
    # Inverse FFT to obtain filtered padded signal
    filtered_padded = np.fft.ifft(fft_data_filtered, axis=0)
    filtered_padded = np.real(filtered_padded)
    
    # Remove the padding
    filtered_data = filtered_padded[pad_length:pad_length + n_samples, :]
    
    return filtered_data

class Beamforming:
    def __init__(self, block_size=2048, num_channels=16, rate=44100, micgeofile='./mic_models/minidsp_uma-16_mirrored.xml'):    
        # Initialize microphone geometry
        self.block_size = block_size
        self.num_channels = num_channels
        self.RATE = rate
        self.micgeofile = micgeofile
        self.mg = acoular.MicGeom(from_file=micgeofile)

        # Speed of sound
        self.sdspd = 345.25 # m/s
        self.spoffset = self.RATE / self.sdspd # sample offset per meter
        

    def process_audio_block(self, audio_data, src_xyz, fc=6000):
        ts = acoular.TimeSamples(
            # name='realtime_data',
            sample_freq=self.RATE,
            numchannels=self.num_channels,
            numsamples=self.block_size
        )

        ts.data = audio_data

        x,y,z = src_xyz
        size = 0.1 # 0.05
        increment = 0.008 # 0.004
        src_point = acoular.RectGrid(
            x_min=x-size, x_max=x+size,
            y_min=y-size, y_max=y+size, 
            z=z, increment=increment
        )
        # src_point = acoular.RectGrid(
        #     x_min=x, x_max=x,
        #     y_min=y, y_max=y, 
        #     z=z
        # )

        st = acoular.SteeringVector(grid=src_point, mics=self.mg)
        block_size_options = [1024, 128, 256, 512, 2048, 4096, 8192, 16384, 32768, 65536]
        new_block_size = max((x for x in block_size_options if x <= self.block_size//4), default=None)
        
        f = acoular.PowerSpectra(source=ts, block_size=new_block_size, 
                                window='Hanning', overlap="50%")

        bb = acoular.BeamformerFunctional(freq_data=f, steer=st, r_diag=False)
        
        frequencies = f.fftfreq()

        frequencies = frequencies[frequencies >= fc]
        bandwidth = 3
        
        # Initialize combined map
        combined_map = None
        
        for cfreq in [18000]:
            pm = bb.synthetic(cfreq, bandwidth)
            Lm = acoular.L_p(pm)
            
            # For the first frequency, initialize the combined map
            if combined_map is None:
                combined_map = Lm
            else:
                # Average with previous frequencies
                combined_map = (combined_map + Lm) / 2
        Lm = combined_map
        
        self.vmin, self.vmax = Lm.max()-3, Lm.max()
        self.Lm = Lm
        return self.get_normalized_Lm()

    def get_spectrogram(self, bb):
        spectrogram = np.array(bb.result[:])
        # If the result is (freq, grid_points) where grid_points = width*height
        if len(spectrogram.shape) == 2:
            # Get the grid dimensions from the beamformer
            grid = bb.steer.grid
            grid_shape = grid.shape
            
            # Find the center index in the flattened grid
            if hasattr(grid, 'shape'):
                # If grid has a shape attribute with dimensions
                center_x = grid_shape[0] // 2
                center_y = grid_shape[1] // 2
                center_idx = center_y + center_x * grid_shape[1]  # Row-major indexing
                return spectrogram[:, center_idx]
            else:
                # If we can't determine the grid shape, try to find the center point
                num_points = spectrogram.shape[1]
                center_idx = num_points // 2
                return spectrogram[:, center_idx]
        return spectrogram

    def get_normalized_Lm(self):
        Lm_norm = (self.Lm - self.vmin) / (self.vmax - self.vmin)
        return Lm_norm

    def get_vrange(self):
        return self.vmin, self.vmax

    def roll_w_zero_padd(self, data, offset):
        if offset > 0:
            data = np.roll(data, offset)
            data[:offset] = 0
        elif offset < 0:
            data = np.roll(data, offset)
            data[offset:] = 0
        return data

    def get_delay_and_sum_audio(self, signal, src_xyz):
        x, y, z = src_xyz

        # Compute azimuth (phi)
        azimuth = np.arctan2(y, x)  # Shape: (N,)

        # Compute elevation (theta)
        distance = np.sqrt(x**2 + y**2 + z**2)
        elevation = np.arcsin(z / distance)

        theta = np.column_stack((azimuth, elevation))  # Shape: (N, 2)

        steering_vector = bf.steering_plane_wave(self.mg.mpos.T, self.sdspd, theta)
        signal_new_single = bf.delay_and_sum(signal.T, self.RATE, steering_vector)[0]
        signal_new = np.repeat(signal_new_single[:, np.newaxis], self.num_channels, axis=1)
        offset_lst = np.zeros(self.num_channels)

        return signal_new_single, signal_new, offset_lst

    def get_offseted_audio(self, signal, src_xyz):
        # signal = np.array(self.audio_buffer_output)
        signal_new = np.zeros((signal.shape[0]*3,signal.shape[1]))
        signal_new[signal.shape[0]:signal.shape[0]*2,:] = signal
        offset_lst = []
        for ch, mic_pos in enumerate(self.mg.mpos.T):
            # mic_pos[1] = -mic_pos[1]
            dist = np.linalg.norm(mic_pos - src_xyz)
            _signal = signal_new[:,ch]
            offset = -int(dist * self.spoffset)
            offset_lst.append(offset)
            signal_new[:,ch] = self.roll_w_zero_padd(_signal,offset)
        offset_lst = np.array(offset_lst)

        # find the index of first non-zero value
        idx = np.nonzero(np.mean(signal_new,axis=1))[0]
        # find the value in the middle of idx
        signal_new = signal_new[idx]
        # take size of 128 from signal_new in the middle
        signal_new = signal_new[len(signal_new)//2-len(signal)//2:len(signal_new)//2+len(signal)//2]

        signal_new_single = np.mean(signal_new,axis=1)
        signal_new = np.concatenate([signal_new, signal_new_single[:, np.newaxis]], axis=1)

        return signal_new_single, signal_new, offset_lst