libs/qm-dsp/dsp/chromagram/ConstantQ.cpp

   1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
   2 /*
   3     QM DSP Library
   4
   5     Centre for Digital Music, Queen Mary, University of London.
   6     This file 2005-2006 Christian Landone.
   7
   8     This program is free software; you can redistribute it and/or
   9     modify it under the terms of the GNU General Public License as
  10     published by the Free Software Foundation; either version 2 of the
  11     License, or (at your option) any later version.  See the file
  12     COPYING included with this distribution for more information.
  13 */
  14
  15 #include "ConstantQ.h"
  16 #include "dsp/transforms/FFT.h"
  17
  18 #include <iostream>
  19
  20 #ifdef NOT_DEFINED
  21 // see note in CQprecalc
  22
  23 #include "CQprecalc.cpp"
  24
  25 static bool push_precalculated(int uk, int fftlength,
  26                                std::vector<unsigned> &is,
  27                                std::vector<unsigned> &js,
  28                                std::vector<double> &real,
  29                                std::vector<double> &imag)
  30 {
  31     if (uk == 76 && fftlength == 16384) {
  32         push_76_16384(is, js, real, imag);
  33         return true;
  34     }
  35     if (uk == 144 && fftlength == 4096) {
  36         push_144_4096(is, js, real, imag);
  37         return true;
  38     }
  39     if (uk == 65 && fftlength == 2048) {
  40         push_65_2048(is, js, real, imag);
  41         return true;
  42     }
  43     if (uk == 84 && fftlength == 65536) {
  44         push_84_65536(is, js, real, imag);
  45         return true;
  46     }
  47     return false;
  48 }
  49 #endif
  50
  51 //---------------------------------------------------------------------------
  52 // nextpow2 returns the smallest integer n such that 2^n >= x.
  53 static double nextpow2(double x) {
  54     double y = ceil(log(x)/log(2.0));
  55     return(y);
  56 }
  57
  58 static double squaredModule(const double & xx, const double & yy) {
  59     return xx*xx + yy*yy;
  60 }
  61
  62 //----------------------------------------------------------------------------
  63
  64 ConstantQ::ConstantQ( CQConfig Config ) :
  65     m_sparseKernel(0)
  66 {
  67     initialise( Config );
  68 }
  69
  70 ConstantQ::~ConstantQ()
  71 {
  72     deInitialise();
  73 }
  74
  75 //----------------------------------------------------------------------------
  76 void ConstantQ::sparsekernel()
  77 {
  78 //    std::cerr << "ConstantQ: initialising sparse kernel, uK = " << m_uK << ", FFTLength = " << m_FFTLength << "...";
  79
  80     SparseKernel *sk = new SparseKernel();
  81
  82 #ifdef NOT_DEFINED
  83     if (push_precalculated(m_uK, m_FFTLength,
  84                            sk->is, sk->js, sk->real, sk->imag)) {
  85 //        std::cerr << "using precalculated kernel" << std::endl;
  86         m_sparseKernel = sk;
  87         return;
  88     }
  89 #endif
  90
  91     //generates spectral kernel matrix (upside down?)
  92     // initialise temporal kernel with zeros, twice length to deal w. complex numbers
  93
  94     double* hammingWindowRe = new double [ m_FFTLength ];
  95     double* hammingWindowIm = new double [ m_FFTLength ];
  96     double* transfHammingWindowRe = new double [ m_FFTLength ];
  97     double* transfHammingWindowIm = new double [ m_FFTLength ];
  98
  99     for (unsigned u=0; u < m_FFTLength; u++)
 100     {
 101         hammingWindowRe[u] = 0;
 102         hammingWindowIm[u] = 0;
 103     }
 104
 105     // Here, fftleng*2 is a guess of the number of sparse cells in the matrix
 106     // The matrix K x fftlength but the non-zero cells are an antialiased
 107     // square root function. So mostly is a line, with some grey point.
 108     sk->is.reserve( m_FFTLength*2 );
 109     sk->js.reserve( m_FFTLength*2 );
 110     sk->real.reserve( m_FFTLength*2 );
 111     sk->imag.reserve( m_FFTLength*2 );
 112
 113     // for each bin value K, calculate temporal kernel, take its fft to
 114     //calculate the spectral kernel then threshold it to make it sparse and
 115     //add it to the sparse kernels matrix
 116     double squareThreshold = m_CQThresh * m_CQThresh;
 117
 118     FFT m_FFT(m_FFTLength);
 119
 120     for (unsigned k = m_uK; k--; )
 121     {
 122         for (unsigned u=0; u < m_FFTLength; u++)
 123         {
 124             hammingWindowRe[u] = 0;
 125             hammingWindowIm[u] = 0;
 126         }
 127
 128         // Computing a hamming window
 129         const unsigned hammingLength = (int) ceil( m_dQ * m_FS / ( m_FMin * pow(2,((double)(k))/(double)m_BPO)));
 130
 131         unsigned origin = m_FFTLength/2 - hammingLength/2;
 132
 133         for (unsigned i=0; i<hammingLength; i++)
 134         {
 135             const double angle = 2*PI*m_dQ*i/hammingLength;
 136             const double real = cos(angle);
 137             const double imag = sin(angle);
 138             const double absol = hamming(hammingLength, i)/hammingLength;
 139             hammingWindowRe[ origin + i ] = absol*real;
 140             hammingWindowIm[ origin + i ] = absol*imag;
 141         }
 142
 143         for (unsigned i = 0; i < m_FFTLength/2; ++i) {
 144             double temp = hammingWindowRe[i];
 145             hammingWindowRe[i] = hammingWindowRe[i + m_FFTLength/2];
 146             hammingWindowRe[i + m_FFTLength/2] = temp;
 147             temp = hammingWindowIm[i];
 148             hammingWindowIm[i] = hammingWindowIm[i + m_FFTLength/2];
 149             hammingWindowIm[i + m_FFTLength/2] = temp;
 150         }
 151
 152         //do fft of hammingWindow
 153         m_FFT.process( 0, hammingWindowRe, hammingWindowIm, transfHammingWindowRe, transfHammingWindowIm );
 154
 155
 156         for (unsigned j=0; j<( m_FFTLength ); j++)
 157         {
 158             // perform thresholding
 159             const double squaredBin = squaredModule( transfHammingWindowRe[ j ], transfHammingWindowIm[ j ]);
 160             if (squaredBin <= squareThreshold) continue;
 161
 162             // Insert non-zero position indexes, doubled because they are floats
 163             sk->is.push_back(j);
 164             sk->js.push_back(k);
 165
 166             // take conjugate, normalise and add to array sparkernel
 167             sk->real.push_back( transfHammingWindowRe[ j ]/m_FFTLength);
 168             sk->imag.push_back(-transfHammingWindowIm[ j ]/m_FFTLength);
 169         }
 170
 171     }
 172
 173     delete [] hammingWindowRe;
 174     delete [] hammingWindowIm;
 175     delete [] transfHammingWindowRe;
 176     delete [] transfHammingWindowIm;
 177
 178 /*
 179     using std::cout;
 180     using std::endl;
 181
 182     cout.precision(28);
 183
 184     int n = sk->is.size();
 185     int w = 8;
 186     cout << "static unsigned int sk_i_" << m_uK << "_" << m_FFTLength << "[" << n << "] = {" << endl;
 187     for (int i = 0; i < n; ++i) {
 188         if (i % w == 0) cout << "    ";
 189         cout << sk->is[i];
 190         if (i + 1 < n) cout << ", ";
 191         if (i % w == w-1) cout << endl;
 192     };
 193     if (n % w != 0) cout << endl;
 194     cout << "};" << endl;
 195
 196     n = sk->js.size();
 197     cout << "static unsigned int sk_j_" << m_uK << "_" << m_FFTLength << "[" << n << "] = {" << endl;
 198     for (int i = 0; i < n; ++i) {
 199         if (i % w == 0) cout << "    ";
 200         cout << sk->js[i];
 201         if (i + 1 < n) cout << ", ";
 202         if (i % w == w-1) cout << endl;
 203     };
 204     if (n % w != 0) cout << endl;
 205     cout << "};" << endl;
 206
 207     w = 2;
 208     n = sk->real.size();
 209     cout << "static double sk_real_" << m_uK << "_" << m_FFTLength << "[" << n << "] = {" << endl;
 210     for (int i = 0; i < n; ++i) {
 211         if (i % w == 0) cout << "    ";
 212         cout << sk->real[i];
 213         if (i + 1 < n) cout << ", ";
 214         if (i % w == w-1) cout << endl;
 215     };
 216     if (n % w != 0) cout << endl;
 217     cout << "};" << endl;
 218
 219     n = sk->imag.size();
 220     cout << "static double sk_imag_" << m_uK << "_" << m_FFTLength << "[" << n << "] = {" << endl;
 221     for (int i = 0; i < n; ++i) {
 222         if (i % w == 0) cout << "    ";
 223         cout << sk->imag[i];
 224         if (i + 1 < n) cout << ", ";
 225         if (i % w == w-1) cout << endl;
 226     };
 227     if (n % w != 0) cout << endl;
 228     cout << "};" << endl;
 229
 230     cout << "static void push_" << m_uK << "_" << m_FFTLength << "(vector<unsigned int> &is, vector<unsigned int> &js, vector<double> &real, vector<double> &imag)" << endl;
 231     cout << "{\n    is.reserve(" << n << ");\n";
 232     cout << "    js.reserve(" << n << ");\n";
 233     cout << "    real.reserve(" << n << ");\n";
 234     cout << "    imag.reserve(" << n << ");\n";
 235     cout << "    for (int i = 0; i < " << n << "; ++i) {" << endl;
 236     cout << "        is.push_back(sk_i_" << m_uK << "_" << m_FFTLength << "[i]);" << endl;
 237     cout << "        js.push_back(sk_j_" << m_uK << "_" << m_FFTLength << "[i]);" << endl;
 238     cout << "        real.push_back(sk_real_" << m_uK << "_" << m_FFTLength << "[i]);" << endl;
 239     cout << "        imag.push_back(sk_imag_" << m_uK << "_" << m_FFTLength << "[i]);" << endl;
 240     cout << "    }" << endl;
 241     cout << "}" << endl;
 242 */
 243 //    std::cerr << "done\n -> is: " << sk->is.size() << ", js: " << sk->js.size() << ", reals: " << sk->real.size() << ", imags: " << sk->imag.size() << std::endl;
 244
 245     m_sparseKernel = sk;
 246     return;
 247 }
 248
 249 //-----------------------------------------------------------------------------
 250 double* ConstantQ::process( const double* fftdata )
 251 {
 252     if (!m_sparseKernel) {
 253         std::cerr << "ERROR: ConstantQ::process: Sparse kernel has not been initialised" << std::endl;
 254         return m_CQdata;
 255     }
 256
 257     SparseKernel *sk = m_sparseKernel;
 258
 259     for (unsigned row=0; row<2*m_uK; row++)
 260     {
 261         m_CQdata[ row ] = 0;
 262         m_CQdata[ row+1 ] = 0;
 263     }
 264     const unsigned *fftbin = &(sk->is[0]);
 265     const unsigned *cqbin  = &(sk->js[0]);
 266     const double   *real   = &(sk->real[0]);
 267     const double   *imag   = &(sk->imag[0]);
 268     const unsigned int sparseCells = sk->real.size();
 269
 270     for (unsigned i = 0; i<sparseCells; i++)
 271     {
 272         const unsigned row = cqbin[i];
 273         const unsigned col = fftbin[i];
 274         const double & r1  = real[i];
 275         const double & i1  = imag[i];
 276         const double & r2  = fftdata[ (2*m_FFTLength) - 2*col - 2 ];
 277         const double & i2  = fftdata[ (2*m_FFTLength) - 2*col - 2 + 1 ];
 278         // add the multiplication
 279         m_CQdata[ 2*row  ] += (r1*r2 - i1*i2);
 280         m_CQdata[ 2*row+1] += (r1*i2 + i1*r2);
 281     }
 282
 283     return m_CQdata;
 284 }
 285
 286
 287 void ConstantQ::initialise( CQConfig Config )
 288 {
 289     m_FS = Config.FS;
 290     m_FMin = Config.min;                // min freq
 291     m_FMax = Config.max;                // max freq
 292     m_BPO = Config.BPO;         // bins per octave
 293     m_CQThresh = Config.CQThresh;// ConstantQ threshold for kernel generation
 294
 295     m_dQ = 1/(pow(2,(1/(double)m_BPO))-1);      // Work out Q value for Filter bank
 296     m_uK = (unsigned int) ceil(m_BPO * log(m_FMax/m_FMin)/log(2.0));    // No. of constant Q bins
 297
 298 //    std::cerr << "ConstantQ::initialise: rate = " << m_FS << ", fmin = " << m_FMin << ", fmax = " << m_FMax << ", bpo = " << m_BPO << ", K = " << m_uK << ", Q = " << m_dQ << std::endl;
 299
 300     // work out length of fft required for this constant Q Filter bank
 301     m_FFTLength = (int) pow(2, nextpow2(ceil( m_dQ*m_FS/m_FMin )));
 302
 303     m_hop = m_FFTLength/8; // <------ hop size is window length divided by 32
 304
 305 //    std::cerr << "ConstantQ::initialise: -> fft length = " << m_FFTLength << ", hop = " << m_hop << std::endl;
 306
 307     // allocate memory for cqdata
 308     m_CQdata = new double [2*m_uK];
 309 }
 310
 311 void ConstantQ::deInitialise()
 312 {
 313     delete [] m_CQdata;
 314     delete m_sparseKernel;
 315 }
 316
 317 void ConstantQ::process(const double *FFTRe, const double* FFTIm,
 318                         double *CQRe, double *CQIm)
 319 {
 320     if (!m_sparseKernel) {
 321         std::cerr << "ERROR: ConstantQ::process: Sparse kernel has not been initialised" << std::endl;
 322         return;
 323     }
 324
 325     SparseKernel *sk = m_sparseKernel;
 326
 327     for (unsigned row=0; row<m_uK; row++)
 328     {
 329         CQRe[ row ] = 0;
 330         CQIm[ row ] = 0;
 331     }
 332
 333     const unsigned *fftbin = &(sk->is[0]);
 334     const unsigned *cqbin  = &(sk->js[0]);
 335     const double   *real   = &(sk->real[0]);
 336     const double   *imag   = &(sk->imag[0]);
 337     const unsigned int sparseCells = sk->real.size();
 338
 339     for (unsigned i = 0; i<sparseCells; i++)
 340     {
 341         const unsigned row = cqbin[i];
 342         const unsigned col = fftbin[i];
 343         const double & r1  = real[i];
 344         const double & i1  = imag[i];
 345         const double & r2  = FFTRe[ m_FFTLength - col - 1 ];
 346         const double & i2  = FFTIm[ m_FFTLength - col - 1 ];
 347         // add the multiplication
 348         CQRe[ row ] += (r1*r2 - i1*i2);
 349         CQIm[ row ] += (r1*i2 + i1*r2);
 350     }
 351 }