diff --git a/.gitignore b/.gitignore index ea4df6d..8017c00 100644 --- a/.gitignore +++ b/.gitignore @@ -18,3 +18,4 @@ _skbuild src/lasp.egg-info test/.ipynb_checkpoints src/lasp/lasp_config.h +_deps diff --git a/.gitmodules b/.gitmodules index 89efc2b..36191d5 100644 --- a/.gitmodules +++ b/.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 diff --git a/src/lasp/CMakeLists.txt b/src/lasp/CMakeLists.txt index be77a2f..8878ec5 100644 --- a/src/lasp/CMakeLists.txt +++ b/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) diff --git a/src/lasp/dsp/CMakeLists.txt b/src/lasp/dsp/CMakeLists.txt index d951e78..a73e30f 100644 --- a/src/lasp/dsp/CMakeLists.txt +++ b/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 ) diff --git a/src/lasp/dsp/lasp_avpowerspectra.cpp b/src/lasp/dsp/lasp_avpowerspectra.cpp index d98d19d..85471de 100644 --- a/src/lasp/dsp/lasp_avpowerspectra.cpp +++ b/src/lasp/dsp/lasp_avpowerspectra.cpp @@ -1,21 +1,23 @@ +#include #define DEBUGTRACE_ENABLED -#include "lasp_avpowerspectra.h" #include "debugtrace.hpp" +#include "lasp_avpowerspectra.h" #include 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 PowerSpectra::compute(const dmat &input) { dmat input_tmp = input; @@ -35,34 +37,64 @@ arma::Cube 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(_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(nfft - _overlap_keep) / time_constant); + } +} + +std::optional AvPowerSpectra::compute(const dmat &timedata) { + + _timeBuf.push(timedata); + std::optional 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 AvPowerSpectra::get_est() { + if (_est.n_cols > 0) + return _est; + return std::nullopt; } diff --git a/src/lasp/dsp/lasp_avpowerspectra.h b/src/lasp/dsp/lasp_avpowerspectra.h index 67acd6f..558ab0f 100644 --- a/src/lasp/dsp/lasp_avpowerspectra.h +++ b/src/lasp/dsp/lasp_avpowerspectra.h @@ -1,8 +1,9 @@ #pragma once -#include #include "lasp_fft.h" #include "lasp_mathtypes.h" +#include "lasp_timebuffer.h" #include "lasp_window.h" +#include #include /** @@ -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 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 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; } }; diff --git a/src/lasp/dsp/lasp_biquadbank.cpp b/src/lasp/dsp/lasp_biquadbank.cpp index e69de29..8d33d76 100644 --- a/src/lasp/dsp/lasp_biquadbank.cpp +++ b/src/lasp/dsp/lasp_biquadbank.cpp @@ -0,0 +1,108 @@ +#include +#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> 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]; + } +} diff --git a/src/lasp/dsp/lasp_biquadbank.h b/src/lasp/dsp/lasp_biquadbank.h index e69de29..efdba4e 100644 --- a/src/lasp/dsp/lasp_biquadbank.h +++ b/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 _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; + +}; diff --git a/src/lasp/dsp/lasp_filter.cpp b/src/lasp/dsp/lasp_filter.cpp index e69de29..a714238 100644 --- a/src/lasp/dsp/lasp_filter.cpp +++ b/src/lasp/dsp/lasp_filter.cpp @@ -0,0 +1,4 @@ +#include "lasp_filter.h" + + + diff --git a/src/lasp/dsp/lasp_filter.h b/src/lasp/dsp/lasp_filter.h index 1c1c48d..e59e739 100644 --- a/src/lasp/dsp/lasp_filter.h +++ b/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() {} diff --git a/src/lasp/dsp/lasp_thread.cpp b/src/lasp/dsp/lasp_thread.cpp new file mode 100644 index 0000000..1dc4a3e --- /dev/null +++ b/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; + +} diff --git a/src/lasp/dsp/lasp_thread.h b/src/lasp/dsp/lasp_thread.h new file mode 100644 index 0000000..7a136bd --- /dev/null +++ b/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(); diff --git a/src/lasp/dsp/lasp_timebuffer.cpp b/src/lasp/dsp/lasp_timebuffer.cpp new file mode 100644 index 0000000..ac1bfc2 --- /dev/null +++ b/src/lasp/dsp/lasp_timebuffer.cpp @@ -0,0 +1,82 @@ +#include "lasp_timebuffer.h" +#include +#include +#include +#include +#include +#include + +using std::runtime_error; + +class TimeBufferImp { + /** + * @brief Storage in a double-ended queue of armadillo row vectors. + */ + std::deque _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 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()) {} +std::optional 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); } diff --git a/src/lasp/dsp/lasp_timebuffer.h b/src/lasp/dsp/lasp_timebuffer.h new file mode 100644 index 0000000..baa4f07 --- /dev/null +++ b/src/lasp/dsp/lasp_timebuffer.h @@ -0,0 +1,40 @@ +#pragma once +#include "lasp_mathtypes.h" +#include +#include + +class TimeBufferImp; +/** + * @brief Implementation of a buffer of time samples, where + */ +class TimeBuffer { + std::unique_ptr _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 pop(const us nframes, const us keep = 0); +}; diff --git a/src/lasp/pybind11/lasp_pyindatahandler.cpp b/src/lasp/pybind11/lasp_pyindatahandler.cpp index b47ed26..2a36d66 100644 --- a/src/lasp/pybind11/lasp_pyindatahandler.cpp +++ b/src/lasp/pybind11/lasp_pyindatahandler.cpp @@ -1,6 +1,7 @@ #include "debugtrace.hpp" #include "lasp_streammgr.h" #include +#include #include #include #include diff --git a/third_party/thread-pool b/third_party/thread-pool new file mode 160000 index 0000000..67fad04 --- /dev/null +++ b/third_party/thread-pool @@ -0,0 +1 @@ +Subproject commit 67fad04348b91cf93bdfad7495d298f54825602c