cannam@52: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
cannam@52: 
cannam@52: /*
cannam@52:     QM DSP Library
cannam@52: 
cannam@52:     Centre for Digital Music, Queen Mary, University of London.
cannam@52:     This file copyright 2008-2009 Matthew Davies and QMUL.
cannam@52:     All rights reserved.
cannam@52: */
cannam@52: 
cannam@52: #include "TempoTrackV2.h"
cannam@52: 
cannam@52: #include <cmath>
cannam@52: #include <cstdlib>
cannam@53: #include <iostream>
cannam@52: 
cannam@54: #include "maths/MathUtilities.h"
cannam@52: 
cannam@52: #define   EPS 0.0000008 // just some arbitrary small number
cannam@52: 
cannam@54: TempoTrackV2::TempoTrackV2(float rate, size_t increment) :
cannam@54:     m_rate(rate), m_increment(increment) { }
cannam@52: TempoTrackV2::~TempoTrackV2() { }
cannam@52: 
cannam@52: void
cannam@52: TempoTrackV2::filter_df(d_vec_t &df)
cannam@52: {
cannam@53:     d_vec_t a(3);
cannam@53:     d_vec_t b(3);
cannam@53:     d_vec_t	lp_df(df.size());
cannam@52: 
cannam@53:     //equivalent in matlab to [b,a] = butter(2,0.4);
cannam@53:     a[0] = 1.0000;
cannam@53:     a[1] = -0.3695;
cannam@53:     a[2] = 0.1958;
cannam@53:     b[0] = 0.2066;
cannam@53:     b[1] = 0.4131;
cannam@53:     b[2] = 0.2066;
cannam@53:     
cannam@53:     double inp1 = 0.;
cannam@53:     double inp2 = 0.;
cannam@53:     double out1 = 0.;
cannam@53:     double out2 = 0.;
cannam@52: 
cannam@52: 
cannam@53:     // forwards filtering
cannam@53:     for (uint i = 0;i < df.size();i++)
cannam@53:     {
cannam@53:         lp_df[i] =  b[0]*df[i] + b[1]*inp1 + b[2]*inp2 - a[1]*out1 - a[2]*out2;
cannam@53:         inp2 = inp1;
cannam@53:         inp1 = df[i];
cannam@53:         out2 = out1;
cannam@53:         out1 = lp_df[i];
cannam@53:     }
cannam@52: 
cannam@53:     // copy forwards filtering to df...
cannam@53:     // but, time-reversed, ready for backwards filtering
cannam@53:     for (uint i = 0;i < df.size();i++)
cannam@53:     {
cannam@53:         df[i] = lp_df[df.size()-i-1];
cannam@53:     }
cannam@52: 
cannam@53:     for (uint i = 0;i < df.size();i++)
cannam@53:     {
cannam@53:         lp_df[i] = 0.;    
cannam@53:     }
cannam@52: 
cannam@53:     inp1 = 0.; inp2 = 0.;
cannam@53:     out1 = 0.; out2 = 0.;
cannam@52: 
cannam@52:   // backwards filetering on time-reversed df
cannam@53:     for (uint i = 0;i < df.size();i++)
cannam@53:     {
cannam@53:         lp_df[i] =  b[0]*df[i] + b[1]*inp1 + b[2]*inp2 - a[1]*out1 - a[2]*out2;
cannam@53:         inp2 = inp1;
cannam@53:         inp1 = df[i];
cannam@53:         out2 = out1;
cannam@53:         out1 = lp_df[i];
cannam@53:     }
cannam@52: 
cannam@52:   // write the re-reversed (i.e. forward) version back to df
cannam@53:     for (uint i = 0;i < df.size();i++)
cannam@53:     {
cannam@53:         df[i] = lp_df[df.size()-i-1];
cannam@53:     }
cannam@52: }
cannam@52: 
cannam@52: 
cannam@52: void
cannam@53: TempoTrackV2::calculateBeatPeriod(const d_vec_t &df, d_vec_t &beat_period,
cannam@53:                                   d_vec_t &tempi)
cannam@52: {
cannam@53:     // to follow matlab.. split into 512 sample frames with a 128 hop size
cannam@53:     // calculate the acf,
cannam@53:     // then the rcf.. and then stick the rcfs as columns of a matrix
cannam@53:     // then call viterbi decoding with weight vector and transition matrix
cannam@53:     // and get best path
cannam@52: 
cannam@53:     uint wv_len = 128;
cannam@53:     double rayparam = 43.;
cannam@52: 
cannam@53:     // make rayleigh weighting curve
cannam@53:     d_vec_t wv(wv_len);
cannam@53:     for (uint i=0; i<wv.size(); i++)
cannam@52:     {
cannam@53:         wv[i] = (static_cast<double> (i) / pow(rayparam,2.)) * exp((-1.*pow(-static_cast<double> (i),2.)) / (2.*pow(rayparam,2.)));
cannam@52:     }
cannam@52: 
cannam@53:     // beat tracking frame size (roughly 6 seconds) and hop (1.5 seconds)
cannam@53:     uint winlen = 512;
cannam@53:     uint step = 128;
cannam@53: 
cannam@53:     // matrix to store output of comb filter bank, increment column of matrix at each frame
cannam@53:     d_mat_t rcfmat;
cannam@53:     int col_counter = -1;
cannam@53: 
cannam@53:     // main loop for beat period calculation
cannam@53:     for (uint i=0; i<(df.size()-winlen); i+=step)
cannam@53:     {
cannam@53:         // get dfframe
cannam@53:         d_vec_t dfframe(winlen);
cannam@53:         for (uint k=0; k<winlen; k++)
cannam@53:         {
cannam@53:             dfframe[k] = df[i+k];
cannam@53:         }
cannam@53:         // get rcf vector for current frame
cannam@53:         d_vec_t rcf(wv_len);    
cannam@53:         get_rcf(dfframe,wv,rcf);
cannam@52:   
cannam@53:         rcfmat.push_back( d_vec_t() ); // adds a new column
cannam@53:         col_counter++;
cannam@53:         for (uint j=0; j<rcf.size(); j++)
cannam@53:         {
cannam@53:             rcfmat[col_counter].push_back( rcf[j] );
cannam@53:         }
cannam@53:     }
cannam@53:   
cannam@53:     // now call viterbi decoding function
cannam@53:     viterbi_decode(rcfmat,wv,beat_period,tempi);
cannam@52: }
cannam@52: 
cannam@52: 
cannam@52: void
cannam@52: TempoTrackV2::get_rcf(const d_vec_t &dfframe_in, const d_vec_t &wv, d_vec_t &rcf)
cannam@52: {
cannam@53:     // calculate autocorrelation function
cannam@53:     // then rcf
cannam@53:     // just hard code for now... don't really need separate functions to do this
cannam@52: 
cannam@53:     // make acf
cannam@52: 
cannam@53:     d_vec_t dfframe(dfframe_in);
cannam@52: 
cannam@54:     MathUtilities::adaptiveThreshold(dfframe);
cannam@52: 
cannam@53:     d_vec_t acf(dfframe.size());
cannam@52: 
cannam@53:     
cannam@53:     for (uint lag=0; lag<dfframe.size(); lag++)
cannam@53:     {
cannam@53:         double sum = 0.;
cannam@53:         double tmp = 0.;
cannam@52: 
cannam@53:         for (uint n=0; n<(dfframe.size()-lag); n++)
cannam@53:         {
cannam@53:             tmp = dfframe[n] * dfframe[n+lag];    
cannam@53:             sum += tmp;
cannam@53:         }
cannam@53:         acf[lag] = static_cast<double> (sum/ (dfframe.size()-lag));
cannam@53:     }
cannam@52: 
cannam@53:     // now apply comb filtering
cannam@53:     int numelem = 4;
cannam@53: 	
cannam@53:     for (uint i = 2;i < rcf.size();i++) // max beat period
cannam@53:     {
cannam@53:         for (int a = 1;a <= numelem;a++) // number of comb elements
cannam@53:         {
cannam@53:             for (int b = 1-a;b <= a-1;b++) // general state using normalisation of comb elements
cannam@53:             {
cannam@53:                 rcf[i-1] += ( acf[(a*i+b)-1]*wv[i-1] ) / (2.*a-1.);	// calculate value for comb filter row
cannam@53:             }
cannam@53:         }
cannam@53:     }
cannam@53:   
cannam@53:     // apply adaptive threshold to rcf
cannam@54:     MathUtilities::adaptiveThreshold(rcf);
cannam@53:   
cannam@53:     double rcfsum =0.;
cannam@53:     for (uint i=0; i<rcf.size(); i++)
cannam@53:     {
cannam@53:         rcf[i] += EPS ;
cannam@53:         rcfsum += rcf[i];
cannam@53:     }
cannam@52: 
cannam@53:     // normalise rcf to sum to unity
cannam@53:     for (uint i=0; i<rcf.size(); i++)
cannam@52:     {
cannam@53:         rcf[i] /= (rcfsum + EPS);
cannam@52:     }
cannam@52: }
cannam@52: 
cannam@52: void
cannam@53: TempoTrackV2::viterbi_decode(const d_mat_t &rcfmat, const d_vec_t &wv, d_vec_t &beat_period, d_vec_t &tempi)
cannam@52: {
cannam@53:     // following Kevin Murphy's Viterbi decoding to get best path of
cannam@53:     // beat periods through rfcmat
cannam@52: 
cannam@53:     // make transition matrix
cannam@53:     d_mat_t tmat;
cannam@53:     for (uint i=0;i<wv.size();i++)
cannam@53:     {
cannam@53:         tmat.push_back ( d_vec_t() ); // adds a new column
cannam@53:         for (uint j=0; j<wv.size(); j++)
cannam@53:         {	
cannam@53:             tmat[i].push_back(0.); // fill with zeros initially
cannam@53:         }
cannam@53:     }
cannam@53:     
cannam@53:     // variance of Gaussians in transition matrix
cannam@53:     // formed of Gaussians on diagonal - implies slow tempo change
cannam@53:     double sigma = 8.;
cannam@53:     // don't want really short beat periods, or really long ones
cannam@53:     for (uint i=20;i <wv.size()-20; i++)
cannam@53:     {
cannam@53:         for (uint j=20; j<wv.size()-20; j++)
cannam@53:         {	
cannam@53:             double mu = static_cast<double>(i);
cannam@53:             tmat[i][j] = exp( (-1.*pow((j-mu),2.)) / (2.*pow(sigma,2.)) );
cannam@53:         }
cannam@53:     }
cannam@52: 
cannam@53:     // parameters for Viterbi decoding... this part is taken from
cannam@53:     // Murphy's matlab
cannam@52: 
cannam@53:     d_mat_t delta;
cannam@53:     i_mat_t psi;
cannam@53:     for (uint i=0;i <rcfmat.size(); i++)
cannam@53:     {
cannam@53:         delta.push_back( d_vec_t());
cannam@53:         psi.push_back( i_vec_t());
cannam@53:         for (uint j=0; j<rcfmat[i].size(); j++)
cannam@53:         {	
cannam@53:             delta[i].push_back(0.); // fill with zeros initially
cannam@53:             psi[i].push_back(0); // fill with zeros initially
cannam@53:         }
cannam@53:     }
cannam@52: 
cannam@52: 
cannam@53:     uint T = delta.size();
cannam@53:     uint Q = delta[0].size();
cannam@52: 
cannam@53:     // initialize first column of delta
cannam@52:     for (uint j=0; j<Q; j++)
cannam@52:     {
cannam@53:         delta[0][j] = wv[j] * rcfmat[0][j];
cannam@53:         psi[0][j] = 0;
cannam@52:     }
cannam@53:     
cannam@52:     double deltasum = 0.;
cannam@52:     for (uint i=0; i<Q; i++)
cannam@52:     {
cannam@53:         deltasum += delta[0][i];
cannam@52:     }      
cannam@52:     for (uint i=0; i<Q; i++)
cannam@52:     {
cannam@53:         delta[0][i] /= (deltasum + EPS);
cannam@52:     }      
cannam@52: 
cannam@52: 
cannam@53:     for (uint t=1; t<T; t++)
cannam@53:     {
cannam@53:         d_vec_t tmp_vec(Q);
cannam@52: 
cannam@53:         for (uint j=0; j<Q; j++)
cannam@53:         {
cannam@53:             for (uint i=0; i<Q; i++)
cannam@53:             {
cannam@53:                 tmp_vec[i] = delta[t-1][i] * tmat[j][i];
cannam@53:             }      
cannam@53:    
cannam@53:             delta[t][j] = get_max_val(tmp_vec);    
cannam@52: 
cannam@53:             psi[t][j] = get_max_ind(tmp_vec);
cannam@53:  
cannam@53:             delta[t][j] *= rcfmat[t][j];
cannam@53:         }
cannam@52: 
cannam@53:         // normalise current delta column
cannam@53:         double deltasum = 0.;
cannam@53:         for (uint i=0; i<Q; i++)
cannam@53:         {
cannam@53:             deltasum += delta[t][i];
cannam@53:         }      
cannam@53:         for (uint i=0; i<Q; i++)
cannam@53:         {
cannam@53:             delta[t][i] /= (deltasum + EPS);
cannam@53:         }      
cannam@53:     }
cannam@52: 
cannam@53:     i_vec_t bestpath(T);
cannam@53:     d_vec_t tmp_vec(Q);
cannam@53:     for (uint i=0; i<Q; i++)
cannam@53:     {  
cannam@53:         tmp_vec[i] = delta[T-1][i];
cannam@53:     }
cannam@52: 
cannam@53:     // find starting point - best beat period for "last" frame
cannam@53:     bestpath[T-1] = get_max_ind(tmp_vec);
cannam@53:  
cannam@53:     // backtrace through index of maximum values in psi
cannam@53:     for (uint t=T-2; t>0 ;t--)
cannam@53:     {
cannam@53:         bestpath[t] = psi[t+1][bestpath[t+1]];
cannam@53:     }
cannam@52: 
cannam@53:     // weird but necessary hack -- couldn't get above loop to terminate at t >= 0
cannam@53:     bestpath[0] = psi[1][bestpath[1]];
cannam@52: 
cannam@53:     uint lastind = 0;
cannam@53:     for (uint i=0; i<T; i++)
cannam@53:     {  
cannam@53:         uint step = 128;
cannam@53:         for (uint j=0; j<step; j++)
cannam@53:         {
cannam@53:             lastind = i*step+j;
cannam@53:             beat_period[lastind] = bestpath[i];
cannam@53:         }
cannam@55:         std::cerr << "bestpath[" << i << "] = " << bestpath[i] << " (used for beat_periods " << i*step << " to " << i*step+step-1 << ")" << std::endl;
cannam@53:     }
cannam@52: 
cannam@53:     //fill in the last values...
cannam@53:     for (uint i=lastind; i<beat_period.size(); i++)
cannam@53:     {
cannam@53:         beat_period[i] = beat_period[lastind];
cannam@53:     }
cannam@52: 
cannam@53:     for (uint i = 0; i < beat_period.size(); i++)
cannam@52:     {
cannam@54:         tempi.push_back((60. * m_rate / m_increment)/beat_period[i]);
cannam@52:     }
cannam@52: }
cannam@52: 
cannam@52: double
cannam@52: TempoTrackV2::get_max_val(const d_vec_t &df)
cannam@52: {
cannam@53:     double maxval = 0.;
cannam@53:     for (uint i=0; i<df.size(); i++)
cannam@52:     {
cannam@53:         if (maxval < df[i])
cannam@53:         {
cannam@53:             maxval = df[i];
cannam@53:         }
cannam@52:     }
cannam@52:     
cannam@53:     return maxval;
cannam@52: }
cannam@52: 
cannam@52: int
cannam@52: TempoTrackV2::get_max_ind(const d_vec_t &df)
cannam@52: {
cannam@53:     double maxval = 0.;
cannam@53:     int ind = 0;
cannam@53:     for (uint i=0; i<df.size(); i++)
cannam@52:     {
cannam@53:         if (maxval < df[i])
cannam@53:         {
cannam@53:             maxval = df[i];
cannam@53:             ind = i;
cannam@53:         }
cannam@52:     }
cannam@53:     
cannam@53:     return ind;
cannam@52: }
cannam@52: 
cannam@52: void
cannam@52: TempoTrackV2::normalise_vec(d_vec_t &df)
cannam@52: {
cannam@53:     double sum = 0.;
cannam@53:     for (uint i=0; i<df.size(); i++)
cannam@53:     {
cannam@53:         sum += df[i];
cannam@53:     }
cannam@53:     
cannam@53:     for (uint i=0; i<df.size(); i++)
cannam@53:     {
cannam@53:         df[i]/= (sum + EPS);
cannam@53:     }
cannam@52: }
cannam@52: 
cannam@52: void
cannam@52: TempoTrackV2::calculateBeats(const d_vec_t &df, const d_vec_t &beat_period,
cannam@52:                              d_vec_t &beats)
cannam@52: {
cannam@53:     d_vec_t cumscore(df.size()); // store cumulative score
cannam@53:     i_vec_t backlink(df.size()); // backlink (stores best beat locations at each time instant)
cannam@53:     d_vec_t localscore(df.size()); // localscore, for now this is the same as the detection function
cannam@52: 
cannam@53:     for (uint i=0; i<df.size(); i++)
cannam@52:     {
cannam@53:         localscore[i] = df[i];
cannam@53:         backlink[i] = -1;
cannam@52:     }
cannam@52: 
cannam@53:     double tightness = 4.;
cannam@53:     double alpha = 0.9;
cannam@52: 
cannam@53:     // main loop
cannam@53:     for (uint i=0; i<localscore.size(); i++)
cannam@53:     {
cannam@53:         int prange_min = -2*beat_period[i];
cannam@53:         int prange_max = round(-0.5*beat_period[i]);
cannam@52: 
cannam@53:         // transition range
cannam@53:         d_vec_t txwt (prange_max - prange_min + 1);
cannam@53:         d_vec_t scorecands (txwt.size());
cannam@52: 
cannam@53:         for (uint j=0;j<txwt.size();j++)
cannam@53:         {
cannam@53:             double mu = static_cast<double> (beat_period[i]);
cannam@53:             txwt[j] = exp( -0.5*pow(tightness * log((round(2*mu)-j)/mu),2));
cannam@52: 
cannam@53:             // IF IN THE ALLOWED RANGE, THEN LOOK AT CUMSCORE[I+PRANGE_MIN+J
cannam@53:             // ELSE LEAVE AT DEFAULT VALUE FROM INITIALISATION:  D_VEC_T SCORECANDS (TXWT.SIZE());
cannam@52: 
cannam@53:             int cscore_ind = i+prange_min+j;
cannam@53:             if (cscore_ind >= 0)   
cannam@53:             {
cannam@53:                 scorecands[j] = txwt[j] * cumscore[cscore_ind];
cannam@53:             }
cannam@53:         }
cannam@52: 
cannam@53:         // find max value and index of maximum value
cannam@53:         double vv = get_max_val(scorecands);
cannam@53:         int xx = get_max_ind(scorecands);
cannam@52: 
cannam@53:         cumscore[i] = alpha*vv + (1.-alpha)*localscore[i];
cannam@53:         backlink[i] = i+prange_min+xx;
cannam@55: 
cannam@55:         std::cerr << "backlink[" << i << "] <= " << backlink[i] << std::endl;
cannam@53:     }
cannam@53: 
cannam@53:     // STARTING POINT, I.E. LAST BEAT.. PICK A STRONG POINT IN cumscore VECTOR
cannam@53:     d_vec_t tmp_vec;
cannam@53:     for (uint i=cumscore.size() - beat_period[beat_period.size()-1] ; i<cumscore.size(); i++)
cannam@53:     {
cannam@53:         tmp_vec.push_back(cumscore[i]);
cannam@53:     }  
cannam@53: 
cannam@53:     int startpoint = get_max_ind(tmp_vec) + cumscore.size() - beat_period[beat_period.size()-1] ;
cannam@53: 
cannam@53:     // USE BACKLINK TO GET EACH NEW BEAT (TOWARDS THE BEGINNING OF THE FILE)
cannam@53:     //  BACKTRACKING FROM THE END TO THE BEGINNING.. MAKING SURE NOT TO GO BEFORE SAMPLE 0
cannam@53:     i_vec_t ibeats;
cannam@53:     ibeats.push_back(startpoint);
cannam@55:     std::cerr << "startpoint = " << startpoint << std::endl;
cannam@53:     while (backlink[ibeats.back()] > 0)
cannam@53:     {
cannam@55:         std::cerr << "backlink[" << ibeats.back() << "] = " << backlink[ibeats.back()] << std::endl;
cannam@53:         ibeats.push_back(backlink[ibeats.back()]);
cannam@53:     }
cannam@52:   
cannam@53:     // REVERSE SEQUENCE OF IBEATS AND STORE AS BEATS
cannam@53:     for (uint i=0; i<ibeats.size(); i++)
cannam@53:     { 
cannam@53:         beats.push_back( static_cast<double>(ibeats[ibeats.size()-i-1]) );
cannam@53:     }
cannam@52: }
cannam@52: 
cannam@52: