SLM seems to be working. Needs proper testing. Not yet fully coupled to Python code

src/lasp/CMakeLists.txt

@@ -6,6 +6,7 @@ add_definitions(-DARMA_DONT_USE_WRAPPER)
 configure_file(lasp_config.h.in lasp_config.h)
 include_directories(${CMAKE_CURRENT_BINARY_DIR})
 include_directories(SYSTEM ../../third_party/armadillo-code/include)
+include_directories(SYSTEM ../../third_party/carma/include)
 include_directories(../../third_party/DebugTrace-cpp/include)
 include_directories(../../third_party/lockfreeThreadsafe/include)
 include_directories(../../third_party/gsl-lite/include)

src/lasp/dsp/lasp_biquadbank.cpp

@@ -1,42 +1,56 @@
-#include <vector>
 #define DEBUGTRACE_ENABLED
-#include "debugtrace.hpp"
 #include "lasp_biquadbank.h"
+#include "debugtrace.hpp"
 #include "lasp_thread.h"
+#include <vector>
 
+using std::cerr;
+using std::endl;
 using std::runtime_error;
 
 SeriesBiquad::SeriesBiquad(const vd &filter_coefs) {
+  DEBUGTRACE_ENTER;
 
   if (filter_coefs.n_rows % 6 != 0) {
-    throw runtime_error("filter_coefs should be multiple of 6");
+    cerr << "Number of rows given: " << filter_coefs.n_rows << endl;
+    throw runtime_error("filter_coefs should be multiple of 6, given: " +
+                        std::to_string(filter_coefs.n_rows));
   }
   us nfilters = filter_coefs.n_rows / 6;
 
+  /// Initialize state to zero
+  state = dmat(2, nfilters, arma::fill::zeros);
+
   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);
+
+  /// 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)) {
+    std::cerr << "Read row: " << sos.row(3) << endl;
+
+    throw std::runtime_error(
+        "Filter coefficients should have fourth element (a0) equal to 1.0");
+  }
+}
+void SeriesBiquad::reset() {
+  DEBUGTRACE_ENTER;
+  state.zeros();
 }
 void SeriesBiquad::filter(vd &inout) {
+
+  DEBUGTRACE_ENTER;
+
   /// 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 b0 = sos(0, filterno);
+    d b1 = sos(1, filterno);
+    d b2 = sos(2, filterno);
+    d a1 = sos(4, filterno);
+    d a2 = sos(5, filterno);
 
     d w1 = state(0, filterno);
     d w2 = state(1, filterno);
@@ -60,7 +74,7 @@ BiquadBank::BiquadBank(const dmat &filters, const vd *gains) {
    * @brief Make sure the pool is created once, such that all threads are ready
    * for use.
    */
-  auto& pool = getPool();
+  getPool();
 
   for (us i = 0; i < filters.n_cols; i++) {
     _filters.emplace_back(filters.col(i));
@@ -86,10 +100,9 @@ void BiquadBank::filter(vd &inout) {
   dmat res(inout.n_rows, _filters.size());
   std::vector<std::future<vd>> futs;
 
-  auto& pool = getPool();
+  auto &pool = getPool();
   for (us i = 0; i < res.n_cols; i++) {
-    futs.emplace_back(
-        pool.submit(
+    futs.emplace_back(pool.submit(
         [&](us i) {
           // Copy column
           vd col = inout;
@@ -106,3 +119,9 @@ void BiquadBank::filter(vd &inout) {
     inout += futs[i].get() * _gains[i];
   }
 }
+void BiquadBank::reset() {
+  DEBUGTRACE_ENTER;
+  for (auto &f : _filters) {
+    f.reset();
+  }
+}

src/lasp/dsp/lasp_biquadbank.h

@@ -1,3 +1,4 @@
+#pragma once
 #include "lasp_filter.h"
 
 /**
@@ -29,8 +30,9 @@ public:
    */
   SeriesBiquad(const vd &filter_coefs);
 
-  virtual void filter(vd &inout) override;
+  virtual void filter(vd &inout) override final;
   virtual ~SeriesBiquad() override {}
+  void reset() override final;
 };
 
 /**
@@ -67,6 +69,8 @@ public:
    */
   us nfilters() const {return _filters.size();}
 
-  virtual void filter(vd &inout) override;
+  virtual void filter(vd &inout) override final;
+
+  void reset() override final;
 
 };

src/lasp/dsp/lasp_filter.h

@@ -15,6 +15,10 @@ public:
    */
   virtual void filter(vd &inout) = 0;
   virtual ~Filter() = 0;
+  /**
+   * @brief Reset filter state to 0 (history was all-zero).
+   */
+  virtual void reset() = 0;
 };
 
 

src/lasp/dsp/lasp_slm.cpp

@@ -0,0 +1,221 @@
+#define DEBUGTRACE_ENABLED
+#include "lasp_slm.h"
+#include "debugtrace.hpp"
+#include "lasp_thread.h"
+#include <algorithm>
+#include <cmath>
+#include <future>
+#include <memory>
+
+using std::cerr;
+using std::endl;
+using std::runtime_error;
+using std::unique_ptr;
+
+SLM::SLM(const d fs, const d Lref, const us downsampling_fac, const d tau,
+         std::unique_ptr<Filter> pre_filter,
+         std::vector<std::unique_ptr<Filter>> bandpass)
+    : _pre_filter(std::move(pre_filter)), _bandpass(std::move(bandpass)),
+      _alpha(exp(-1 / (fs * tau))),
+      _sp_storage(_bandpass.size(), arma::fill::zeros), // Storage for
+                                                        // components of
+                                                        // single pole low pass
+                                                        // filter
+      Lref(Lref),                                       // Reference level
+      downsampling_fac(downsampling_fac),
+
+      // Initalize mean square
+      Pm(_bandpass.size(), arma::fill::zeros),
+
+      // Initalize max
+      Pmax(_bandpass.size(), arma::fill::zeros),
+
+      // Initalize peak
+      Ppeak(_bandpass.size(), arma::fill::zeros)
+
+{
+  DEBUGTRACE_ENTER;
+
+  // Make sure thread pool is running
+  getPool();
+
+  if (Lref <= 0) {
+    throw runtime_error("Invalid reference level");
+  }
+  if (tau <= 0) {
+    throw runtime_error("Invalid time constant for Single pole lowpass filter");
+  }
+  if (fs <= 0) {
+    throw runtime_error("Invalid sampling frequency");
+  }
+}
+SLM::~SLM() {}
+
+/**
+ * @brief Create set bandpass filters from filter coefficients
+ *
+ * @param coefs
+ *
+ * @return
+ */
+std::vector<unique_ptr<Filter>> createBandPass(const dmat &coefs) {
+  DEBUGTRACE_ENTER;
+  std::vector<unique_ptr<Filter>> bf;
+  for (us colno = 0; colno < coefs.n_cols; colno++) {
+    bf.emplace_back(std::make_unique<SeriesBiquad>(coefs.col(colno)));
+  }
+  return bf;
+}
+
+SLM SLM::fromBiquads(const d fs, const d Lref, const us downsampling_fac,
+                     const d tau, const vd &pre_filter_coefs,
+                     const dmat &bandpass_coefs) {
+  DEBUGTRACE_ENTER;
+
+  return SLM(fs, Lref, downsampling_fac, tau,
+             std::make_unique<SeriesBiquad>(pre_filter_coefs),
+             createBandPass(bandpass_coefs));
+}
+SLM SLM::fromBiquads(const d fs, const d Lref, const us downsampling_fac,
+                     const d tau, const dmat &bandpass_coefs) {
+
+  DEBUGTRACE_ENTER;
+
+  return SLM(fs, Lref, downsampling_fac, tau,
+             nullptr,                       // Pre-filter
+             createBandPass(bandpass_coefs) // Bandpass coefficients
+  );
+}
+
+vd SLM::run_single(const us i, d *_work, const us size) {
+
+  vd work(_work, // Aux Memory pointer
+          size,  // Number of elements
+          false, // Copy aux mem
+          true   // Stric, means we keep being bound to the memory
+  );
+
+  // Filter input in-place
+  _bandpass[i]->filter(work);
+
+  /* cerr << "Filter done" << endl; */
+
+  // Square input --> Signal powers
+  /* work.transform([](d j) { return j * j; }); */
+  work %= work;
+
+  // Compute peak level, that is before single-pole low pass filter
+  Ppeak(i) = std::max(Ppeak(i), arma::max(work));
+
+  // Create copy of N, as we run this in multiple threads.
+  us N_local = N;
+  DEBUGTRACE_PRINT(N);
+
+  // Obtain storage of single_pole low pass filter
+  d cur_storage = _sp_storage(i);
+
+  for (us j = 0; j < work.n_rows; j++) {
+    // Update mean square of signal, work is here still signal power
+    Pm(i) = (Pm(i) * static_cast<d>(N_local) + work(j)) /
+            (static_cast<d>(N_local) + 1);
+
+    N_local++;
+
+    cur_storage = _alpha * cur_storage + (1 - _alpha) * work(j);
+
+    // Now work is single-pole lowpassed signal power
+    work(j) = cur_storage;
+  }
+
+  // And update storage of low-pass filter
+  _sp_storage(i) = cur_storage;
+
+  Pmax(i) = std::max(Pmax(i), arma::max(work));
+
+  // Convert to levels in dB
+  /* work.transform([&](d val) { */
+  /*   return 10 * log10((val */
+
+  /*                      + arma::datum::eps // Add a bit of machine epsilon to */
+  /*                                         // the values to not compute -inf. */
+  /*                      ) / */
+  /*                     (Lref * Lref)); */
+  /* }); */
+  work = 10*arma::log10((work+arma::datum::eps)/(Lref*Lref));
+
+  return work;
+}
+
+dmat SLM::run(const vd &input_orig) {
+
+  DEBUGTRACE_ENTER;
+  vd input = input_orig;
+
+  // _pre_filter filters in-place
+  if (_pre_filter) {
+    _pre_filter->filter(input);
+  }
+
+  std::vector<std::future<vd>> futs;
+  auto &pool = getPool();
+
+  // Fan out over multiple threads, as it is typically a heavy load
+  dmat res(input.n_rows, _bandpass.size());
+
+  // Perform operations in-place.
+  for (us i = 0; i < _bandpass.size(); i++) {
+    res.col(i) = input;
+
+    /// It is not possible to forward a vector of values from this array in a
+    /// sensible way to a different thread. We therefore break it down to
+    /// good-old pointers and values.
+    futs.emplace_back(
+        pool.submit(&SLM::run_single, this, i, res.colptr(i), res.n_rows));
+  }
+
+  /* DEBUGTRACE_PRINT(_bandpass.size()); */
+  /* DEBUGTRACE_PRINT(res.n_cols); */
+  /* DEBUGTRACE_PRINT(res.n_rows); */
+  /* DEBUGTRACE_PRINT(futs.size()); */
+
+  // Wait for all threads to complete
+  for (us i = 0; i < _bandpass.size(); i++) {
+    futs[i].get();
+  }
+
+  // Update the total number of samples harvested so far. NOTE: *This should be
+  // done AFTER the threads are done!!!*
+  N += input.n_rows;
+
+  // Downsample, if applicable
+  if (downsampling_fac > 1) {
+    dmat res_ds;
+    us rowno = 0;
+    while (cur_offset < res.n_rows) {
+      res_ds.insert_rows(rowno, res.row(cur_offset));
+      rowno++;
+
+      cur_offset += downsampling_fac;
+    }
+    cur_offset -= res.n_rows;
+    // Instead, return a downsampled version
+    return res_ds;
+  }
+
+  return res;
+}
+void SLM::reset() {
+  Pm.zeros();
+  Pmax.zeros();
+  Ppeak.zeros();
+  for (auto &f : _bandpass) {
+    f.reset();
+  }
+  if (_pre_filter) {
+    _pre_filter->reset();
+  }
+  _sp_storage.zeros();
+  cur_offset = 0;
+
+  N = 0;
+}

src/lasp/dsp/lasp_slm.h

@@ -0,0 +1,130 @@
+#pragma once
+#include "lasp_biquadbank.h"
+#include "lasp_filter.h"
+#include <memory>
+#include <optional>
+
+/**
+ * @brief Sound Level Meter implementation that gives a result for each
+ * channel. A channel is the result  of a filtered signal
+ */
+class SLM {
+  /**
+   * @brief A, C or Z weighting, depending on the pre-filter installed.
+   */
+  std::unique_ptr<Filter> _pre_filter;
+
+  /**
+   * @brief Bandpass filters for each channel
+   */
+  std::vector<std::unique_ptr<Filter>> _bandpass;
+  /**
+   * @brief Storage for the single-pole low-pass filter coefficient based on
+   * the Fast / Slow time constant. < 0 means the filter is disabled.
+   */
+  d _alpha = -1;
+  vd _sp_storage;
+
+  d Lref;         /// Reference value for computing decibels
+  us downsampling_fac; /// Every x'th sample is returned.
+  us cur_offset = 0;   /// Storage for offset point in input arrays
+                       ///
+public:
+  /**
+   * @brief Public storage for the mean of the square of the signal.
+   */
+  vd Pm;
+  /**
+   * @brief Public storage for the maximum signal power, after single pole
+   * low-pass filter.
+   */
+  vd Pmax;  /// Storage for maximum computed signal power so far.
+  /**
+   * @brief Public storage for the peak signal power, before single pole
+   * low-pass filter.
+   */
+  vd Ppeak;
+
+  us N = 0;     /// Counter for the number of time samples counted that came
+                /// in;
+
+  /**
+   * @brief Initialize a Sound Level Meter
+   *
+   * @param fs Sampling frequency [Hz]
+   * @param Lref Level reference, used to scale to proper decibel units (dB
+   * SPL / dBV, etc)
+   * @param downsampling_fac Every 1/downsampling_fac value is returned from
+   * compute()
+   * @param tau Time consant of level meter
+   * @param pre_filter The pre-filter (Typically an A/C frequency weighting
+   * filter)
+   * @param bandpass The parallel set of bandpass filters.
+   */
+  SLM(const d fs, const d Lref, const us downsampling_fac, const d tau,
+      std::unique_ptr<Filter> pre_filter,
+      std::vector<std::unique_ptr<Filter>> bandpass);
+
+  /**
+   * @brief Convenience function to create a Sound Level meter from Biquad
+   * filters only.
+   *
+   * @param fs Sampling frequency [Hz]
+   * @param Lref Level reference, used to scale to proper decibel units (dB
+   * SPL / dBV, etc)
+   * @param downsampling_fac Every 1/downsampling_fac value is returned from
+   * compute()
+   * @param tau Time consant of level meter
+   * @param pre_filter_coefs Biquad filter coefficients for pre-filter
+   * @param bandpass_coefs Biquad filter coeffiecients for bandpass filter
+   *
+   * @return Sound Level Meter object
+   */
+  static SLM fromBiquads(const d fs, const d Lref, const us downsampling_fac,
+                         const d tau, const vd &pre_filter_coefs,
+                         const dmat &bandpass_coefs);
+  /**
+   * @brief Convenience function to create a Sound Level meter from Biquad
+   * filters only. No pre-filter, only bandpass.
+   *
+   * @param fs Sampling frequency [Hz]
+   * @param Lref Level reference, used to scale to proper decibel units (dB
+   * SPL / dBV, etc)
+   * @param downsampling_fac Every 1/downsampling_fac value is returned from
+   * compute()
+   * @param tau Time consant of level meter
+   * @param bandpass_coefs Biquad filter coeffiecients for bandpass filter
+   *
+   * @return Sound Level Meter object
+   */
+  static SLM fromBiquads(const d fs, const d Lref, const us downsampling_fac,
+                         const d tau, const dmat &bandpass_coefs);
+
+  ~SLM();
+
+  /**
+   * @brief Reset state related to samples acquired. All filters reset to zero.
+   * Start again from no history.
+   */
+  void reset();
+
+  SLM(const SLM &o) = delete;
+  SLM &operator=(const SLM &o) = delete;
+  SLM(SLM &&o) = default;
+
+  /**
+   * @brief Run the sound level meter on given input data. Return downsampled
+   * level data for each filterbank channel.
+   *
+   * @param input Raw input data
+   *
+   * @return Filtered level data.
+   */
+  dmat run(const vd &input);
+  vd Lpeak() const { return 10*arma::log10(Ppeak/Lref);};
+  vd Leq() const { return 10*arma::log10(Pm/Lref);};
+  vd Lmax() const { return 10*arma::log10(Pmax/Lref);};
+
+private:
+  vd run_single(const us i,d* inout,const us size);
+};

src/lasp/pybind11/lasp_dsp_pybind.cpp

@@ -1,13 +1,16 @@
-#include "lasp_window.h"
+#include "carma"
+#include "lasp_biquadbank.h"
 #include "lasp_siggen.h"
 #include "lasp_siggen_impl.h"
+#include "lasp_slm.h"
+#include "lasp_window.h"
 #include <iostream>
 #include <pybind11/pybind11.h>
 
 using std::cerr;
 namespace py = pybind11;
 
-void init_dsp(py::module& m) {
+void init_dsp(py::module &m) {
 
   py::class_<Window> w(m, "Window");
 
@@ -20,10 +23,31 @@ void init_dsp(py::module& m) {
       .export_values();
 
   py::class_<Siggen, std::shared_ptr<Siggen>> siggen(m, "Siggen");
-  siggen.def("setLevel", &Siggen::setLevel, "Set the level of the signal generator");
-
+  siggen.def("setLevel", &Siggen::setLevel,
+             "Set the level of the signal generator");
 
   py::class_<Sine, std::shared_ptr<Sine>> sw(m, "Sine", siggen);
   sw.def(py::init<const d>());
 
+  py::class_<SeriesBiquad> sbq(m, "SeriesBiquad");
+  sbq.def(py::init<const vd &>());
+  sbq.def("filter", [](SeriesBiquad &s, const vd &input) {
+    vd res = input;
+    s.filter(res);
+    return res;
+  });
+
+  py::class_<SLM> slm(m, "SLM");
+  slm.def_static(
+      "fromBiquads",
+      py::overload_cast<const d, const d, const us, const d, const dmat &>(
+          &SLM::fromBiquads));
+  slm.def_static(
+      "fromBiquads",
+      py::overload_cast<const d, const d, const us, const d, const vd &,
+                        const dmat &>(&SLM::fromBiquads));
+  slm.def("run", &SLM::run);
+  slm.def_readonly("Pm", &SLM::Pm);
+  slm.def_readonly("Pmax", &SLM::Pmax);
+  slm.def_readonly("Ppeak", &SLM::Ppeak);
 }