Added filterbank and toebehoren

beamforming/c/CMakeLists.txt

@@ -16,7 +16,7 @@ add_library(beamforming_lib
   mq.c
   worker.c
   dfifo.c
-  filter.c
+  filterbank.c
   )
 
 

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 <numpy/ndarrayobject.h>
+#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
+//////////////////////////////////////////////////////////////////////

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;col<nfilters;col++) {
+            vc output_fft_col = cmat_column(&output_fft,col);
+            vc filter_col = cmat_column(&fb->filters,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;
+}
+
+
+//////////////////////////////////////////////////////////////////////

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
+//////////////////////////////////////////////////////////////////////

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)
+

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

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)