c@277: /* -*- c-basic-offset: 4 indent-tabs-mode: nil -*-  vi:set ts=8 sts=4 sw=4: */
c@277: 
c@277: /*
c@277:     QM DSP Library
c@277: 
c@277:     Centre for Digital Music, Queen Mary, University of London.
c@277:     This file copyright 2008-2009 Matthew Davies and QMUL.
c@277:     All rights reserved.
c@277: */
c@277: 
c@277: #include "TempoTrackV2.h"
c@277: 
c@277: #include <cmath>
c@277: #include <cstdlib>
c@277: 
c@277: 
c@277: //#define		FRAMESIZE	512
c@277: //#define		BIGFRAMESIZE	1024
c@277: #define   TWOPI 6.283185307179586232
c@277: #define   EPS 0.0000008 // just some arbitrary small number
c@277: 
c@277: TempoTrackV2::TempoTrackV2() { }
c@277: TempoTrackV2::~TempoTrackV2() { }
c@277: 
c@277: void
c@277: TempoTrackV2::adapt_thresh(d_vec_t &df)
c@277: {
c@277: 
c@277:   d_vec_t smoothed(df.size());
c@277: 	
c@277: 	int p_post = 7;
c@277: 	int p_pre = 8;
c@277: 
c@277: 	int t = std::min(static_cast<int>(df.size()),p_post);	// what is smaller, p_post of df size. This is to avoid accessing outside of arrays
c@277: 
c@277: 	// find threshold for first 't' samples, where a full average cannot be computed yet 
c@277: 	for (int i = 0;i <= t;i++)
c@277: 	{	
c@277:             int k = std::min((i+p_pre),static_cast<int>(df.size()));
c@277: 		smoothed[i] = mean_array(df,1,k);
c@277: 	}
c@277: 	// find threshold for bulk of samples across a moving average from [i-p_pre,i+p_post]
c@277: 	for (uint i = t+1;i < df.size()-p_post;i++)
c@277: 	{
c@277: 		smoothed[i] = mean_array(df,i-p_pre,i+p_post);
c@277: 	}
c@277: 	// for last few samples calculate threshold, again, not enough samples to do as above
c@277: 	for (uint i = df.size()-p_post;i < df.size();i++)
c@277: 	{
c@277:             int k = std::max((static_cast<int> (i) -p_post),1);
c@277: 		smoothed[i] = mean_array(df,k,df.size());
c@277: 	}
c@277: 
c@277: 	// subtract the threshold from the detection function and check that it is not less than 0
c@277: 	for (uint i = 0;i < df.size();i++)
c@277: 	{
c@277: 		df[i] -= smoothed[i];
c@277: 		if (df[i] < 0)
c@277: 		{
c@277: 			df[i] = 0;
c@277: 		}
c@277: 	}
c@277: }
c@277: 
c@277: double
c@277: TempoTrackV2::mean_array(const d_vec_t &dfin,int start,int end)
c@277: {
c@277: 
c@277: 	double sum = 0.;
c@277: 	
c@277: 	// find sum
c@277: 	for (int i = start;i < end+1;i++)
c@277: 	{
c@277: 		sum += dfin[i];
c@277: 	}
c@277: 
c@277: 	return static_cast<double> (sum / (end - start + 1) );	// average and return
c@277: }
c@277: 
c@277: void
c@277: TempoTrackV2::filter_df(d_vec_t &df)
c@277: {
c@277: 
c@277: 
c@277:   d_vec_t a(3);
c@277:   d_vec_t b(3);
c@277:   d_vec_t	lp_df(df.size());
c@277: 
c@277:   //equivalent in matlab to [b,a] = butter(2,0.4);
c@277: 	a[0] = 1.0000;
c@277: 	a[1] = -0.3695;
c@277: 	a[2] = 0.1958;
c@277: 	b[0] = 0.2066;
c@277: 	b[1] = 0.4131;
c@277: 	b[2] = 0.2066;
c@277: 	
c@277:   double inp1 = 0.;
c@277:   double inp2 = 0.;
c@277:   double out1 = 0.;
c@277:   double out2 = 0.;
c@277: 
c@277: 
c@277:   // forwards filtering
c@277: 	for (uint i = 0;i < df.size();i++)
c@277: 	{
c@277:     lp_df[i] =  b[0]*df[i] + b[1]*inp1 + b[2]*inp2 - a[1]*out1 - a[2]*out2;
c@277:     inp2 = inp1;
c@277:     inp1 = df[i];
c@277:     out2 = out1;
c@277:     out1 = lp_df[i];
c@277: 	}
c@277: 
c@277: 
c@277:   // copy forwards filtering to df...
c@277:   // but, time-reversed, ready for backwards filtering
c@277: 	for (uint i = 0;i < df.size();i++)
c@277: 	{
c@277:     df[i] = lp_df[df.size()-i];    
c@277:   }
c@277: 
c@277: 	for (uint i = 0;i < df.size();i++)
c@277: 	{
c@277:     lp_df[i] = 0.;    
c@277:   }
c@277: 
c@277:   inp1 = 0.; inp2 = 0.;
c@277:   out1 = 0.; out2 = 0.;
c@277: 
c@277:   // backwards filetering on time-reversed df
c@277: 	for (uint i = 0;i < df.size();i++)
c@277: 	{
c@277:     lp_df[i] =  b[0]*df[i] + b[1]*inp1 + b[2]*inp2 - a[1]*out1 - a[2]*out2;
c@277:     inp2 = inp1;
c@277:     inp1 = df[i];
c@277:     out2 = out1;
c@277:     out1 = lp_df[i];
c@277: 	}
c@277: 
c@277:   // write the re-reversed (i.e. forward) version back to df
c@277: 	for (uint i = 0;i < df.size();i++)
c@277: 	{
c@277:     df[i] = lp_df[df.size()-i];    
c@277:   }
c@277: 
c@277: 
c@277: }
c@277: 
c@277: 
c@277: void
c@277: TempoTrackV2::calculateBeatPeriod(const d_vec_t &df, d_vec_t &beat_period)
c@277: {
c@277: 
c@277: // to follow matlab.. split into 512 sample frames with a 128 hop size
c@277: // calculate the acf,
c@277: // then the rcf.. and then stick the rcfs as columns of a matrix
c@277: // then call viterbi decoding with weight vector and transition matrix
c@277: // and get best path
c@277: 
c@277:   uint wv_len = 128;
c@277:   double rayparam = 43.;
c@277: 
c@277:   // make rayleigh weighting curve
c@277:   d_vec_t wv(wv_len);
c@277:   for (uint i=0; i<wv.size(); i++)
c@277:   {
c@277:     wv[i] = (static_cast<double> (i) / pow(rayparam,2.)) * exp((-1.*pow(-static_cast<double> (i),2.)) / (2.*pow(rayparam,2.)));
c@277:   }
c@277: 
c@277: 
c@277:   uint winlen = 512;
c@277:   uint step = 128;
c@277: 
c@277:   d_mat_t rcfmat;
c@277:   int col_counter = -1;
c@277:   // main loop for beat period calculation
c@277:   for (uint i=0; i<(df.size()-winlen); i+=step)
c@277:   {
c@277:     // get dfframe
c@277:     d_vec_t dfframe(winlen);
c@277:     for (uint k=0; k<winlen; k++)
c@277:     {
c@277:       dfframe[k] = df[i+k];
c@277:     }
c@277:     // get rcf vector for current frame
c@277:     d_vec_t rcf(wv_len);    
c@277:     get_rcf(dfframe,wv,rcf);
c@277:   
c@277:     rcfmat.push_back( d_vec_t() ); // adds a new column
c@277:     col_counter++;
c@277:     for (uint j=0; j<rcf.size(); j++)
c@277:     {
c@277:       rcfmat[col_counter].push_back( rcf[j] );
c@277:     }
c@277: 
c@277:   }
c@277:   
c@277:   // now call viterbi decoding function
c@277:   viterbi_decode(rcfmat,wv,beat_period);
c@277: 
c@277: 
c@277: 
c@277: }
c@277: 
c@277: 
c@277: void
c@277: TempoTrackV2::get_rcf(const d_vec_t &dfframe_in, const d_vec_t &wv, d_vec_t &rcf)
c@277: {
c@277:   // calculate autocorrelation function
c@277:   // then rcf
c@277:   // just hard code for now... don't really need separate functions to do this
c@277: 
c@277:   // make acf
c@277: 
c@277:   d_vec_t dfframe(dfframe_in);
c@277: 
c@277:   adapt_thresh(dfframe);
c@277: 
c@277:   d_vec_t acf(dfframe.size());
c@277: 
c@277: 
c@277:   for (uint lag=0; lag<dfframe.size(); lag++)
c@277:   {
c@277: 
c@277:     double sum = 0.;
c@277:     double tmp = 0.;
c@277: 
c@277:     for (uint n=0; n<(dfframe.size()-lag); n++)
c@277:     {
c@277:       tmp = dfframe[n] * dfframe[n+lag];    
c@277:       sum += tmp;
c@277:     }
c@277:     acf[lag] = static_cast<double> (sum/ (dfframe.size()-lag));
c@277:   }
c@277: 
c@277: 
c@277: //  for (uint i=0; i<dfframe.size(); i++)
c@277: //  {
c@277: //    cout << dfframe[i] << " " << acf[i]  << endl;
c@277: //  }
c@277: 
c@277: //    cout << "~~~~~~~~~~~~~~" << endl;
c@277: 
c@277: 
c@277: 
c@277: 
c@277: 
c@277:   // now apply comb filtering
c@277: 	int numelem = 4;
c@277: 	
c@277: //	for (uint i = 1;i < 118;i++) // max beat period
c@277: 	for (uint i = 2;i < rcf.size();i++) // max beat period
c@277: 	{
c@277: 		for (int a = 1;a <= numelem;a++) // number of comb elements
c@277: 		{
c@277: 			for (int b = 1-a;b <= a-1;b++) // general state using normalisation of comb elements
c@277: 			{
c@277: 				rcf[i-1] += ( acf[(a*i+b)-1]*wv[i-1] ) / (2.*a-1.);	// calculate value for comb filter row
c@277: 			}
c@277: 		}
c@277: 	}
c@277:   
c@277:   // apply adaptive threshold to rcf
c@277:   adapt_thresh(rcf);
c@277:   
c@277:   double rcfsum =0.;
c@277:   for (uint i=0; i<rcf.size(); i++)
c@277:   {
c@277:  //  rcf[i] *= acf[i];
c@277:     rcf[i] += EPS ;
c@277:     rcfsum += rcf[i];
c@277:   }
c@277: 
c@277:   // normalise rcf to sum to unity
c@277:   for (uint i=0; i<rcf.size(); i++)
c@277:   {
c@277:     rcf[i] /= (rcfsum + EPS);
c@277:   }
c@277: 
c@277: 
c@277: 
c@277: }
c@277: 
c@277: void
c@277: TempoTrackV2::viterbi_decode(const d_mat_t &rcfmat, const d_vec_t &wv, d_vec_t &beat_period)
c@277: {
c@277: 
c@277:   // make transition matrix
c@277: 	d_mat_t tmat;
c@277: 	for (uint i=0;i<wv.size();i++)
c@277: 	{
c@277: 		tmat.push_back ( d_vec_t() ); // adds a new column
c@277: 		for (uint j=0; j<wv.size(); j++)
c@277: 		{	
c@277: 			tmat[i].push_back(0.); // fill with zeros initially
c@277: 		}
c@277: 	}
c@277: 
c@277:   double sigma = 8.;
c@277: 	for (uint i=20;i <wv.size()-20; i++)
c@277: 	{
c@277: 		for (uint j=20; j<wv.size()-20; j++)
c@277: 		{	
c@277:       double mu = static_cast<double>(i);
c@277: 			tmat[i][j] = exp( (-1.*pow((j-mu),2.)) / (2.*pow(sigma,2.)) );
c@277: 		}
c@277: 	}
c@277: 
c@277:   d_mat_t delta;
c@277:   i_mat_t psi;
c@277: 	for (uint i=0;i <rcfmat.size(); i++)
c@277: 	{
c@277:     delta.push_back( d_vec_t());
c@277:     psi.push_back( i_vec_t());
c@277: 		for (uint j=0; j<rcfmat[i].size(); j++)
c@277: 		{	
c@277: 			delta[i].push_back(0.); // fill with zeros initially
c@277: 			psi[i].push_back(0); // fill with zeros initially
c@277: 		}
c@277: 	}
c@277: 
c@277: 
c@277:   uint T = delta.size();
c@277:   uint Q = delta[0].size();
c@277: 
c@277:   // initialize first column of delta
c@277:   for (uint j=0; j<Q; j++)
c@277:   {
c@277:     delta[0][j] = wv[j] * rcfmat[0][j];
c@277:     psi[0][j] = 0;
c@277:   }
c@277: 
c@277:   double deltasum = 0.;
c@277:   for (uint i=0; i<Q; i++)
c@277:   {
c@277:     deltasum += delta[0][i];
c@277:   }      
c@277:   for (uint i=0; i<Q; i++)
c@277:   {
c@277:     delta[0][i] /= (deltasum + EPS);
c@277:   }      
c@277: 
c@277: 
c@277: 
c@277:   for (uint t=1; t<T; t++)
c@277:   {
c@277:     d_vec_t tmp_vec(Q);
c@277: 
c@277:     for (uint j=0; j<Q; j++)
c@277:     {
c@277: 
c@277:       for (uint i=0; i<Q; i++)
c@277:       {
c@277:         tmp_vec[i] = delta[t-1][i] * tmat[j][i];
c@277:       }      
c@277:    
c@277:       delta[t][j] = get_max_val(tmp_vec);    
c@277: 
c@277:       psi[t][j] = get_max_ind(tmp_vec);
c@277:  
c@277:       delta[t][j] *= rcfmat[t][j];
c@277: 
c@277: 
c@277:     }
c@277: 
c@277:     double deltasum = 0.;
c@277:     for (uint i=0; i<Q; i++)
c@277:     {
c@277:       deltasum += delta[t][i];
c@277:     }      
c@277:     for (uint i=0; i<Q; i++)
c@277:     {
c@277:       delta[t][i] /= (deltasum + EPS);
c@277:     }      
c@277: 
c@277: 
c@277: 
c@277: 
c@277:   }
c@277: 
c@277: 
c@277: //  ofstream tmatfile;
c@277: //  tmatfile.open("/home/matthewd/Desktop/tmat.txt");
c@277: 
c@277: // 	for (uint i=0;i <delta.size(); i++)
c@277: //	{
c@277: //		for (uint j=0; j<delta[i].size(); j++)
c@277: //		{	
c@277: //      tmatfile << rcfmat[i][j] << endl;
c@277: //		}
c@277: //	}
c@277: 
c@277: //  tmatfile.close();
c@277: 
c@277:   i_vec_t bestpath(T);
c@277:   d_vec_t tmp_vec(Q);
c@277:   for (uint i=0; i<Q; i++)
c@277:   {  
c@277:     tmp_vec[i] = delta[T-1][i];
c@277:   }
c@277: 
c@277:   
c@277:   bestpath[T-1] = get_max_ind(tmp_vec);
c@277:  
c@277:   for (uint t=T-2; t>0 ;t--)
c@277:   {
c@277:     bestpath[t] = psi[t+1][bestpath[t+1]];
c@277:   }
c@277:   // very weird hack!
c@277:   bestpath[0] = psi[1][bestpath[1]];
c@277: 
c@277: //  for (uint i=0; i<bestpath.size(); i++)
c@277: //  {
c@277: //    cout << bestpath[i] << endl;
c@277: //  }
c@277: 
c@277: 
c@277:   uint lastind = 0;
c@277:   for (uint i=0; i<T; i++)
c@277:   {  
c@277:     uint step = 128;
c@277:  //   cout << bestpath[i] << " " << i << endl;
c@277:     for (uint j=0; j<step; j++)
c@277:     {
c@277:       lastind = i*step+j;
c@277:       beat_period[lastind] = bestpath[i];
c@277:       
c@277:     }
c@277:   }
c@277: 
c@277:   //fill in the last values...
c@277:   for (uint i=lastind; i<beat_period.size(); i++)
c@277:   {
c@277:     beat_period[i] = beat_period[lastind];
c@277:   }
c@277:  
c@277: 
c@277: 
c@277: }
c@277: 
c@277: double
c@277: TempoTrackV2::get_max_val(const d_vec_t &df)
c@277: {
c@277:   double maxval = 0.;
c@277:   for (uint i=0; i<df.size(); i++)
c@277:   {
c@277: 
c@277:     if (maxval < df[i])
c@277:     {
c@277:       maxval = df[i];
c@277:     }
c@277: 
c@277:   }
c@277: 
c@277:     
c@277:   return maxval;
c@277: 
c@277: }
c@277: 
c@277: int
c@277: TempoTrackV2::get_max_ind(const d_vec_t &df)
c@277: {
c@277: 
c@277:   double maxval = 0.;
c@277:   int ind = 0;
c@277:   for (uint i=0; i<df.size(); i++)
c@277:   {
c@277:     if (maxval < df[i])
c@277:     {
c@277:       maxval = df[i];
c@277:       ind = i;
c@277:     }
c@277: 
c@277:   }
c@277:   
c@277:   return ind;
c@277: 
c@277: }
c@277: 
c@277: void
c@277: TempoTrackV2::normalise_vec(d_vec_t &df)
c@277: {
c@277:   double sum = 0.;
c@277:   for (uint i=0; i<df.size(); i++)
c@277:   {
c@277:     sum += df[i];
c@277:   }
c@277: 
c@277:   for (uint i=0; i<df.size(); i++)
c@277:   {
c@277:     df[i]/= (sum + EPS);
c@277:   }
c@277: 
c@277: 
c@277: }
c@277: 
c@277: void
c@277: TempoTrackV2::calculateBeats(const d_vec_t &df, const d_vec_t &beat_period,
c@277:                              d_vec_t &beats)
c@277: {
c@277: 
c@277:   d_vec_t cumscore(df.size());
c@277:   i_vec_t backlink(df.size());
c@277:   d_vec_t localscore(df.size());
c@277: 
c@277:   // WHEN I FIGURE OUT HOW, I'LL WANT TO DO SOME FILTERING ON THIS... 
c@277:   for (uint i=0; i<df.size(); i++)
c@277:   {
c@277:     localscore[i] = df[i];
c@277:     backlink[i] = -1;
c@277:   }
c@277: 
c@277:   double tightness = 4.;
c@277:   double alpha = 0.9;
c@277: 
c@277:   // main loop
c@277:   for (uint i=3*beat_period[0]; i<localscore.size(); i++)
c@277:   {
c@277:     int prange_min = -2*beat_period[i];
c@277:     int prange_max = round(-0.5*beat_period[i]);
c@277: 
c@277:     d_vec_t txwt (prange_max - prange_min + 1);
c@277:     d_vec_t scorecands (txwt.size());
c@277: 
c@277:     for (uint j=0;j<txwt.size();j++)
c@277:     {
c@277:       double mu = static_cast<double> (beat_period[i]);
c@277:       txwt[j] = exp( -0.5*pow(tightness * log((round(2*mu)-j)/mu),2));
c@277: 
c@277:       scorecands[j] = txwt[j] * cumscore[i+prange_min+j];
c@277:     }
c@277: 
c@277:     double vv = get_max_val(scorecands);
c@277:     int xx = get_max_ind(scorecands);
c@277: 
c@277:     cumscore[i] = alpha*vv + (1.-alpha)*localscore[i];
c@277: 
c@277:     backlink[i] = i+prange_min+xx;
c@277: 
c@277:   }
c@277: 
c@277: 
c@277:   d_vec_t tmp_vec;
c@277:   for (uint i=cumscore.size() - beat_period[beat_period.size()-1] ; i<cumscore.size(); i++)
c@277:   {
c@277:     tmp_vec.push_back(cumscore[i]);
c@277:   }  
c@277: 
c@277:   int startpoint = get_max_ind(tmp_vec) + cumscore.size() - beat_period[beat_period.size()-1] ;
c@277: 
c@277:   i_vec_t ibeats;
c@277:   ibeats.push_back(startpoint);
c@277:   while (backlink[ibeats.back()] > 3*beat_period[0])
c@277:   {
c@277:     ibeats.push_back(backlink[ibeats.back()]);
c@277:   }
c@277:   
c@277: 
c@277:   for (uint i=0; i<ibeats.size(); i++)
c@277:   { 
c@277: 
c@277:     beats.push_back( static_cast<double>(ibeats[i]) );
c@277: 
c@277:  //   cout << ibeats[i] << " "  << beats[i] <<endl;
c@277:   }
c@277: }
c@277: 
c@277: