CLAM-Development  1.4.0
FundFreqDetect.cxx
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2004 MUSIC TECHNOLOGY GROUP (MTG)
3  * UNIVERSITAT POMPEU FABRA
4  *
5  *
6  * This program is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software
18  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
19  *
20  */
21 
22 #include "Complex.hxx"
23 #include "FundFreqDetect.hxx"
24 #include "Fundamental.hxx"
25 #include "SpectralPeakArray.hxx"
26 #include <cmath>
27 
28 #define INFINITE_MAGNITUD 1000000
29 
30 namespace CLAM
31 {
32 
34  : mInput( "Input", this),
35  mOutput( "Output", this ),
36  mFundFreqValue( "Fund Freq Value", this )
37  {
39  }
40 
42  : mInput( "Input", this ),
43  mOutput( "Output", this ),
44  mFundFreqValue( "Fund Freq Value", this )
45  {
46  Configure(c);
47  }
48 
50 
51  /* Configure the Processing Object according to the Config object */
52  bool FundFreqDetect::ConcreteConfigure(const ProcessingConfig& c)
53  {
54  CopyAsConcreteConfig(mConfig, c);
55 
56  mReferenceFundFreq = mConfig.GetReferenceFundFreq();
57  mLowestFundFreq = mConfig.GetLowestFundFreq();
58  mHighestFundFreq = mConfig.GetHighestFundFreq();
59  mMaxCandMagDiff = mConfig.GetMaxCandMagDiff();
60  mMaxFundFreqError = mConfig.GetMaxFundFreqError();
61  mnInt = mConfig.GetNInt();
62  mPMp = mConfig.GetPMp();
63  mPMq = mConfig.GetPMq();
64  mPMr = mConfig.GetPMr();
65  mMPp = mConfig.GetMPp();
66  mMPq = mConfig.GetMPq();
67  mMPr = mConfig.GetMPr();
68  mPMnPeaks = mConfig.GetPMnPeaks();
69  mMPnPeaks = mConfig.GetMPnPeaks();
70  mPMCont = mConfig.GetPMCont();
71  mMPCont = mConfig.GetMPCont();
72  mnMaxCandidates= TData(mConfig.GetNMaxCandidates());
73 
74  return true;
75  }
76 
78  {
79  mConfig.SetMaxCandMagDiff(mMaxCandMagDiff);
80  mConfig.SetMaxFundFreqError(mMaxFundFreqError);
81  mConfig.SetNInt(mnInt);
82  mConfig.SetPMp(mPMp);
83  mConfig.SetPMq(mPMq);
84  mConfig.SetPMr(mPMr);
85  mConfig.SetMPp(mMPp);
86  mConfig.SetMPq(mMPq);
87  mConfig.SetMPr(mMPr);
88  mConfig.SetPMnPeaks(mPMnPeaks);
89  mConfig.SetMPnPeaks(mMPnPeaks);
90  mConfig.SetPMCont(mPMCont);
91  mConfig.SetMPCont(mMPCont);
92  mConfig.SetNMaxCandidates(TSize(mnMaxCandidates));
93  return mConfig;
94  }
95 
96  /* The supervised Do() function */
97  bool FundFreqDetect::Do(void)
98  {
99  mOutput.GetData().SetnMaxCandidates(1);
100 
101  bool result = Do( mInput.GetData(), mOutput.GetData() );
102  mInput.Consume();
103  mOutput.Produce();
104 
105  return result;
106  }
107 
108  /* The unsupervised Do() function */
110  {
111  outFreq.Init();
112 
113  // Check Number of Candidates required
114  CLAM_ASSERT (outFreq.GetnMaxCandidates() > 0,
115  "FundFreqDet::Detection: negative number of candidates wanted");
116 
117  // See if the number of best candidates to be calculated is less than the maximum permitted
118  CLAM_ASSERT (outFreq.GetnMaxCandidates() <= mnMaxCandidates,
119  "FundFreqDet::Detection:Number of candidates wanted bigger "
120  "than the maximum configured on the algorithm");
121 
122  // not enough peak information available for fundamental frequency detection");
123  if (peaks.GetnPeaks() <= 0)
124  {
125  mFundFreqValue.SendControl(0.0f);
126  return false;
127  }
128 
129  // Calculate Maximun Magnitude Peak
130  TIndex nMaxMagPeak = peaks.GetMaxMagPos();
131  TData maxMag = peaks.GetMag(nMaxMagPeak);
132 
133  // 1.- SELECT PEAKS
134  // Add an index to the PeakArray
135  if(!peaks.HasIndexArray())
136  {
137  peaks.AddIndexArray();
138  peaks.UpdateData();
139  peaks.SetnMaxPeaks(peaks.GetnMaxPeaks());
140  }
141 
142  // Reset indices in the peak array
143  peaks.ResetIndices();
144 
145  // Delete peaks below the lowest pitch
146  DataArray& peakMagnitudes=peaks.GetMagBuffer();
147  DataArray& peakFrequencies=peaks.GetFreqBuffer();
148  DataArray& peakBinPosBuffer=peaks.GetBinPosBuffer();
149 
150  const TData spectralRes = peakFrequencies[nMaxMagPeak]/peakBinPosBuffer[nMaxMagPeak];
151 
152  TData lowestFundFreqBinPos = mLowestFundFreq/spectralRes;
153  TIndex z=0;
154  while((z<peaks.GetnPeaks()) && (peakBinPosBuffer[z]<lowestFundFreqBinPos))
155  {
156  peaks.DeleteIndex(z);
157  z++;
158  }
159 
160  // Before the maximum magnitude peak
161  for(int i=z; i<nMaxMagPeak; i++)
162  {
163  if(peakMagnitudes[i] < maxMag - 30)
164  peaks.DeleteIndex(i);
165  }
166 
167  // Delete peaks above 3000
168  TData peaklimitBinPos = 3000.0/spectralRes;
169  for (int i=peaks.GetnPeaks()-1; i > nMaxMagPeak; i--)
170  {
171  if (peakBinPosBuffer[i] <= peaklimitBinPos) break;
172  peaks.DeleteIndex(i);
173  }
174 
175  // After the maximum magnitude peak
176  TData x,y,a,b;
177  a = - 10*spectralRes/TData(1000.0);
178  b = maxMag - 50 - a*(TData)peakBinPosBuffer[nMaxMagPeak];
179  for(int i=nMaxMagPeak+1; i<z; i++)
180  {
181  y = peakMagnitudes[i];
182  x = peakBinPosBuffer[i];
183  if(y < (a*x+b))
184  {
185  peaks.DeleteIndex(i);
186  }
187  }
188 
189  const IndexArray & peakIndexes = peaks.GetIndexArray();
190  // If there no valid peaks for calculate a fundamental frequency
191  if (peakIndexes.Size() <= 0)
192  {
193  mFundFreqValue.SendControl(0.0f);
194  return false;
195  }
196 
197  // Find maximun magnitude peak from the selected ones
198  nMaxMagPeak = peaks.GetMaxMagIndex(); // only indexed peaks
199  maxMag = peakMagnitudes[peakIndexes[nMaxMagPeak]];
200 
201  // 2.- FIND mnMaxCandidates CANDIDATES
202 
203  Fundamental tmpFreq; // this will be used throughout the algorithm to allocate new candidates
204  tmpFreq.AddCandidatesFreq();
205  tmpFreq.AddCandidatesErr();
206  tmpFreq.UpdateData();
207  tmpFreq.SetnCandidates(0);
208  tmpFreq.SetnMaxCandidates(int(mnMaxCandidates));
209 
210  DataArray & candidatesFrequency = tmpFreq.GetCandidatesFreq();
211  DataArray & candidatesError = tmpFreq.GetCandidatesErr();
212 
213  // 2.0.- Reference Fundamental Frequency
214  if( IsGoodCandidate(mReferenceFundFreq) )
215  tmpFreq.AddElem(mReferenceFundFreq);
216 
217  // 2.1.- Three maximum magnitude peaks and its integer ratios
218  TIndex nMaxMagPeak2 = nMaxMagPeak;
219  TIndex nMaxMagPeak3 = nMaxMagPeak;
220  if(peakIndexes.Size() >= 2)
221  {
222  // find second max magnitude peak
223  peakMagnitudes[peakIndexes[nMaxMagPeak]]=-2000;
224  nMaxMagPeak2 = peaks.GetMaxMagIndex();
225  if(peakIndexes.Size() >= 3)
226  {
227  TData aux;
228  // find third max magnitude peak
229  aux = peakMagnitudes[peakIndexes[nMaxMagPeak2]];
230  peakMagnitudes[peakIndexes[nMaxMagPeak2]]=-2000;
231  nMaxMagPeak3 = peaks.GetMaxMagIndex();
232  // add candidate
233  if ( IsGoodCandidate(peakFrequencies[peakIndexes[nMaxMagPeak3]]) )
234  tmpFreq.AddElem(peakFrequencies[peakIndexes[nMaxMagPeak3]]);
235  // restore second peak information
236  peakMagnitudes[peakIndexes[nMaxMagPeak2]]=aux;
237  }
238  // add candidate
239  if ( IsGoodCandidate(peakFrequencies[peakIndexes[nMaxMagPeak2]]) )
240  tmpFreq.AddElem(peakFrequencies[peakIndexes[nMaxMagPeak2]]);
241 
242  // restore first peak information
243  peakMagnitudes[peakIndexes[nMaxMagPeak]]=maxMag;
244  }
245  // Add peaks as candidates
246  if ( IsGoodCandidate(peakFrequencies[peakIndexes[nMaxMagPeak]]) )
247  tmpFreq.AddElem(peakFrequencies[peakIndexes[nMaxMagPeak]]);
248 
249  // 2.2.- Peaks below the maximum magnitude peak (except for the 3 max peaks)
250  for (int i=0; i < nMaxMagPeak; i++ )
251  {
252  // be careful not to exceed the maximun permitted
253  if (candidatesFrequency.Size() >= mnMaxCandidates) break;
254  if (i==nMaxMagPeak2) continue;
255  if (i==nMaxMagPeak3) continue;
256  if (peakMagnitudes[peakIndexes[i]] <= maxMag - mMaxCandMagDiff ) continue;
257  if (! IsGoodCandidate(peakFrequencies[peakIndexes[i]]) ) continue;
258  tmpFreq.AddElem(peakFrequencies[peakIndexes[i]]);
259  }
260 
261  // 2.3.- Frequency offset between peaks above the maximun magnitude peak and the maximun magnitude peak
262  TData freq;
263  for (int i = nMaxMagPeak+1; i<peakIndexes.Size(); i++)
264  {
265  // be careful not to exceed the maximun permitted
266  if (candidatesFrequency.Size() >= mnMaxCandidates) break;
267  freq = peakFrequencies[peakIndexes[i]] - peakFrequencies[peakIndexes[nMaxMagPeak]];
268  if (freq >= peakFrequencies[peakIndexes[nMaxMagPeak]]*1.1) continue;
269  if (!IsGoodCandidate(freq)) continue;
270  tmpFreq.AddElem(freq);
271  }
272 
273  // 2.4.- Frequency offset between peaks
274  for (int i = 0; i<peakIndexes.Size(); i++ )
275  {
276  if (i==nMaxMagPeak) continue;
277  for (int j = i+1; j<peakIndexes.Size(); j++)
278  {
279  // be careful not to exceed the maximun permitted
280  if (candidatesFrequency.Size() >= mnMaxCandidates) break;
281  freq = peakFrequencies[peakIndexes[j]] - peakFrequencies[peakIndexes[i]];
282  if (freq < peakFrequencies[peakIndexes[nMaxMagPeak]]*1.1)
283  if (IsGoodCandidate(freq))
284  tmpFreq.AddElem(freq);
285  }
286  }
287 
288  // 2.5.- Frequencies related to peaks by integer ratios (before: except for the 3 maximun peaks. not now)
289  for (int i=0; i<peakIndexes.Size(); i++ )
290  {
291  for (int j=1; j <= mnInt; j++)
292  {
293  // be careful not to exceed the maximun permitted
294  if (candidatesFrequency.Size() >= mnMaxCandidates) break;
295  freq = peakFrequencies[peakIndexes[i]]/j;
296  if (freq < peakFrequencies[peakIndexes[nMaxMagPeak]]*1.1)
297  if (IsGoodCandidate(freq))
298  tmpFreq.AddElem(freq);
299  }
300  }
301 
302  if(candidatesFrequency.Size() <= 0)
303  {
304  mFundFreqValue.SendControl(0.0f);
305  return false;
306  }
307 
308  // 3.- CALCULATE ERRORS (TMW procedure)
309  TData oneOverSeventyFive = 0.013333333333f;
310 
311  const int nPeaks = peaks.GetIndexArray().Size();
312 
313  int i;
314 
315  //These are all needed by the WeightCandidate and it is much better to compute just once
316  DataArray magFactor(nPeaks);
317  DataArray magFactor3(nPeaks);
318  DataArray magFactor4(nPeaks);
319  DataArray magFactor4r(nPeaks);
320  DataArray magFactor4q(nPeaks);
321  DataArray magFactor34q(nPeaks);
322 
323  for (i=0; i<nPeaks; i++)
324  {
325  TData Mag = peakMagnitudes[peakIndexes[i]];
326  TData tmpMagFactor = TData(CLAM_max(0.0,maxMag - Mag + 20.0));
327  tmpMagFactor = TData(1.0) - tmpMagFactor*oneOverSeventyFive;
328  if (tmpMagFactor < 0)
329  tmpMagFactor = 0;
330  magFactor[i] = tmpMagFactor;
331  magFactor3[i] = tmpMagFactor*tmpMagFactor*tmpMagFactor;
332  magFactor4[i] = magFactor3[i]*tmpMagFactor;
333  magFactor4r[i] = magFactor4[i]*mMPr;
334  magFactor4q[i] = magFactor4[i]*mMPq;
335  magFactor34q[i] = magFactor3[i] + magFactor4q[i];
336  }
337 
338  int maxNMP = CLAM_min(mMPnPeaks,nPeaks);
339  // predicted to measured mismatch error
340  int maxNPM = 10;
341  //xamat: does not depend on variable data: compute out of the loop!!
342  if (nPeaks > 4)
343  maxNPM = CLAM_min(mPMnPeaks,nPeaks);
344 
345  for (int i=0; i<candidatesFrequency.Size(); i++)
346  {
347  TData myFrequency = candidatesFrequency[i];
348  TData myError = WeightCandidate(myFrequency,peaks, magFactor, magFactor34q, magFactor4r, maxNMP, maxNPM);
349  candidatesError[i] = myError;
350  }
351 
352  // 4.- CHOOSE THE BEST CANDIDATES: choose the FundFreq.NCandidates() candidates with the smallest error
353 
354  tmpFreq.SortByError();
355  Fundamental tmpFreq2;
356  tmpFreq2.SetnMaxCandidates(candidatesFrequency.Size());
357  DataArray & candidates2Frequency = tmpFreq2.GetCandidatesFreq();
358  DataArray & candidates2Error = tmpFreq2.GetCandidatesErr();
359 
360  for (int i=0;i<candidatesFrequency.Size();i++)
361  {
362  if (i<=0)
363  {
364  tmpFreq2.AddElem(candidatesFrequency[i],candidatesError[i]);
365  continue;
366  }
367  bool addedNearOne=false;
368  for(int j=0; j<candidates2Frequency.Size(); j++)
369  {
370  if (candidatesFrequency[i]<=0.95*candidates2Frequency[j]) continue;
371  if (candidatesFrequency[i]>=1.1*candidates2Frequency[j]) continue;
372  addedNearOne = true;
373  if(candidates2Error[j] > candidatesError[i])
374  {
375  candidates2Frequency[j] = candidatesFrequency[i];
376  candidates2Error[j] = candidatesError[i];
377  }
378  }
379  if(!addedNearOne)
380  tmpFreq2.AddElem(candidatesFrequency[i],candidatesError[i]);
381  }
382 
383  // 5.- SEARCH AROUND FOR A RELATIVE MINIMUM
384  TData nMinimum = std::min(3,candidates2Frequency.Size());
385  for(int i=0; i<nMinimum; i++)
386  {
387  TData & myFrequency = candidates2Frequency[i];
388  TData Low = myFrequency*TData(.9);
389  TData High = myFrequency*TData(1.1);
390  TData Incr = std::max(TData(1.0), myFrequency*TData(.008));
391 
392  TData FinalPitch = candidates2Frequency[i];
393  TData FinalError = candidates2Error[i];
394  for(TData lPitch = Low; lPitch <=High; lPitch += Incr)
395  {
396  TData lErr = WeightCandidate(lPitch,peaks, magFactor, magFactor34q, magFactor4r, maxNMP, maxNPM);
397  if (lPitch > peakFrequencies[peakIndexes[nMaxMagPeak]]*1.1)
398  lErr +=10;
399  if (lErr < FinalError)
400  {
401  FinalPitch = lPitch;
402  FinalError = lErr;
403  }
404  }
405  candidates2Frequency[i] = FinalPitch;
406  candidates2Error[i] = FinalError;
407  }
408 
409  // Ordering the minimum
410  tmpFreq2.SortByError();
411 
412  TIndex nCandidates = std::min(outFreq.GetnMaxCandidates(),candidates2Frequency.Size());
413  for(int i=0; i<nCandidates; i++)
414  if(candidates2Error[i] <= mMaxFundFreqError)
415  outFreq.AddElem(candidates2Frequency[i], candidates2Error[i]);
416 
417  if(outFreq.GetnCandidates() == 0)
418  {
419  mFundFreqValue.SendControl(0.0f);
420  return false;
421  }
422 
423  // Added to get into account fundamental frequency for consecutive frames
424  // Set Reference fundFreq to last FundFreq
425  mReferenceFundFreq = outFreq.GetFreq(0);
426  mFundFreqValue.SendControl( mReferenceFundFreq );
427 
428  return true;
429  }
430 
431  TData FundFreqDetect::WeightCandidate(TData freq, const SpectralPeakArray& peaks, const DataArray& magFactor, const DataArray& magFactor34q,
432  const DataArray& magFactor4r, int maxNMP, int maxNPM) const
433  {
434  TData Tmp;
435  const int nPeaks = peaks.GetIndexArray().Size();
436  const IndexArray & peakIndexes = peaks.GetIndexArray();
437  DataArray& peakFrequencies=peaks.GetFreqBuffer();
438  //DataArray& peakMagnitudes=peaks.GetMagBuffer();
439 
440  TData ErrorPM = 0;
441  TData ErrorMP = 0;
442 
443  TData HarmonicPm = freq;
444  TSize nPM = maxNPM;
445  TData lastFreq=peakFrequencies[peakIndexes[nPeaks-1]];
446 
447  // measured to predicted mismatch error
448  TData HarmonicMp = freq;
449  TSize nMP = nPeaks;
450 
451  int Peak =0;
452  int i;
453 
454  bool finishedPM = false;
455  bool finishedMP = false;
456 
457  TData tenTimesFreq = freq*10.f;
458  bool isFreqHigh = (freq>500.);
459  TData oneOverFundFreq = 1./freq;
460 
461  for (i=0; i<nPeaks; i++)
462  {
463  if(!finishedPM)
464  {
465  if(i<maxNPM)
466  {
467  if (HarmonicPm > lastFreq)
468  {
469  nPM = i+1;
470  finishedPM = true;
471  if (finishedMP) break;
472  }
473  else
474  {
475  Peak = GetClosestPeak(HarmonicPm,Peak, peakIndexes, peakFrequencies);
476  //xamat: does not depend on variable data: compute out of the loop!!
477  TData Freq = peakFrequencies[peakIndexes[Peak]];
478  //TData Mag = peakMagnitudes[peakIndexes[Peak]];
479 
480  TData FreqDistance = Abs(Freq - HarmonicPm);
481  //xamat: note that default value for mPMp is 0.5 thus yielding a sqrt
482 #ifdef CLAM_OPTIMIZE
483  Tmp = FreqDistance / CLAM_sqrt(HarmonicPm);
484 #else
485  Tmp = FreqDistance * CLAM_pow(HarmonicPm, -mPMp);
486 #endif
487  ErrorPM += (Tmp +magFactor[Peak] * (mPMq * Tmp - mPMr));
488  HarmonicPm += freq;
489  }
490  }
491  }
492 
493  if(!finishedMP)
494  {
495  TData Freq = TData(peakFrequencies[peakIndexes[i]]);
496  // For high frequency candidates, not get into account too-low peaks
497  if ( (isFreqHigh) && (Freq < 100))
498  continue;
499 
500  HarmonicMp = GetClosestHarmonic(Freq,freq, oneOverFundFreq);
501  TData FreqDistance = Abs(Freq - HarmonicMp);
502  //xamat: note that default value for mMPp is 0.5 thus yielding a sqrt
503 #ifdef CLAM_OPTIMIZE
504  Tmp = FreqDistance / CLAM_sqrt(Freq);
505 #else
506  Tmp = FreqDistance * CLAM_pow(Freq, -mMPp);
507 #endif
508  ErrorMP += magFactor34q[i] * Tmp - magFactor4r[i];
509  if (Freq > tenTimesFreq){
510  if (i > maxNMP)
511  {
512  nMP = i+1;
513  finishedMP = true;
514  if (finishedPM) break;
515  }
516  }
517  }
518  }
519 
520 
521  // total error
522  if (ErrorPM > 20)
523  ErrorPM = 20 + (ErrorPM-20)*(ErrorPM-20);
524  if (ErrorMP > 20)
525  ErrorMP = 20 + (ErrorMP-20)*(ErrorMP-20);
526 
527  return (mPMCont * ErrorPM/nPM + mMPCont * ErrorMP/nMP);
528  }
529 
530  /* Get the closest peak to a given frequency
531  and returns the number of the closest peak
532  there's another parameter, peak, that contains the last peak taken */
533  int FundFreqDetect::GetClosestPeak(TData freq, int firstPeak, const IndexArray& peakIndexes, const DataArray & peakFrequencies) const
534  {
535  const int size = peakIndexes.Size();
536  int bestpeak = firstPeak;
537  TData distance = INFINITE_MAGNITUD;
538  for (int peak=firstPeak; peak < size; peak++)
539  {
540  TData nextdistance = Abs(freq - peakFrequencies[peakIndexes[peak]]);
541  if (nextdistance >= distance)
542  return peak-1;
543  bestpeak = peak;
544  distance=nextdistance;
545  }
546  return bestpeak;
547  }
548 
549  /* Get Closest Harmonic */
550  TData FundFreqDetect::GetClosestHarmonic(TData peak, TData fundfreq, TData oneOverFundfreq) const
551  {
552  if(peak<fundfreq)
553  return fundfreq;
554  //xamat: this should be optimized!
555  return floor(peak*oneOverFundfreq+.5)*fundfreq;
556  }
557 
558  bool FundFreqDetect::IsGoodCandidate(TData freq) const
559  {
560  return (freq >= mLowestFundFreq) && (freq <= mHighestFundFreq);
561  }
562 
563 } // namespace CLAM
564