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