libs/qm-dsp/dsp/tempotracking/TempoTrackV2.cpp

   1 /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
   2
   3 /*
   4     QM DSP Library
   5
   6     Centre for Digital Music, Queen Mary, University of London.
   7     This file copyright 2008-2009 Matthew Davies and QMUL.
   8
   9     This program is free software; you can redistribute it and/or
  10     modify it under the terms of the GNU General Public License as
  11     published by the Free Software Foundation; either version 2 of the
  12     License, or (at your option) any later version.  See the file
  13     COPYING included with this distribution for more information.
  14 */
  15
  16 #include "TempoTrackV2.h"
  17
  18 #include <cmath>
  19 #include <cstdlib>
  20 #include <iostream>
  21
  22 #include "maths/MathUtilities.h"
  23
  24 #define   EPS 0.0000008 // just some arbitrary small number
  25
  26 TempoTrackV2::TempoTrackV2(float rate, size_t increment) :
  27     m_rate(rate), m_increment(increment) { }
  28 TempoTrackV2::~TempoTrackV2() { }
  29
  30 void
  31 TempoTrackV2::filter_df(d_vec_t &df)
  32 {
  33     d_vec_t a(3);
  34     d_vec_t b(3);
  35     d_vec_t     lp_df(df.size());
  36
  37     //equivalent in matlab to [b,a] = butter(2,0.4);
  38     a[0] = 1.0000;
  39     a[1] = -0.3695;
  40     a[2] = 0.1958;
  41     b[0] = 0.2066;
  42     b[1] = 0.4131;
  43     b[2] = 0.2066;
  44
  45     double inp1 = 0.;
  46     double inp2 = 0.;
  47     double out1 = 0.;
  48     double out2 = 0.;
  49
  50
  51     // forwards filtering
  52     for (unsigned int i = 0;i < df.size();i++)
  53     {
  54         lp_df[i] =  b[0]*df[i] + b[1]*inp1 + b[2]*inp2 - a[1]*out1 - a[2]*out2;
  55         inp2 = inp1;
  56         inp1 = df[i];
  57         out2 = out1;
  58         out1 = lp_df[i];
  59     }
  60
  61     // copy forwards filtering to df...
  62     // but, time-reversed, ready for backwards filtering
  63     for (unsigned int i = 0;i < df.size();i++)
  64     {
  65         df[i] = lp_df[df.size()-i-1];
  66     }
  67
  68     for (unsigned int i = 0;i < df.size();i++)
  69     {
  70         lp_df[i] = 0.;
  71     }
  72
  73     inp1 = 0.; inp2 = 0.;
  74     out1 = 0.; out2 = 0.;
  75
  76   // backwards filetering on time-reversed df
  77     for (unsigned int i = 0;i < df.size();i++)
  78     {
  79         lp_df[i] =  b[0]*df[i] + b[1]*inp1 + b[2]*inp2 - a[1]*out1 - a[2]*out2;
  80         inp2 = inp1;
  81         inp1 = df[i];
  82         out2 = out1;
  83         out1 = lp_df[i];
  84     }
  85
  86   // write the re-reversed (i.e. forward) version back to df
  87     for (unsigned int i = 0;i < df.size();i++)
  88     {
  89         df[i] = lp_df[df.size()-i-1];
  90     }
  91 }
  92
  93
  94 void
  95 TempoTrackV2::calculateBeatPeriod(const vector<double> &df,
  96                                   vector<double> &beat_period,
  97                                   vector<double> &tempi)
  98 {
  99     // to follow matlab.. split into 512 sample frames with a 128 hop size
 100     // calculate the acf,
 101     // then the rcf.. and then stick the rcfs as columns of a matrix
 102     // then call viterbi decoding with weight vector and transition matrix
 103     // and get best path
 104
 105     unsigned int wv_len = 128;
 106     double rayparam = 43.;
 107
 108     // make rayleigh weighting curve
 109     d_vec_t wv(wv_len);
 110     for (unsigned int i=0; i<wv.size(); i++)
 111     {
 112         wv[i] = (static_cast<double> (i) / pow(rayparam,2.)) * exp((-1.*pow(-static_cast<double> (i),2.)) / (2.*pow(rayparam,2.)));
 113     }
 114
 115     // beat tracking frame size (roughly 6 seconds) and hop (1.5 seconds)
 116     unsigned int winlen = 512;
 117     unsigned int step = 128;
 118
 119     // matrix to store output of comb filter bank, increment column of matrix at each frame
 120     d_mat_t rcfmat;
 121     int col_counter = -1;
 122
 123     // main loop for beat period calculation
 124     for (unsigned int i=0; i+winlen<df.size(); i+=step)
 125     {
 126         // get dfframe
 127         d_vec_t dfframe(winlen);
 128         for (unsigned int k=0; k<winlen; k++)
 129         {
 130             dfframe[k] = df[i+k];
 131         }
 132         // get rcf vector for current frame
 133         d_vec_t rcf(wv_len);
 134         get_rcf(dfframe,wv,rcf);
 135
 136         rcfmat.push_back( d_vec_t() ); // adds a new column
 137         col_counter++;
 138         for (unsigned int j=0; j<rcf.size(); j++)
 139         {
 140             rcfmat[col_counter].push_back( rcf[j] );
 141         }
 142     }
 143
 144     // now call viterbi decoding function
 145     viterbi_decode(rcfmat,wv,beat_period,tempi);
 146 }
 147
 148
 149 void
 150 TempoTrackV2::get_rcf(const d_vec_t &dfframe_in, const d_vec_t &wv, d_vec_t &rcf)
 151 {
 152     // calculate autocorrelation function
 153     // then rcf
 154     // just hard code for now... don't really need separate functions to do this
 155
 156     // make acf
 157
 158     d_vec_t dfframe(dfframe_in);
 159
 160     MathUtilities::adaptiveThreshold(dfframe);
 161
 162     d_vec_t acf(dfframe.size());
 163
 164
 165     for (unsigned int lag=0; lag<dfframe.size(); lag++)
 166     {
 167         double sum = 0.;
 168         double tmp = 0.;
 169
 170         for (unsigned int n=0; n<(dfframe.size()-lag); n++)
 171         {
 172             tmp = dfframe[n] * dfframe[n+lag];
 173             sum += tmp;
 174         }
 175         acf[lag] = static_cast<double> (sum/ (dfframe.size()-lag));
 176     }
 177
 178     // now apply comb filtering
 179     int numelem = 4;
 180
 181     for (unsigned int i = 2;i < rcf.size();i++) // max beat period
 182     {
 183         for (int a = 1;a <= numelem;a++) // number of comb elements
 184         {
 185             for (int b = 1-a;b <= a-1;b++) // general state using normalisation of comb elements
 186             {
 187                 rcf[i-1] += ( acf[(a*i+b)-1]*wv[i-1] ) / (2.*a-1.);     // calculate value for comb filter row
 188             }
 189         }
 190     }
 191
 192     // apply adaptive threshold to rcf
 193     MathUtilities::adaptiveThreshold(rcf);
 194
 195     double rcfsum =0.;
 196     for (unsigned int i=0; i<rcf.size(); i++)
 197     {
 198         rcf[i] += EPS ;
 199         rcfsum += rcf[i];
 200     }
 201
 202     // normalise rcf to sum to unity
 203     for (unsigned int i=0; i<rcf.size(); i++)
 204     {
 205         rcf[i] /= (rcfsum + EPS);
 206     }
 207 }
 208
 209 void
 210 TempoTrackV2::viterbi_decode(const d_mat_t &rcfmat, const d_vec_t &wv, d_vec_t &beat_period, d_vec_t &tempi)
 211 {
 212     // following Kevin Murphy's Viterbi decoding to get best path of
 213     // beat periods through rfcmat
 214
 215     // make transition matrix
 216     d_mat_t tmat;
 217     for (unsigned int i=0;i<wv.size();i++)
 218     {
 219         tmat.push_back ( d_vec_t() ); // adds a new column
 220         for (unsigned int j=0; j<wv.size(); j++)
 221         {
 222             tmat[i].push_back(0.); // fill with zeros initially
 223         }
 224     }
 225
 226     // variance of Gaussians in transition matrix
 227     // formed of Gaussians on diagonal - implies slow tempo change
 228     double sigma = 8.;
 229     // don't want really short beat periods, or really long ones
 230     for (unsigned int i=20;i <wv.size()-20; i++)
 231     {
 232         for (unsigned int j=20; j<wv.size()-20; j++)
 233         {
 234             double mu = static_cast<double>(i);
 235             tmat[i][j] = exp( (-1.*pow((j-mu),2.)) / (2.*pow(sigma,2.)) );
 236         }
 237     }
 238
 239     // parameters for Viterbi decoding... this part is taken from
 240     // Murphy's matlab
 241
 242     d_mat_t delta;
 243     i_mat_t psi;
 244     for (unsigned int i=0;i <rcfmat.size(); i++)
 245     {
 246         delta.push_back( d_vec_t());
 247         psi.push_back( i_vec_t());
 248         for (unsigned int j=0; j<rcfmat[i].size(); j++)
 249         {
 250             delta[i].push_back(0.); // fill with zeros initially
 251             psi[i].push_back(0); // fill with zeros initially
 252         }
 253     }
 254
 255
 256     unsigned int T = delta.size();
 257
 258     if (T < 2) return; // can't do anything at all meaningful
 259
 260     unsigned int Q = delta[0].size();
 261
 262     // initialize first column of delta
 263     for (unsigned int j=0; j<Q; j++)
 264     {
 265         delta[0][j] = wv[j] * rcfmat[0][j];
 266         psi[0][j] = 0;
 267     }
 268
 269     double deltasum = 0.;
 270     for (unsigned int i=0; i<Q; i++)
 271     {
 272         deltasum += delta[0][i];
 273     }
 274     for (unsigned int i=0; i<Q; i++)
 275     {
 276         delta[0][i] /= (deltasum + EPS);
 277     }
 278
 279
 280     for (unsigned int t=1; t<T; t++)
 281     {
 282         d_vec_t tmp_vec(Q);
 283
 284         for (unsigned int j=0; j<Q; j++)
 285         {
 286             for (unsigned int i=0; i<Q; i++)
 287             {
 288                 tmp_vec[i] = delta[t-1][i] * tmat[j][i];
 289             }
 290
 291             delta[t][j] = get_max_val(tmp_vec);
 292
 293             psi[t][j] = get_max_ind(tmp_vec);
 294
 295             delta[t][j] *= rcfmat[t][j];
 296         }
 297
 298         // normalise current delta column
 299         double deltasum = 0.;
 300         for (unsigned int i=0; i<Q; i++)
 301         {
 302             deltasum += delta[t][i];
 303         }
 304         for (unsigned int i=0; i<Q; i++)
 305         {
 306             delta[t][i] /= (deltasum + EPS);
 307         }
 308     }
 309
 310     i_vec_t bestpath(T);
 311     d_vec_t tmp_vec(Q);
 312     for (unsigned int i=0; i<Q; i++)
 313     {
 314         tmp_vec[i] = delta[T-1][i];
 315     }
 316
 317     // find starting point - best beat period for "last" frame
 318     bestpath[T-1] = get_max_ind(tmp_vec);
 319
 320     // backtrace through index of maximum values in psi
 321     for (unsigned int t=T-2; t>0 ;t--)
 322     {
 323         bestpath[t] = psi[t+1][bestpath[t+1]];
 324     }
 325
 326     // weird but necessary hack -- couldn't get above loop to terminate at t >= 0
 327     bestpath[0] = psi[1][bestpath[1]];
 328
 329     unsigned int lastind = 0;
 330     for (unsigned int i=0; i<T; i++)
 331     {
 332         unsigned int step = 128;
 333         for (unsigned int j=0; j<step; j++)
 334         {
 335             lastind = i*step+j;
 336             beat_period[lastind] = bestpath[i];
 337         }
 338 //        std::cerr << "bestpath[" << i << "] = " << bestpath[i] << " (used for beat_periods " << i*step << " to " << i*step+step-1 << ")" << std::endl;
 339     }
 340
 341     //fill in the last values...
 342     for (unsigned int i=lastind; i<beat_period.size(); i++)
 343     {
 344         beat_period[i] = beat_period[lastind];
 345     }
 346
 347     for (unsigned int i = 0; i < beat_period.size(); i++)
 348     {
 349         tempi.push_back((60. * m_rate / m_increment)/beat_period[i]);
 350     }
 351 }
 352
 353 double
 354 TempoTrackV2::get_max_val(const d_vec_t &df)
 355 {
 356     double maxval = 0.;
 357     for (unsigned int i=0; i<df.size(); i++)
 358     {
 359         if (maxval < df[i])
 360         {
 361             maxval = df[i];
 362         }
 363     }
 364
 365     return maxval;
 366 }
 367
 368 int
 369 TempoTrackV2::get_max_ind(const d_vec_t &df)
 370 {
 371     double maxval = 0.;
 372     int ind = 0;
 373     for (unsigned int i=0; i<df.size(); i++)
 374     {
 375         if (maxval < df[i])
 376         {
 377             maxval = df[i];
 378             ind = i;
 379         }
 380     }
 381
 382     return ind;
 383 }
 384
 385 void
 386 TempoTrackV2::normalise_vec(d_vec_t &df)
 387 {
 388     double sum = 0.;
 389     for (unsigned int i=0; i<df.size(); i++)
 390     {
 391         sum += df[i];
 392     }
 393
 394     for (unsigned int i=0; i<df.size(); i++)
 395     {
 396         df[i]/= (sum + EPS);
 397     }
 398 }
 399
 400 void
 401 TempoTrackV2::calculateBeats(const vector<double> &df,
 402                              const vector<double> &beat_period,
 403                              vector<double> &beats)
 404 {
 405     if (df.empty() || beat_period.empty()) return;
 406
 407     d_vec_t cumscore(df.size()); // store cumulative score
 408     i_vec_t backlink(df.size()); // backlink (stores best beat locations at each time instant)
 409     d_vec_t localscore(df.size()); // localscore, for now this is the same as the detection function
 410
 411     for (unsigned int i=0; i<df.size(); i++)
 412     {
 413         localscore[i] = df[i];
 414         backlink[i] = -1;
 415     }
 416
 417     double tightness = 4.;
 418     double alpha = 0.9;
 419
 420     // main loop
 421     for (unsigned int i=0; i<localscore.size(); i++)
 422     {
 423         int prange_min = -2*beat_period[i];
 424         int prange_max = round(-0.5*beat_period[i]);
 425
 426         // transition range
 427         d_vec_t txwt (prange_max - prange_min + 1);
 428         d_vec_t scorecands (txwt.size());
 429
 430         for (unsigned int j=0;j<txwt.size();j++)
 431         {
 432             double mu = static_cast<double> (beat_period[i]);
 433             txwt[j] = exp( -0.5*pow(tightness * log((round(2*mu)-j)/mu),2));
 434
 435             // IF IN THE ALLOWED RANGE, THEN LOOK AT CUMSCORE[I+PRANGE_MIN+J
 436             // ELSE LEAVE AT DEFAULT VALUE FROM INITIALISATION:  D_VEC_T SCORECANDS (TXWT.SIZE());
 437
 438             int cscore_ind = i+prange_min+j;
 439             if (cscore_ind >= 0)
 440             {
 441                 scorecands[j] = txwt[j] * cumscore[cscore_ind];
 442             }
 443         }
 444
 445         // find max value and index of maximum value
 446         double vv = get_max_val(scorecands);
 447         int xx = get_max_ind(scorecands);
 448
 449         cumscore[i] = alpha*vv + (1.-alpha)*localscore[i];
 450         backlink[i] = i+prange_min+xx;
 451
 452 //        std::cerr << "backlink[" << i << "] <= " << backlink[i] << std::endl;
 453     }
 454
 455     // STARTING POINT, I.E. LAST BEAT.. PICK A STRONG POINT IN cumscore VECTOR
 456     d_vec_t tmp_vec;
 457     for (unsigned int i=cumscore.size() - beat_period[beat_period.size()-1] ; i<cumscore.size(); i++)
 458     {
 459         tmp_vec.push_back(cumscore[i]);
 460     }
 461
 462     int startpoint = get_max_ind(tmp_vec) + cumscore.size() - beat_period[beat_period.size()-1] ;
 463
 464     // can happen if no results obtained earlier (e.g. input too short)
 465     if (startpoint >= backlink.size()) startpoint = backlink.size()-1;
 466
 467     // USE BACKLINK TO GET EACH NEW BEAT (TOWARDS THE BEGINNING OF THE FILE)
 468     //  BACKTRACKING FROM THE END TO THE BEGINNING.. MAKING SURE NOT TO GO BEFORE SAMPLE 0
 469     i_vec_t ibeats;
 470     ibeats.push_back(startpoint);
 471 //    std::cerr << "startpoint = " << startpoint << std::endl;
 472     while (backlink[ibeats.back()] > 0)
 473     {
 474 //        std::cerr << "backlink[" << ibeats.back() << "] = " << backlink[ibeats.back()] << std::endl;
 475         int b = ibeats.back();
 476         if (backlink[b] == b) break; // shouldn't happen... haha
 477         ibeats.push_back(backlink[b]);
 478     }
 479
 480     // REVERSE SEQUENCE OF IBEATS AND STORE AS BEATS
 481     for (unsigned int i=0; i<ibeats.size(); i++)
 482     {
 483         beats.push_back( static_cast<double>(ibeats[ibeats.size()-i-1]) );
 484     }
 485 }
 486
 487