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

This commit is contained in:
Anne de Jong 2022-08-16 21:22:35 +02:00
parent c75f0dddc5
commit f8e8ab422b
7 changed files with 431 additions and 28 deletions

View File

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

View File

@ -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)) {
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");
}
}
/// Initialize state to zero
state = dmat(2, nfilters, arma::fill::zeros);
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));
@ -88,8 +102,7 @@ void BiquadBank::filter(vd &inout) {
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();
}
}

View File

@ -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;
};

View File

@ -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;
};

221
src/lasp/dsp/lasp_slm.cpp Normal file
View File

@ -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;
}

130
src/lasp/dsp/lasp_slm.h Normal file
View File

@ -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);
};

View File

@ -1,6 +1,9 @@
#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>
@ -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);
}