view cpp-qm-dsp/CQKernel.cpp @ 69:27007f8302f4

Copyrights, licence

/* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
/*
    Constant-Q library
    Copyright (c) 2013-2014 Queen Mary, University of London

    Permission is hereby granted, free of charge, to any person
    obtaining a copy of this software and associated documentation
    files (the "Software"), to deal in the Software without
    restriction, including without limitation the rights to use, copy,
    modify, merge, publish, distribute, sublicense, and/or sell copies
    of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be
    included in all copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
    EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
    MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
    NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
    CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF
    CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
    WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

    Except as contained in this notice, the names of the Centre for
    Digital Music; Queen Mary, University of London; and Chris Cannam
    shall not be used in advertising or otherwise to promote the sale,
    use or other dealings in this Software without prior written
    authorization.
*/

#include "CQKernel.h"

#include "maths/MathUtilities.h"
#include "dsp/transforms/FFT.h"
#include "base/Window.h"

#include <cmath>
#include <cassert>
#include <vector>
#include <iostream>
#include <algorithm>

using std::vector;
using std::complex;
using std::cerr;
using std::endl;

typedef std::complex<double> C;

CQKernel::CQKernel(double sampleRate, double maxFreq, int binsPerOctave) :
    m_fft(0)
{
    m_p.sampleRate = sampleRate;
    m_p.maxFrequency = maxFreq;
    m_p.binsPerOctave = binsPerOctave;
    generateKernel();
}

CQKernel::~CQKernel()
{
    delete m_fft;
}

void
CQKernel::generateKernel()
{
    double q = 1;
    double atomHopFactor = 0.25;
    double thresh = 0.0005;

    double bpo = m_p.binsPerOctave;

    m_p.minFrequency = (m_p.maxFrequency / 2) * pow(2, 1.0/bpo);
    m_p.Q = q / (pow(2, 1.0/bpo) - 1.0);

    double maxNK = round(m_p.Q * m_p.sampleRate / m_p.minFrequency);
    double minNK = round
        (m_p.Q * m_p.sampleRate /
         (m_p.minFrequency * pow(2, (bpo - 1.0) / bpo)));

    m_p.atomSpacing = round(minNK * atomHopFactor);
    m_p.firstCentre = m_p.atomSpacing * ceil(ceil(maxNK / 2.0) / m_p.atomSpacing);
    m_p.fftSize = MathUtilities::nextPowerOfTwo
        (m_p.firstCentre + ceil(maxNK / 2.0));

    m_p.atomsPerFrame = floor
        (1.0 + (m_p.fftSize - ceil(maxNK / 2.0) - m_p.firstCentre) / m_p.atomSpacing);

    cerr << "atomsPerFrame = " << m_p.atomsPerFrame << " (atomHopFactor = " << atomHopFactor << ")" << endl;

    m_p.lastCentre = m_p.firstCentre + (m_p.atomsPerFrame - 1) * m_p.atomSpacing;

    m_p.fftHop = (m_p.lastCentre + m_p.atomSpacing) - m_p.firstCentre;

    cerr << "fftHop = " << m_p.fftHop << endl;

    m_fft = new FFT(m_p.fftSize);

    for (int k = 1; k <= m_p.binsPerOctave; ++k) {
        
        int nk = round(m_p.Q * m_p.sampleRate /
                       (m_p.minFrequency * pow(2, ((k-1.0) / bpo))));

        // The MATLAB version uses a symmetric window, but our windows
        // are periodic. A symmetric window of size N is a periodic
        // one of size N-1 with the first element stuck on the end
        Window<double> w(BlackmanHarrisWindow, nk-1);
        vector<double> win = w.getWindowData();
        win.push_back(win[0]);

        for (int i = 0; i < (int)win.size(); ++i) {
            win[i] = sqrt(win[i]) / nk;
        }

        double fk = m_p.minFrequency * pow(2, ((k-1.0) / bpo));

        vector<double> reals, imags;
        
        for (int i = 0; i < nk; ++i) {
            double arg = (2.0 * M_PI * fk * i) / m_p.sampleRate;
            reals.push_back(win[i] * cos(arg));
            imags.push_back(win[i] * sin(arg));
        }

        int atomOffset = m_p.firstCentre - int(ceil(nk/2.0));

        for (int i = 0; i < m_p.atomsPerFrame; ++i) {

            int shift = atomOffset + (i * m_p.atomSpacing);

            vector<double> rin(m_p.fftSize, 0.0);
            vector<double> iin(m_p.fftSize, 0.0);

            for (int j = 0; j < nk; ++j) {
                rin[j + shift] = reals[j];
                iin[j + shift] = imags[j];
            }

            vector<double> rout(m_p.fftSize, 0.0);
            vector<double> iout(m_p.fftSize, 0.0);

            m_fft->process(false,
                           rin.data(), iin.data(),
                           rout.data(), iout.data());

            // Keep this dense for the moment (until after
            // normalisation calculations)

            vector<C> row;

            for (int j = 0; j < m_p.fftSize; ++j) {
                if (sqrt(rout[j] * rout[j] + iout[j] * iout[j]) < thresh) {
                    row.push_back(C(0, 0));
                } else {
                    row.push_back(C(rout[j] / m_p.fftSize,
                                    iout[j] / m_p.fftSize));
                }
            }

            m_kernel.origin.push_back(0);
            m_kernel.data.push_back(row);
        }
    }

    assert((int)m_kernel.data.size() == m_p.binsPerOctave * m_p.atomsPerFrame);

    // print density as diagnostic

    int nnz = 0;
    for (int i = 0; i < (int)m_kernel.data.size(); ++i) {
        for (int j = 0; j < (int)m_kernel.data[i].size(); ++j) {
            if (m_kernel.data[i][j] != C(0, 0)) {
                ++nnz;
            }
        }
    }

    cerr << "size = " << m_kernel.data.size() << "*" << m_kernel.data[0].size() << " (fft size = " << m_p.fftSize << ")" << endl;

    assert((int)m_kernel.data.size() == m_p.binsPerOctave * m_p.atomsPerFrame);
    assert((int)m_kernel.data[0].size() == m_p.fftSize);

    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;

    finaliseKernel();
}

static bool ccomparator(C &c1, C &c2)
{
    return abs(c1) < abs(c2);
}

static int maxidx(vector<C> &v)
{
    return std::max_element(v.begin(), v.end(), ccomparator) - v.begin();
}

void
CQKernel::finaliseKernel()
{
    // calculate weight for normalisation

    int wx1 = maxidx(m_kernel.data[0]);
    int wx2 = maxidx(m_kernel.data[m_kernel.data.size()-1]);

    vector<vector<C> > subset(m_kernel.data.size());
    for (int j = wx1; j <= wx2; ++j) {
        for (int i = 0; i < (int)m_kernel.data.size(); ++i) {
            subset[i].push_back(m_kernel.data[i][j]);
        }
    }

    int nrows = subset.size();
    int ncols = subset[0].size();
    vector<vector<C> > square(ncols); // conjugate transpose of subset * subset

    for (int i = 0; i < nrows; ++i) {
        assert((int)subset[i].size() == ncols);
    }

    for (int j = 0; j < ncols; ++j) {
        for (int i = 0; i < ncols; ++i) {
            C v(0, 0);
            for (int k = 0; k < nrows; ++k) {
                v += subset[k][i] * conj(subset[k][j]);
            }
            square[i].push_back(v);
        }
    }

    vector<double> wK;
    double q = 1; //!!! duplicated from constructor
    for (int i = round(1.0/q); i < ncols - round(1.0/q) - 2; ++i) {
        wK.push_back(abs(square[i][i]));
    }

    double weight = double(m_p.fftHop) / m_p.fftSize;
    weight /= MathUtilities::mean(wK.data(), wK.size());
    weight = sqrt(weight);
    
    cerr << "weight = " << weight << endl;

    // apply normalisation weight, make sparse, and store conjugates
    // (our multiplication order means we will effectively be using
    // the adjoint or conjugate transpose of the kernel matrix)

    KernelMatrix sk;

    for (int i = 0; i < (int)m_kernel.data.size(); ++i) {

        sk.origin.push_back(0);
        sk.data.push_back(vector<C>());

        int lastNZ = 0;
        for (int j = (int)m_kernel.data[i].size()-1; j >= 0; --j) {
            if (abs(m_kernel.data[i][j]) != 0.0) {
                lastNZ = j;
                break;
            }
        }

        bool haveNZ = false;
        for (int j = 0; j <= lastNZ; ++j) {
            if (haveNZ || abs(m_kernel.data[i][j]) != 0.0) {
                if (!haveNZ) sk.origin[i] = j;
                haveNZ = true;
                sk.data[i].push_back(conj(m_kernel.data[i][j]) * weight);
            }
        }
    }

    m_kernel = sk;
}

vector<C>
CQKernel::process(const vector<C> &cv)
{
    // matrix multiply m_kernel.data by in, converting in to complex
    // as we go

    int nrows = m_p.binsPerOctave * m_p.atomsPerFrame;

    vector<C> rv(nrows, C(0, 0));

    for (int i = 0; i < nrows; ++i) {
        for (int j = 0; j < (int)m_kernel.data[i].size(); ++j) {
            rv[i] += cv[j + m_kernel.origin[i]] * m_kernel.data[i][j];
        }
    }

    return rv;
}