Added threadpool, biquadbank could be working for equalizer. First steps of SLM and AvPowerSpectra

.gitignore

@@ -18,3 +18,4 @@ _skbuild
 src/lasp.egg-info
 test/.ipynb_checkpoints
 src/lasp/lasp_config.h
+_deps

.gitmodules

@@ -19,3 +19,6 @@
 [submodule "third_party/carma"]
 	path = third_party/carma
 	url = https://github.com/RUrlus/carma
+[submodule "third_party/thread-pool"]
+	path = third_party/thread-pool
+	url = https://github.com/bshoshany/thread-pool

src/lasp/CMakeLists.txt

@@ -10,6 +10,7 @@ include_directories(../../third_party/DebugTrace-cpp/include)
 include_directories(../../third_party/lockfreeThreadsafe/include)
 include_directories(../../third_party/gsl-lite/include)
 include_directories(../../third_party/tomlplusplus/include)
+include_directories(../../third_party/thread-pool)
 
 add_subdirectory(device)
 add_subdirectory(dsp)

src/lasp/dsp/CMakeLists.txt

@@ -7,6 +7,10 @@ set(lasp_dsp_files
     lasp_window.cpp
     lasp_fft.cpp
     lasp_avpowerspectra.cpp
+    lasp_biquadbank.cpp
+    lasp_thread.cpp
+    lasp_timebuffer.cpp
+    lasp_slm.cpp
     )
 
 

src/lasp/dsp/lasp_avpowerspectra.cpp

@@ -1,21 +1,23 @@
+#include <optional>
 #define DEBUGTRACE_ENABLED
-#include "lasp_avpowerspectra.h"
 #include "debugtrace.hpp"
+#include "lasp_avpowerspectra.h"
 #include <cmath>
 
 using std::runtime_error;
 
 PowerSpectra::PowerSpectra(const us nfft, const Window::WindowType w)
-  : PowerSpectra(Window::create(w, nfft)) {}
+    : PowerSpectra(Window::create(w, nfft)) {}
 
 PowerSpectra::PowerSpectra(const vd &window)
-  : nfft(window.size()), _fft(nfft), _window(window) {
+    : nfft(window.size()), _fft(nfft), _window(window) {
 
-    d win_pow = arma::sum(window % window);
+  d win_pow = arma::sum(window % window);
+
+  /* Scale fft such that power is easily computed */
+  _scale_fac = sqrt(2 / win_pow) / nfft;
+}
 
-    /* Scale fft such that power is easily computed */
-    _scale_fac = sqrt(2 / win_pow) / nfft;
-  }
 arma::Cube<c> PowerSpectra::compute(const dmat &input) {
 
   dmat input_tmp = input;
@@ -35,34 +37,64 @@ arma::Cube<c> PowerSpectra::compute(const dmat &input) {
 }
 
 AvPowerSpectra::AvPowerSpectra(const us nfft, const Window::WindowType w,
-    const d overlap_percentage,
-    const int time_constant)
-  : _ps(nfft, w) {
-
-    DEBUGTRACE_ENTER;
-    if (overlap_percentage >= 100 || overlap_percentage < 0) {
-      throw runtime_error("Overlap percentage should be >= 0 and < 100");
-    }
-
-    _overlap_jump = nfft - (nfft * overlap_percentage) / 100;
-    if (_overlap_jump == 0) {
-      throw runtime_error("Overlap is too high. Results in no jump. Please "
-          "choose a smaller overlap percentage or a higher nfft");
-    }
-    if(time_constant < 0) {
-      _mode = Mode::Averaging;
-    } else if(time_constant == 0) {
-      _mode = Mode::Spectrogram;
-    }
-    else {
-      _mode = Mode::Leaking;
-      _alpha = exp(-static_cast<d>(_overlap_jump)/time_constant);
-    }
+                               const d overlap_percentage,
+                               const int time_constant)
+    : _ps(nfft, w) {
 
+  DEBUGTRACE_ENTER;
+  if (overlap_percentage >= 100 || overlap_percentage < 0) {
+    throw runtime_error("Overlap percentage should be >= 0 and < 100");
   }
 
-arma::cx_cube* AvPowerSpectra::compute(const dmat& timedata) {
-
-
-  return nullptr;
+  _overlap_keep = (nfft * overlap_percentage) / 100;
+  if (_overlap_keep >= nfft) {
+    throw runtime_error("Overlap is too high. Results in no jump. Please "
+                        "choose a smaller overlap percentage or a higher nfft");
+  }
+  if (time_constant < 0) {
+    _mode = Mode::Averaging;
+  } else if (time_constant == 0) {
+    _mode = Mode::Spectrogram;
+  } else {
+    _mode = Mode::Leaking;
+    _alpha = exp(-static_cast<d>(nfft - _overlap_keep) / time_constant);
+  }
+}
+
+std::optional<arma::cx_cube> AvPowerSpectra::compute(const dmat &timedata) {
+
+  _timeBuf.push(timedata);
+  std::optional<arma::cx_cube> res;
+
+  while (auto samples = _timeBuf.pop(_ps.nfft, _overlap_keep)) {
+    switch (_mode) {
+    case (Mode::Spectrogram): {
+      res.emplace(_ps.compute(samples.value()));
+    } break;
+    case (Mode::Averaging): {
+      n_averages++;
+      if (n_averages == 1) {
+        _est = _ps.compute(samples.value());
+      } else {
+        _est = _est * (n_averages - 1) / n_averages +
+               _ps.compute(samples.value()) / n_averages;
+      }
+      res = _est;
+    } break;
+    case (Mode::Leaking): {
+      if (arma::size(_est) == arma::size(0, 0, 0)) {
+        _est = _ps.compute(samples.value());
+      } else {
+        _est = _alpha * _est + (1 - _alpha) * _ps.compute(samples.value());
+      }
+      res = _est;
+    } break;
+    } // end switch mode
+  }
+  return res;
+}
+std::optional<arma::cx_cube> AvPowerSpectra::get_est() {
+  if (_est.n_cols > 0)
+    return _est;
+  return std::nullopt;
 }

src/lasp/dsp/lasp_avpowerspectra.h

@@ -1,8 +1,9 @@
 #pragma once
-#include <memory>
 #include "lasp_fft.h"
 #include "lasp_mathtypes.h"
+#include "lasp_timebuffer.h"
 #include "lasp_window.h"
+#include <memory>
 #include <optional>
 
 /**
@@ -71,7 +72,8 @@ class AvPowerSpectra {
   };
 
   Mode _mode;
-  d _alpha; // Only valid in case of 'Leaking'
+  d _alpha;          // Only valid in case of 'Leaking'
+  us n_averages = 0; // Only valid in case of 'Averaging'
 
   PowerSpectra _ps;
   /**
@@ -82,15 +84,11 @@ class AvPowerSpectra {
   /**
    * @brief Buffer of storage of time data.
    */
-  dmat _timeBuf;
+  TimeBuffer _timeBuf;
   /**
-   * @brief Current time index in ring buffer
+   * @brief The amount of samples to keep in the overlap
    */
-  us _curTimeIdx = 0;
-  /**
-   * @brief The amount of samples to jump in the overlap
-   */
-  us _overlap_jump;
+  us _overlap_keep;
 
 public:
   /**
@@ -132,10 +130,10 @@ public:
    *
    * @return Optionally, a reference (NOT OWNING POINTER) to a new estimate of
    * the power spectra. An update is only given if the amount of new time data
-   * is enough to compute a new estimate. This is the case when the amount of
-   * new time data is enough.
+   * is enough to compute a new estimate.
+   *
    */
-  arma::cx_cube *compute(const dmat &timedata);
+  std::optional<arma::cx_cube> compute(const dmat &timedata);
 
   /**
    * @brief Returns the latest estimate of cps (cross-power spectra.
@@ -143,7 +141,7 @@ public:
    * @return Pointer (reference, not owning!) to spectral estimate date, or
    * nullptr, in case nothing could yet be computed.
    */
-  arma::cx_cube *get_est();
+  std::optional<arma::cx_cube> get_est();
 
   /**
    * @brief The overlap is rounded to a certain amount of time samples. This
@@ -151,5 +149,5 @@ public:
    *
    * @return The amount of samples in overlapping.
    */
-  us exactOverlapSamples() const { return _ps.nfft - _overlap_jump; }
+  us exactOverlapSamples() const { return _ps.nfft - _overlap_keep; }
 };

src/lasp/dsp/lasp_biquadbank.cpp

@@ -0,0 +1,108 @@
+#include <vector>
+#define DEBUGTRACE_ENABLED
+#include "debugtrace.hpp"
+#include "lasp_biquadbank.h"
+#include "lasp_thread.h"
+
+using std::runtime_error;
+
+SeriesBiquad::SeriesBiquad(const vd &filter_coefs) {
+
+  if (filter_coefs.n_rows % 6 != 0) {
+    throw runtime_error("filter_coefs should be multiple of 6");
+  }
+  us nfilters = filter_coefs.n_rows / 6;
+
+  sos.resize(6, nfilters);
+  for (us i = 0; i < nfilters; i++) {
+    sos.col(i) = filter_coefs.subvec(6 * i, 6 * (i + 1) - 1);
+
+    /// Check if third row in this matrix equals unity.
+    if (!arma::approx_equal(sos.row(3),
+                            arma::rowvec(nfilters, arma::fill::ones), "absdiff",
+                            1e-9)) {
+      throw std::runtime_error(
+          "Filter coefficients should have fourth element (a0) equal to 1.0");
+    }
+  }
+  /// Initialize state to zero
+  state = dmat(2, nfilters, arma::fill::zeros);
+}
+void SeriesBiquad::filter(vd &inout) {
+  /// Implementation is based on Proakis & Manolakis - Digital Signal
+  /// Processing, Fourth Edition, p. 550
+  for (us filterno = 0; filterno < sos.n_cols; filterno++) {
+    d b0 = sos(filterno, 0);
+    d b1 = sos(filterno, 1);
+    d b2 = sos(filterno, 2);
+    d a1 = sos(filterno, 4);
+    d a2 = sos(filterno, 5);
+
+    d w1 = state(0, filterno);
+    d w2 = state(1, filterno);
+
+    for (us sample = 0; sample < inout.size(); sample++) {
+      d w0 = inout(sample) - a1 * w1 - a2 * w2;
+      d yn = b0 * w0 + b1 * w1 + b2 * w2;
+      w2 = w1;
+      w1 = w0;
+      inout(sample) = yn;
+    }
+
+    state(0, filterno) = w1;
+    state(1, filterno) = w2;
+  }
+}
+
+BiquadBank::BiquadBank(const dmat &filters, const vd *gains) {
+  DEBUGTRACE_ENTER;
+  /**
+   * @brief Make sure the pool is created once, such that all threads are ready
+   * for use.
+   */
+  auto& pool = getPool();
+
+  for (us i = 0; i < filters.n_cols; i++) {
+    _filters.emplace_back(filters.col(i));
+  }
+
+  if (gains != nullptr) {
+    setGains(*gains);
+  } else {
+    _gains = vd(_filters.size(), arma::fill::ones);
+  }
+}
+void BiquadBank::setGains(const vd &gains) {
+  DEBUGTRACE_ENTER;
+  const us nfilters = _filters.size();
+  if (gains.size() != nfilters) {
+    throw runtime_error("Invalid number of gain values given.");
+  }
+  _gains = gains;
+}
+
+void BiquadBank::filter(vd &inout) {
+
+  dmat res(inout.n_rows, _filters.size());
+  std::vector<std::future<vd>> futs;
+
+  auto& pool = getPool();
+  for (us i = 0; i < res.n_cols; i++) {
+    futs.emplace_back(
+        pool.submit(
+        [&](us i) {
+          // Copy column
+          vd col = inout;
+          _filters[i].filter(col);
+          return col;
+        }, // Launch a task to filter.
+        i  // Column i as argument to the lambda function above.
+        ));
+  }
+
+  // Zero-out in-out and sum-up the filtered values
+  inout.zeros();
+  for (us i = 0; i < res.n_cols; i++) {
+    inout += futs[i].get() * _gains[i];
+  }
+}

src/lasp/dsp/lasp_biquadbank.h

@@ -0,0 +1,72 @@
+#include "lasp_filter.h"
+
+/**
+ * @brief A set of Biquad filters in series.
+ */
+class SeriesBiquad : public Filter {
+
+  /// The filter coefficients for each of the filters in the Filterbank
+  /// The *first* axis is the filter no, the second axis contains the
+  /// filter coefficients, in the order, b_0, b_1, b_2, a_0, a_1, a_2, which
+  /// corresponds to the transfer function
+  ///                 b_0 + b_1 z^-1 + b_2 z^-2
+  ///         H[z] =  -------------------------
+  ///                 a_0 + a_1 z^-1 + a_2 z^-2
+  ///
+  dmat sos; /// sos[coef, filter_no]
+            ///
+  /// Storage for the current state of the output, first axis correspond to
+  /// the state axis, the second axis to the series filter no. For each filter
+  /// in series, two state coefficients are remembered.
+  dmat state;
+
+public:
+  /**
+   * @brief Initalize a SeriesBiquad filter.
+   *
+   * @param filter_coefs Filter coefficients, should be given in the order as
+   * [b0, b1, b2, a0==1, a1, a2, b0,...]
+   */
+  SeriesBiquad(const vd &filter_coefs);
+
+  virtual void filter(vd &inout) override;
+  virtual ~SeriesBiquad() override {}
+};
+
+/**
+ * @brief Multiple biquad filters in parallel, each multiplied with a gain
+ * value, and finally all added together. This class can be used to create
+ * a graphic equalizer.
+ */
+class BiquadBank : public Filter {
+  std::vector<SeriesBiquad> _filters;
+  vd _gains;
+
+public:
+  /**
+   * @brief Initialize biquadbank.
+   *
+   * @param filters Filters for each filter in the bank
+   * @param gains Gain values. Given as pointer, if not given (nulltpr), gains
+   * are initialized with unity gain.
+   */
+  BiquadBank(const dmat &filters, const vd *gains = nullptr);
+
+  /**
+   * @brief Set new gain values for each filter in the BiquadBank
+   *
+   * @param gains Vector of gain values. Should be of same length as the number
+   * of filters installed.
+   */
+  void setGains(const vd& gains);
+
+  /**
+   * @brief Returns the number of Filters
+   *
+   * @return The number of filters
+   */
+  us nfilters() const {return _filters.size();}
+
+  virtual void filter(vd &inout) override;
+
+};

src/lasp/dsp/lasp_filter.cpp

@@ -0,0 +1,4 @@
+#include "lasp_filter.h"
+
+
+

src/lasp/dsp/lasp_filter.h

@@ -8,7 +8,14 @@
  */
 class Filter {
 public:
-  virtual void filter(const vd &inout) = 0;
+  /**
+   * @brief Filter input, and provides output in same array as input
+   *
+   * @param inout Vector of input / output samples. 
+   */
+  virtual void filter(vd &inout) = 0;
   virtual ~Filter() = 0;
 };
 
+
+inline Filter::~Filter() {}

src/lasp/dsp/lasp_thread.cpp

@@ -0,0 +1,9 @@
+#include "lasp_thread.h"
+#include "BS_thread_pool.hpp"
+
+BS::thread_pool& getPool() {
+  static BS::thread_pool pool;
+
+  return pool;
+
+}

src/lasp/dsp/lasp_thread.h

@@ -0,0 +1,11 @@
+#pragma once
+#include "BS_thread_pool.hpp"
+
+/**
+ * @brief Return reference to global (singleton) thread pool. The threadpool is
+ * created using the default argument, which results in exactly
+ * hardware_concurrency() amount of threads.
+ *
+ * @return Thread pool ref.
+ */
+BS::thread_pool& getPool();

src/lasp/dsp/lasp_timebuffer.cpp

@@ -0,0 +1,82 @@
+#include "lasp_timebuffer.h"
+#include <algorithm>
+#include <cassert>
+#include <deque>
+#include <memory>
+#include <optional>
+#include <stdexcept>
+
+using std::runtime_error;
+
+class TimeBufferImp {
+  /**
+   * @brief Storage in a double-ended queue of armadillo row vectors.
+   */
+  std::deque<arma::rowvec> _storage;
+
+public:
+  void reset() {
+    _storage.clear();
+  }
+  void push(const dmat &mat) {
+#if LASP_DEBUG==1
+    if(!_storage.empty()) {
+      if(mat.n_cols != _storage.front().n_cols) {
+        throw runtime_error("Invalid number of channels in mat");
+      }
+    }
+#endif
+    for (us i = 0; i < mat.n_rows; i++) {
+      _storage.push_back(mat.row(i));
+    }
+  }
+
+  std::optional<dmat> pop(const us nsamples, us keep) {
+    if (keep > nsamples)
+      throw runtime_error("keep should be <= nsamples");
+
+    if (nsamples <= n_frames()) {
+
+      assert(!_storage.empty());
+
+      dmat res(nsamples, _storage.front().n_cols);
+
+      for (us i = 0; i < nsamples; i++) {
+
+        if (i + keep >= nsamples) {
+          // Suppose keep == 0, then we never arrive here
+          // Suppose keep == 1, then storage[0] is copyied over.
+          // Suppose keep == 2, then storage[0[ and storage[1] is copyied over.
+          // Etc.
+          res.row(i) = _storage[i + keep - nsamples];
+        } else {
+
+          // Just pop elements and copy over
+          res.row(i) = _storage.front();
+          _storage.pop_front();
+        }
+      }
+      return res;
+    }
+
+    // If nsamples is too much for what we have, we just return nothing.
+    return std::nullopt;
+  }
+
+  /**
+   * @brief Counts the number of available frames in the queue
+   *
+   * @return
+   */
+  us n_frames() const { return _storage.size(); }
+};
+
+TimeBuffer::TimeBuffer() : _imp(std::make_unique<TimeBufferImp>()) {}
+std::optional<dmat> TimeBuffer::pop(const us n_rows,const us keep) {
+  return _imp->pop(n_rows, keep);
+}
+TimeBuffer::~TimeBuffer(){}
+void TimeBuffer::reset() {
+  _imp->reset();
+}
+void TimeBuffer::push(const dmat &dat) { _imp->push(dat); }

src/lasp/dsp/lasp_timebuffer.h

@@ -0,0 +1,40 @@
+#pragma once
+#include "lasp_mathtypes.h"
+#include <memory>
+#include <optional>
+
+class TimeBufferImp;
+/**
+ * @brief Implementation of a buffer of time samples, where
+ */
+class TimeBuffer {
+  std::unique_ptr<TimeBufferImp> _imp;
+
+public:
+  TimeBuffer();
+  ~TimeBuffer();
+  /**
+   * @brief Put samples in the buffer. Number of channels should match other
+   * frames, otherwise things go wrong.
+   *
+   * @param mat Samples to push, axes should be as mat(frame, channel).
+   */
+  void push(const dmat &mat);
+
+  /**
+   * @brief Reset (empties) the time buffer.
+   */
+  void reset();
+
+  /**
+   * @brief Try top pop frames from the buffer.
+   *
+   * @param nframes The number of rows
+   * @param keep The number of frames to copy, but also to keep in the buffer
+   * (usage: overlap)
+   *
+   * @return An optional container, containing a matrix of time samples for
+   * each channel
+   */
+  std::optional<dmat> pop(const us nframes, const us keep = 0);
+};

src/lasp/pybind11/lasp_pyindatahandler.cpp

@@ -1,6 +1,7 @@
 #include "debugtrace.hpp"
 #include "lasp_streammgr.h"
 #include <atomic>
+#include <boost/lockfree/policies.hpp>
 #include <boost/lockfree/spsc_queue.hpp>
 #include <chrono>
 #include <pybind11/buffer_info.h>

third_party/thread-pool

@@ -0,0 +1 @@
+Subproject commit 67fad04348b91cf93bdfad7495d298f54825602c