Chris@366: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
Chris@366: /*
Chris@366:     Constant-Q library
Chris@366:     Copyright (c) 2013-2014 Queen Mary, University of London
Chris@366: 
Chris@366:     Permission is hereby granted, free of charge, to any person
Chris@366:     obtaining a copy of this software and associated documentation
Chris@366:     files (the "Software"), to deal in the Software without
Chris@366:     restriction, including without limitation the rights to use, copy,
Chris@366:     modify, merge, publish, distribute, sublicense, and/or sell copies
Chris@366:     of the Software, and to permit persons to whom the Software is
Chris@366:     furnished to do so, subject to the following conditions:
Chris@366: 
Chris@366:     The above copyright notice and this permission notice shall be
Chris@366:     included in all copies or substantial portions of the Software.
Chris@366: 
Chris@366:     THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
Chris@366:     EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
Chris@366:     MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
Chris@366:     NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
Chris@366:     CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
Chris@366:     CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
Chris@366:     WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
Chris@366: 
Chris@366:     Except as contained in this notice, the names of the Centre for
Chris@366:     Digital Music; Queen Mary, University of London; and Chris Cannam
Chris@366:     shall not be used in advertising or otherwise to promote the sale,
Chris@366:     use or other dealings in this Software without prior written
Chris@366:     authorization.
Chris@366: */
Chris@366: 
Chris@366: #include "CQKernel.h"
Chris@366: 
Chris@366: #include "dsp/MathUtilities.h"
Chris@366: #include "dsp/FFT.h"
Chris@366: #include "dsp/Window.h"
Chris@366: 
Chris@366: #include <cmath>
Chris@366: #include <cassert>
Chris@366: #include <vector>
Chris@366: #include <iostream>
Chris@366: #include <algorithm>
Chris@366: 
Chris@366: #include <cmath>
Chris@366: 
Chris@366: using std::vector;
Chris@366: using std::complex;
Chris@366: using std::cerr;
Chris@366: using std::endl;
Chris@366: 
Chris@366: typedef std::complex<double> C;
Chris@366: 
Chris@366: //#define DEBUG_CQ_KERNEL 1
Chris@366: 
Chris@366: CQKernel::CQKernel(CQParameters params) :
Chris@366:     m_inparams(params),
Chris@366:     m_valid(false),
Chris@366:     m_fft(0)
Chris@366: {
Chris@366:     m_p.sampleRate = params.sampleRate;
Chris@366:     m_p.maxFrequency = params.maxFrequency;
Chris@366:     m_p.binsPerOctave = params.binsPerOctave;
Chris@366:     m_valid = generateKernel();
Chris@366: }
Chris@366: 
Chris@366: CQKernel::~CQKernel()
Chris@366: {
Chris@366:     delete m_fft;
Chris@366: }
Chris@366: 
Chris@366: vector<double>
Chris@366: CQKernel::makeWindow(int len) const
Chris@366: {
Chris@366:     // The MATLAB version uses a symmetric window, but our windows
Chris@366:     // are periodic. A symmetric window of size N is a periodic
Chris@366:     // one of size N-1 with the first element stuck on the end.
Chris@366: 
Chris@366:     WindowType wt(BlackmanHarrisWindow);
Chris@366: 
Chris@366:     switch (m_inparams.window) {
Chris@366:     case CQParameters::SqrtBlackmanHarris:
Chris@366:     case CQParameters::BlackmanHarris:
Chris@366:         wt = BlackmanHarrisWindow;
Chris@366:         break;
Chris@366:     case CQParameters::SqrtBlackman:
Chris@366:     case CQParameters::Blackman:
Chris@366:         wt = BlackmanWindow;
Chris@366:         break;
Chris@366:     case CQParameters::SqrtHann:
Chris@366:     case CQParameters::Hann:
Chris@366:         wt = HanningWindow;
Chris@366:         break;
Chris@366:     }
Chris@366: 
Chris@366:     Window<double> w(wt, len-1);
Chris@366:     vector<double> win = w.getWindowData();
Chris@366:     win.push_back(win[0]);
Chris@366: 
Chris@366:     switch (m_inparams.window) {
Chris@366:     case CQParameters::SqrtBlackmanHarris:
Chris@366:     case CQParameters::SqrtBlackman:
Chris@366:     case CQParameters::SqrtHann:
Chris@366:         for (int i = 0; i < (int)win.size(); ++i) {
Chris@366:             win[i] = sqrt(win[i]) / len;
Chris@366:         }
Chris@366:         break;
Chris@366:     case CQParameters::BlackmanHarris:
Chris@366:     case CQParameters::Blackman:
Chris@366:     case CQParameters::Hann:
Chris@366:         for (int i = 0; i < (int)win.size(); ++i) {
Chris@366:             win[i] = win[i] / len;
Chris@366:         }
Chris@366:         break;
Chris@366:     }
Chris@366: 
Chris@366:     return win;
Chris@366: }
Chris@366: 
Chris@366: bool
Chris@366: CQKernel::generateKernel()
Chris@366: {
Chris@366:     double q = m_inparams.q;
Chris@366:     double atomHopFactor = m_inparams.atomHopFactor;
Chris@366:     double thresh = m_inparams.threshold;
Chris@366: 
Chris@366:     double bpo = m_p.binsPerOctave;
Chris@366: 
Chris@366:     m_p.minFrequency = (m_p.maxFrequency / 2) * pow(2, 1.0/bpo);
Chris@366:     m_p.Q = q / (pow(2, 1.0/bpo) - 1.0);
Chris@366: 
Chris@366:     double maxNK = int(m_p.Q * m_p.sampleRate / m_p.minFrequency + 0.5);
Chris@366:     double minNK = int
Chris@366:         (m_p.Q * m_p.sampleRate /
Chris@366:          (m_p.minFrequency * pow(2, (bpo - 1.0) / bpo)) + 0.5);
Chris@366: 
Chris@366:     if (minNK == 0 || maxNK == 0) {
Chris@366:         // most likely pathological parameters of some sort
Chris@366:         cerr << "WARNING: CQKernel::generateKernel: minNK or maxNK is zero (minNK == " << minNK << ", maxNK == " << maxNK << "), not generating a kernel" << endl;
Chris@366:         m_p.atomSpacing = 0;
Chris@366:         m_p.firstCentre = 0;
Chris@366:         m_p.fftSize = 0;
Chris@366:         m_p.atomsPerFrame = 0;
Chris@366:         m_p.lastCentre = 0;
Chris@366:         m_p.fftHop = 0;
Chris@366:         return false;
Chris@366:     }
Chris@366: 
Chris@366:     m_p.atomSpacing = int(minNK * atomHopFactor + 0.5);
Chris@366:     m_p.firstCentre = m_p.atomSpacing * ceil(ceil(maxNK / 2.0) / m_p.atomSpacing);
Chris@366:     m_p.fftSize = MathUtilities::nextPowerOfTwo
Chris@366:         (m_p.firstCentre + ceil(maxNK / 2.0));
Chris@366: 
Chris@366:     m_p.atomsPerFrame = floor
Chris@366:         (1.0 + (m_p.fftSize - ceil(maxNK / 2.0) - m_p.firstCentre) / m_p.atomSpacing);
Chris@366: 
Chris@366: #ifdef DEBUG_CQ_KERNEL
Chris@366:     cerr << "atomsPerFrame = " << m_p.atomsPerFrame << " (q = " << q << ", Q = " << m_p.Q << ", atomHopFactor = " << atomHopFactor << ", atomSpacing = " << m_p.atomSpacing << ", fftSize = " << m_p.fftSize << ", maxNK = " << maxNK << ", firstCentre = " << m_p.firstCentre << ")" << endl;
Chris@366: #endif
Chris@366: 
Chris@366:     m_p.lastCentre = m_p.firstCentre + (m_p.atomsPerFrame - 1) * m_p.atomSpacing;
Chris@366: 
Chris@366:     m_p.fftHop = (m_p.lastCentre + m_p.atomSpacing) - m_p.firstCentre;
Chris@366: 
Chris@366: #ifdef DEBUG_CQ_KERNEL
Chris@366:     cerr << "fftHop = " << m_p.fftHop << endl;
Chris@366: #endif
Chris@366: 
Chris@366:     m_fft = new FFT(m_p.fftSize);
Chris@366: 
Chris@366:     for (int k = 1; k <= m_p.binsPerOctave; ++k) {
Chris@366:         
Chris@366:         int nk = int(m_p.Q * m_p.sampleRate /
Chris@366:                      (m_p.minFrequency * pow(2, ((k-1.0) / bpo))) + 0.5);
Chris@366: 
Chris@366:         vector<double> win = makeWindow(nk);
Chris@366: 
Chris@366:         double fk = m_p.minFrequency * pow(2, ((k-1.0) / bpo));
Chris@366: 
Chris@366:         vector<double> reals, imags;
Chris@366:         
Chris@366:         for (int i = 0; i < nk; ++i) {
Chris@366:             double arg = (2.0 * M_PI * fk * i) / m_p.sampleRate;
Chris@366:             reals.push_back(win[i] * cos(arg));
Chris@366:             imags.push_back(win[i] * sin(arg));
Chris@366:         }
Chris@366: 
Chris@366:         int atomOffset = m_p.firstCentre - int(ceil(nk/2.0));
Chris@366: 
Chris@366:         for (int i = 0; i < m_p.atomsPerFrame; ++i) {
Chris@366: 
Chris@366:             int shift = atomOffset + (i * m_p.atomSpacing);
Chris@366: 
Chris@366:             vector<double> rin(m_p.fftSize, 0.0);
Chris@366:             vector<double> iin(m_p.fftSize, 0.0);
Chris@366: 
Chris@366:             for (int j = 0; j < nk; ++j) {
Chris@366:                 rin[j + shift] = reals[j];
Chris@366:                 iin[j + shift] = imags[j];
Chris@366:             }
Chris@366: 
Chris@366:             vector<double> rout(m_p.fftSize, 0.0);
Chris@366:             vector<double> iout(m_p.fftSize, 0.0);
Chris@366: 
Chris@366:             m_fft->process(false,
Chris@366:                            rin.data(), iin.data(),
Chris@366:                            rout.data(), iout.data());
Chris@366: 
Chris@366:             // Keep this dense for the moment (until after
Chris@366:             // normalisation calculations)
Chris@366: 
Chris@366:             vector<C> row;
Chris@366: 
Chris@366:             for (int j = 0; j < m_p.fftSize; ++j) {
Chris@366:                 if (sqrt(rout[j] * rout[j] + iout[j] * iout[j]) < thresh) {
Chris@366:                     row.push_back(C(0, 0));
Chris@366:                 } else {
Chris@366:                     row.push_back(C(rout[j] / m_p.fftSize,
Chris@366:                                     iout[j] / m_p.fftSize));
Chris@366:                 }
Chris@366:             }
Chris@366: 
Chris@366:             m_kernel.origin.push_back(0);
Chris@366:             m_kernel.data.push_back(row);
Chris@366:         }
Chris@366:     }
Chris@366: 
Chris@366:     assert((int)m_kernel.data.size() == m_p.binsPerOctave * m_p.atomsPerFrame);
Chris@366: 
Chris@366:     // print density as diagnostic
Chris@366: 
Chris@366:     int nnz = 0;
Chris@366:     for (int i = 0; i < (int)m_kernel.data.size(); ++i) {
Chris@366:         for (int j = 0; j < (int)m_kernel.data[i].size(); ++j) {
Chris@366:             if (m_kernel.data[i][j] != C(0, 0)) {
Chris@366:                 ++nnz;
Chris@366:             }
Chris@366:         }
Chris@366:     }
Chris@366: 
Chris@366: #ifdef DEBUG_CQ_KERNEL
Chris@366:     cerr << "size = " << m_kernel.data.size() << "*" << m_kernel.data[0].size() << " (fft size = " << m_p.fftSize << ")" << endl;
Chris@366: #endif
Chris@366: 
Chris@366:     assert((int)m_kernel.data.size() == m_p.binsPerOctave * m_p.atomsPerFrame);
Chris@366:     assert((int)m_kernel.data[0].size() == m_p.fftSize);
Chris@366: 
Chris@366: #ifdef DEBUG_CQ_KERNEL
Chris@366:     cerr << "density = " << double(nnz) / double(m_p.binsPerOctave * m_p.atomsPerFrame * m_p.fftSize) << " (" << nnz << " of " << m_p.binsPerOctave * m_p.atomsPerFrame * m_p.fftSize << ")" << endl;
Chris@366: #endif
Chris@366: 
Chris@366:     finaliseKernel();
Chris@366:     return true;
Chris@366: }
Chris@366: 
Chris@366: static bool ccomparator(C &c1, C &c2)
Chris@366: {
Chris@366:     return abs(c1) < abs(c2);
Chris@366: }
Chris@366: 
Chris@366: static int maxidx(vector<C> &v)
Chris@366: {
Chris@366:     return std::max_element(v.begin(), v.end(), ccomparator) - v.begin();
Chris@366: }
Chris@366: 
Chris@366: void
Chris@366: CQKernel::finaliseKernel()
Chris@366: {
Chris@366:     // calculate weight for normalisation
Chris@366: 
Chris@366:     int wx1 = maxidx(m_kernel.data[0]);
Chris@366:     int wx2 = maxidx(m_kernel.data[m_kernel.data.size()-1]);
Chris@366: 
Chris@366:     vector<vector<C> > subset(m_kernel.data.size());
Chris@366:     for (int j = wx1; j <= wx2; ++j) {
Chris@366:         for (int i = 0; i < (int)m_kernel.data.size(); ++i) {
Chris@366:             subset[i].push_back(m_kernel.data[i][j]);
Chris@366:         }
Chris@366:     }
Chris@366: 
Chris@366:     int nrows = subset.size();
Chris@366:     int ncols = subset[0].size();
Chris@366:     vector<vector<C> > square(ncols); // conjugate transpose of subset * subset
Chris@366: 
Chris@366:     for (int i = 0; i < nrows; ++i) {
Chris@366:         assert((int)subset[i].size() == ncols);
Chris@366:     }
Chris@366: 
Chris@366:     for (int j = 0; j < ncols; ++j) {
Chris@366:         for (int i = 0; i < ncols; ++i) {
Chris@366:             C v(0, 0);
Chris@366:             for (int k = 0; k < nrows; ++k) {
Chris@366:                 v += subset[k][i] * conj(subset[k][j]);
Chris@366:             }
Chris@366:             square[i].push_back(v);
Chris@366:         }
Chris@366:     }
Chris@366: 
Chris@366:     vector<double> wK;
Chris@366:     double q = m_inparams.q;
Chris@366:     for (int i = int(1.0/q + 0.5); i < ncols - int(1.0/q + 0.5) - 2; ++i) {
Chris@366:         wK.push_back(abs(square[i][i]));
Chris@366:     }
Chris@366: 
Chris@366:     double weight = double(m_p.fftHop) / m_p.fftSize;
Chris@366:     if (!wK.empty()) {
Chris@366:         weight /= MathUtilities::mean(wK.data(), wK.size());
Chris@366:     }
Chris@366:     weight = sqrt(weight);
Chris@366: 
Chris@366: #ifdef DEBUG_CQ_KERNEL    
Chris@366:     cerr << "weight = " << weight << " (from " << wK.size() << " elements in wK, ncols = " << ncols << ", q = " << q << ")" << endl;
Chris@366: #endif
Chris@366: 
Chris@366:     // apply normalisation weight, make sparse, and store conjugate
Chris@366:     // (we use the adjoint or conjugate transpose of the kernel matrix
Chris@366:     // for the forward transform, the plain kernel for the inverse
Chris@366:     // which we expect to be less common)
Chris@366: 
Chris@366:     KernelMatrix sk;
Chris@366: 
Chris@366:     for (int i = 0; i < (int)m_kernel.data.size(); ++i) {
Chris@366: 
Chris@366:         sk.origin.push_back(0);
Chris@366:         sk.data.push_back(vector<C>());
Chris@366: 
Chris@366:         int lastNZ = 0;
Chris@366:         for (int j = (int)m_kernel.data[i].size()-1; j >= 0; --j) {
Chris@366:             if (abs(m_kernel.data[i][j]) != 0.0) {
Chris@366:                 lastNZ = j;
Chris@366:                 break;
Chris@366:             }
Chris@366:         }
Chris@366: 
Chris@366:         bool haveNZ = false;
Chris@366:         for (int j = 0; j <= lastNZ; ++j) {
Chris@366:             if (haveNZ || abs(m_kernel.data[i][j]) != 0.0) {
Chris@366:                 if (!haveNZ) sk.origin[i] = j;
Chris@366:                 haveNZ = true;
Chris@366:                 sk.data[i].push_back(conj(m_kernel.data[i][j]) * weight);
Chris@366:             }
Chris@366:         }
Chris@366:     }
Chris@366: 
Chris@366:     m_kernel = sk;
Chris@366: }
Chris@366: 
Chris@366: vector<C>
Chris@366: CQKernel::processForward(const vector<C> &cv)
Chris@366: {
Chris@366:     // straightforward matrix multiply (taking into account m_kernel's
Chris@366:     // slightly-sparse representation)
Chris@366: 
Chris@366:     if (m_kernel.data.empty()) return vector<C>();
Chris@366: 
Chris@366:     int nrows = m_p.binsPerOctave * m_p.atomsPerFrame;
Chris@366: 
Chris@366:     vector<C> rv(nrows, C());
Chris@366: 
Chris@366:     for (int i = 0; i < nrows; ++i) {
Chris@366:         int len = m_kernel.data[i].size();
Chris@366:         for (int j = 0; j < len; ++j) {
Chris@366:             rv[i] += cv[j + m_kernel.origin[i]] * m_kernel.data[i][j];
Chris@366:         }
Chris@366:     }
Chris@366: 
Chris@366:     return rv;
Chris@366: }
Chris@366: 
Chris@366: vector<C>
Chris@366: CQKernel::processInverse(const vector<C> &cv)
Chris@366: {
Chris@366:     // matrix multiply by conjugate transpose of m_kernel. This is
Chris@366:     // actually the original kernel as calculated, we just stored the
Chris@366:     // conjugate-transpose of the kernel because we expect to be doing
Chris@366:     // more forward transforms than inverse ones.
Chris@366: 
Chris@366:     if (m_kernel.data.empty()) return vector<C>();
Chris@366: 
Chris@366:     int ncols = m_p.binsPerOctave * m_p.atomsPerFrame;
Chris@366:     int nrows = m_p.fftSize;
Chris@366: 
Chris@366:     vector<C> rv(nrows, C());
Chris@366: 
Chris@366:     for (int j = 0; j < ncols; ++j) {
Chris@366:         int i0 = m_kernel.origin[j];
Chris@366:         int i1 = i0 + m_kernel.data[j].size();
Chris@366:         for (int i = i0; i < i1; ++i) {
Chris@366:             rv[i] += cv[j] * conj(m_kernel.data[j][i - i0]);
Chris@366:         }
Chris@366:     }
Chris@366: 
Chris@366:     return rv;
Chris@366: }
Chris@366: 
Chris@366: