diff --git a/beamforming/c/CMakeLists.txt b/beamforming/c/CMakeLists.txt index c45610b..17ba622 100644 --- a/beamforming/c/CMakeLists.txt +++ b/beamforming/c/CMakeLists.txt @@ -16,7 +16,7 @@ add_library(beamforming_lib mq.c worker.c dfifo.c - filter.c + filterbank.c ) diff --git a/beamforming/c/ascee_python.h b/beamforming/c/ascee_python.h new file mode 100644 index 0000000..47232cf --- /dev/null +++ b/beamforming/c/ascee_python.h @@ -0,0 +1,60 @@ +// ascee_python.h +// +// Author: J.A. de Jong - ASCEE +// +// Description: +// Some routines to generate numpy arrays from matrices and vectors. +////////////////////////////////////////////////////////////////////// +#pragma once +#ifndef ASCEE_PYTHON_H +#define ASCEE_PYTHON_H +#include +#ifdef ASCEE_DOUBLE_PRECISION +#define ASCEE_NUMPY_FLOAT_TYPE NPY_FLOAT64 +#define ASCEE_NUMPY_COMPLEX_TYPE NPY_COMPLEX128 +#else +#define ASCEE_NUMPY_FLOAT_TYPE NPY_FLOAT32 +#endif + + +static inline PyObject* dmat_to_ndarray(dmat* mat,bool transfer_ownership) { + fsTRACE(15); + dbgassert(mat,NULLPTRDEREF); + + import_array(); + + // Dimensions given in wrong order, as mat is + // Fortran-contiguous. Later on we transpose the result. This is + // more easy than using the PyArray_New syntax. + npy_intp dims[] = {mat->n_cols,mat->n_rows}; + PyObject* arr_t = PyArray_SimpleNewFromData(2,dims, + ASCEE_NUMPY_FLOAT_TYPE, + mat->_data); + if(!arr_t) { + WARN("Array creation failure"); + feTRACE(15); + return NULL; + } + + if(transfer_ownership) { + mat->_foreign_data = true; + PyArray_ENABLEFLAGS(arr_t, NPY_OWNDATA); + } + + // Transpose the array + PyObject* arr = PyArray_Transpose(arr_t,NULL); + if(!arr) { + WARN("Array transpose failure"); + feTRACE(15); + return NULL; + } + Py_DECREF(arr_t); + + + feTRACE(15); + return arr; +} + + +#endif // ASCEE_PYTHON_H +////////////////////////////////////////////////////////////////////// diff --git a/beamforming/c/filterbank.c b/beamforming/c/filterbank.c new file mode 100644 index 0000000..463dfd6 --- /dev/null +++ b/beamforming/c/filterbank.c @@ -0,0 +1,167 @@ +// filterbank.c +// +// Author: J.A. de Jong -ASCEE +// +// Description: +// FilterBank implementation. +////////////////////////////////////////////////////////////////////// +#include "filterbank.h" +#include "fft.h" +#include "dfifo.h" +#include "ascee_tracer.h" +#include "ascee_alg.h" +#define FIFO_SIZE_MULT 2 + +typedef struct FilterBank_s { + us nfft; + + us P_m_1; /**< Filter length minus one */ + + cmat filters; /* Frequency-domain filter + * coefficients */ + dFifo* output_fifo; + dFifo* input_fifo; + + Fft* fft; /* Handle to internal FFT-function */ + +} FilterBank; + +FilterBank* FilterBank_create(const dmat* h, + const us nfft) { + + fsTRACE(15); + dbgassert(h,NULLPTRDEREF); + const us P = h->n_rows; + const us nfilters = h->n_cols; + + if(P > nfft/2) { + WARN("Filter order should be <= nfft/2"); + return NULL; + } + + Fft* fft = Fft_create(nfft); + if(!fft) { + WARN("Fft allocation failed"); + return NULL; + } + + FilterBank* fb = a_malloc(sizeof(FilterBank)); + + fb->nfft = nfft; + fb->P_m_1 = P-1; + fb->fft = fft; + fb->filters = cmat_alloc(nfft/2+1,nfilters); + + fb->output_fifo = dFifo_create(nfilters,FIFO_SIZE_MULT*nfft); + fb->input_fifo = dFifo_create(1,FIFO_SIZE_MULT*nfft); + + /* Create a temporary buffer which is going to be FFT'th to + * contain the filter transfer functions. + */ + dmat temp = dmat_alloc(nfft,nfilters); + dmat_set(&temp,0); + dmat_copy_rows(&temp,h,0,0,h->n_rows); + + /* Fft the FIR impulse responses */ + Fft_fft(fb->fft,&temp,&fb->filters); + + dmat_free(&temp); + + /* Push initial output of zeros to the output fifo */ + dmat initial_output = dmat_alloc(fb->P_m_1,nfilters); + dmat_set(&initial_output,0); + dFifo_push(fb->output_fifo,&initial_output); + dmat_free(&initial_output); + + feTRACE(15); + return fb; +} +void FilterBank_free(FilterBank* fb) { + fsTRACE(15); + dbgassert(fb,NULLPTRDEREF); + cmat_free(&fb->filters); + dFifo_free(fb->input_fifo); + dFifo_free(fb->output_fifo); + Fft_free(fb->fft); + a_free(fb); + feTRACE(15); +} +dmat FilterBank_filter(FilterBank* fb, + const vd* x) { + + fsTRACE(15); + dbgassert(fb && x ,NULLPTRDEREF); + + dFifo* input_fifo = fb->input_fifo; + dFifo* output_fifo = fb->output_fifo; + + const us nfft = fb->nfft; + const us nfilters = fb->filters.n_cols; + + /* Push samples to the input fifo */ + dmat samples = dmat_foreign_vd((vd*) x); + dFifo_push(fb->input_fifo,&samples); + dmat_free(&samples); + + dmat input_block = dmat_alloc(nfft,1); + + /* Output is ready to be multiplied with FFt'th filter */ + cmat input_fft = cmat_alloc(nfft/2+1,1); + + /* Output is ready to be multiplied with FFt'th filter */ + cmat output_fft = cmat_alloc(nfft/2+1,nfilters); + dmat output_block = dmat_alloc(nfft,nfilters); + + while (dFifo_size(input_fifo) >= nfft) { + + us nsamples = dFifo_pop(input_fifo, + &input_block, + fb->P_m_1 /* save P-1 samples */ + ); + dbgassert(nsamples == nfft,"BUG in dFifo"); + + Fft_fft(fb->fft,&input_block,&input_fft); + + vc input_fft_col = cmat_column(&input_fft,0); + + for(us col=0;colfilters,col); + vc_hadamard(&output_fft_col, + &input_fft_col, + &filter_col); + + vc_free(&output_fft_col); + vc_free(&filter_col); + } + vc_free(&input_fft_col); + + Fft_ifft(fb->fft,&output_fft,&output_block); + + dmat valid_samples = dmat_submat(&output_block, + 0,0, /* startrow, startcol */ + nfft-fb->P_m_1, /* Number of rows */ + output_block.n_cols); + dFifo_push(fb->output_fifo,&valid_samples); + dmat_free(&valid_samples); + + } + + dmat_free(&input_block); + cmat_free(&input_fft); + cmat_free(&output_fft); + dmat_free(&output_block); + + us samples_done = dFifo_size(output_fifo); + uVARTRACE(20,samples_done); + dmat filtered_result = dmat_alloc(samples_done,nfilters); + if(samples_done) { + us samples_done2 = dFifo_pop(output_fifo,&filtered_result,0); + dbgassert(samples_done2 == samples_done,"BUG in dFifo"); + } + feTRACE(15); + return filtered_result; +} + + +////////////////////////////////////////////////////////////////////// diff --git a/beamforming/c/filterbank.h b/beamforming/c/filterbank.h new file mode 100644 index 0000000..ed8a1f9 --- /dev/null +++ b/beamforming/c/filterbank.h @@ -0,0 +1,59 @@ +// filterbank.h +// +// Author: J.A. de Jong - ASCEE +// +// Description: Implemententation of a discrete filterbank using fast +// convolution and the overlap-save (overlap-scrap method). Multiple +// filters can be applied to the same input data (*filterbank*). +// Implementation is computationally efficient, as the forward FFT is +// performed only over the input data, and the backwards transfer for +// each filter in the filterbank. +////////////////////////////////////////////////////////////////////// +#pragma once +#ifndef FILTERBANK_H +#define FILTERBANK_H +#include "types.h" +#include "ascee_math.h" +typedef struct FilterBank_s FilterBank; + +/** + * Initializes a fast convolution filter bank and returns a FilterBank + * handle. The nfft will be chosen to be at least four times the + * length of the FIR filters. + * + * @param h: matrix with filter coefficients of each of the + * filters. First axis is the axis of the filter coefficients, second + * axis is the filter number. Maximum length of the filter is nfft/2. + * + * @param nfft: FTT length for fast convolution. For good performance, + * nfft should be chosen as the nearest power of 2, approximately four + * times the filter lengths. For the lowest possible latency, it is + * better to set nfft at twice the filter length. + * + * @return FilterBank handle, NULL on error. + */ +FilterBank* FilterBank_create(const dmat* h,const us nfft); + +/** + * Filters x using h, returns y + * + * @param x Input time sequence block. Should have at least one sample. + + * @return Filtered output in an allocated array. The number of + * columns in this array equals the number of filters in the + * filterbank. The number of output samples is equal to the number of + * input samples in x. + */ +dmat FilterBank_filter(FilterBank* fb, + const vd* x); + +/** + * Cleans up an existing filter bank. + * + * @param f Filter handle + */ +void FilterBank_free(FilterBank* f); + + +#endif // FILTERBANK_H +////////////////////////////////////////////////////////////////////// diff --git a/beamforming/config.pxi.in b/beamforming/config.pxi.in index fa2c3cf..ac2767b 100644 --- a/beamforming/config.pxi.in +++ b/beamforming/config.pxi.in @@ -28,7 +28,7 @@ cdef extern from "ascee_tracer.h": void TRACE(int,const char*) void fsTRACE(int) void feTRACE(int) - + void clearScreen() cdef extern from "ascee_math.h": ctypedef struct dmat: us n_cols @@ -50,10 +50,16 @@ cdef extern from "ascee_math.h": cmat cmat_alloc(us n_rows,us n_cols) dmat dmat_alloc(us n_rows,us n_cols) - + vd vd_foreign(const us size,d* data) + void vd_free(vd*) + void dmat_free(dmat*) void cmat_free(cmat*) void cmat_copy(cmat* to,cmat* from_) -cdef extern from "ascee_tracer.h": - void clearScreen() +cdef extern from "numpy/arrayobject.h": + void PyArray_ENABLEFLAGS(np.ndarray arr, int flags) + +cdef extern from "ascee_python.h": + object dmat_to_ndarray(dmat*,bint transfer_ownership) + diff --git a/beamforming/wrappers.pyx b/beamforming/wrappers.pyx index 78e32ba..9e3614a 100644 --- a/beamforming/wrappers.pyx +++ b/beamforming/wrappers.pyx @@ -259,3 +259,37 @@ cdef class AvPowerSpectra: dmat_free(&td) return result + +cdef extern from "filterbank.h": + ctypedef struct c_FilterBank "FilterBank": + pass + c_FilterBank* FilterBank_create(const dmat* h,const us nfft) nogil + dmat FilterBank_filter(c_FilterBank* fb,const vd* x) nogil + void FilterBank_free(c_FilterBank* fb) nogil + + +cdef class FilterBank: + cdef: + c_FilterBank* fb + def __cinit__(self,d[::1,:] h, us nfft): + cdef dmat hmat = dmat_foreign(h.shape[0],h.shape[1],&h[0,0]) + self.fb = FilterBank_create(&hmat,nfft) + dmat_free(&hmat) + if not self.fb: + raise RuntimeError('Error creating FilberBank') + + def __dealloc__(self): + if self.fb: + FilterBank_free(self.fb) + + def filter_(self,d[:] input_): + cdef vd input_vd = vd_foreign(input_.size,&input_[0]) + cdef dmat output = FilterBank_filter(self.fb,&input_vd) + + # Steal the pointer from output + result = dmat_to_ndarray(&output,True) + + dmat_free(&output) + vd_free(&input_vd) + + return result diff --git a/test/filterbank_test.py b/test/filterbank_test.py new file mode 100644 index 0000000..1d1c53b --- /dev/null +++ b/test/filterbank_test.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +Created on Mon Jan 15 19:45:33 2018 + +@author: anne +""" +import numpy as np +from beamforming import FilterBank, cls +cls() + +nfft=50 +P = 4 +L = nfft-P+2 +nfilters = 1 +print('nfft:',nfft) + +h = np.zeros((P,nfilters),order='F') +h[P-1,0] = 1. +#h[0,1] = 1. + +fb = FilterBank(h,nfft) +# %% +#data = np.zeros(nfft//2,order = 'F') +data = np.zeros(L,order = 'F') +data[:] = 1 +#data[:] = 1. +#data[1] = 1. +#data[2] = 1. + +res = fb.filter_(data) +print('res shape %i:\n' %res.shape[0]) +print(res) +#for i in range(4): +# res = fb.filter_(data) +# print('res:\n',res) \ No newline at end of file