87 lines
2.4 KiB
C++
87 lines
2.4 KiB
C++
|
// #define DEBUGTRACE_ENABLED
|
||
|
#include "debugtrace.hpp"
|
||
|
#include "lasp_freqsmooth.h"
|
||
|
|
||
|
using rte = std::runtime_error;
|
||
|
|
||
|
vd freqSmooth(const vd& freq, const vd& X, const unsigned w) {
|
||
|
DEBUGTRACE_ENTER;
|
||
|
if (freq.size() < 2) {
|
||
|
throw rte("Invalid frequency size");
|
||
|
}
|
||
|
if (freq.size() != X.size()) {
|
||
|
throw rte("Sizes of freq and X do not match");
|
||
|
}
|
||
|
if (freq.size() > std::numeric_limits<long>::max() / 2) {
|
||
|
throw rte("Frequency size limit for smoothing is 2^30");
|
||
|
}
|
||
|
if (w == 0) {
|
||
|
throw rte("Invalid number of octaves");
|
||
|
}
|
||
|
|
||
|
// Smoothing width in unit of number of octaves
|
||
|
const d Delta = 1 / d(w);
|
||
|
|
||
|
d freq_min;
|
||
|
if (freq(0) == 0) {
|
||
|
freq_min = freq(1);
|
||
|
} else {
|
||
|
freq_min = freq(0);
|
||
|
}
|
||
|
const d freq_max = freq(freq.size() - 1);
|
||
|
DEBUGTRACE_PRINT(freq_min);
|
||
|
DEBUGTRACE_PRINT(freq_max);
|
||
|
const vd freq_log =
|
||
|
arma::logspace(d_log10(freq_min), d_log10(freq_max), 10 * freq.size());
|
||
|
DEBUGTRACE_PRINT("freq_log = ");
|
||
|
// std::cerr << freq_log << std::endl;
|
||
|
|
||
|
const long N = freq_log.size();
|
||
|
|
||
|
// Interpolate X to logscale
|
||
|
vd X_log;
|
||
|
DEBUGTRACE_PRINT("X_log = :");
|
||
|
arma::interp1(freq, X, freq_log, X_log, "*linear");
|
||
|
// Last point is not interpolated well.
|
||
|
X_log(N - 1) = X(X.size() - 1);
|
||
|
// std::cerr << X_log << std::endl;
|
||
|
|
||
|
vd Xsm_log(freq_log.size());
|
||
|
const d beta = d_log10(N) / d_log10(2) / (N - 1);
|
||
|
DEBUGTRACE_PRINT(beta);
|
||
|
|
||
|
const long mu = int(Delta / 2 / beta);
|
||
|
DEBUGTRACE_PRINT(mu);
|
||
|
// Long is at least 32 bits. So +/- 2M points length
|
||
|
for (long k = 0; k < freq_log.size(); k++) {
|
||
|
// const d fcur = freq_log(k);
|
||
|
const us idx_start = std::max(k - mu, 0l);
|
||
|
const us idx_stop = std::min(k + mu, N - 1);
|
||
|
// DEBUGTRACE_PRINT(idx_start)
|
||
|
// DEBUGTRACE_PRINT(idx_stop);
|
||
|
|
||
|
Xsm_log(k) = arma::mean(X_log.subvec(idx_start, idx_stop));
|
||
|
}
|
||
|
DEBUGTRACE_PRINT("Xsm_log:");
|
||
|
// std::cerr << Xsm_log << std::endl;
|
||
|
|
||
|
// Back-interpolate to a linear scale
|
||
|
vd Xsm(freq.size());
|
||
|
if (freq(0) == 0) {
|
||
|
Xsm(0) = X(0);
|
||
|
vd Xsm_gt0;
|
||
|
arma::interp1(freq_log, Xsm_log, freq.subvec(1, freq.size() - 1), Xsm_gt0,
|
||
|
"*linear");
|
||
|
Xsm.subvec(1, freq.size() - 1) = Xsm_gt0;
|
||
|
Xsm(1) = Xsm_log(1);
|
||
|
|
||
|
} else {
|
||
|
Xsm(0) = X(0);
|
||
|
arma::interp1(freq_log, Xsm_log, freq, Xsm, "*linear");
|
||
|
}
|
||
|
Xsm(freq.size() - 1) = Xsm_log(N - 1);
|
||
|
DEBUGTRACE_PRINT("Xsm:");
|
||
|
// std::cerr << Xsm << std::endl;
|
||
|
|
||
|
return Xsm;
|
||
|
}
|