28 #define INFINITE_MAGNITUD 1000000
34 : mInput(
"Input", this),
35 mOutput(
"Output", this ),
36 mFundFreqValue(
"Fund Freq Value", this )
42 : mInput(
"Input", this ),
43 mOutput(
"Output", this ),
44 mFundFreqValue(
"Fund Freq Value", this )
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());
79 mConfig.SetMaxCandMagDiff(mMaxCandMagDiff);
80 mConfig.SetMaxFundFreqError(mMaxFundFreqError);
81 mConfig.SetNInt(mnInt);
88 mConfig.SetPMnPeaks(mPMnPeaks);
89 mConfig.SetMPnPeaks(mMPnPeaks);
90 mConfig.SetPMCont(mPMCont);
91 mConfig.SetMPCont(mMPCont);
92 mConfig.SetNMaxCandidates(
TSize(mnMaxCandidates));
115 "FundFreqDet::Detection: negative number of candidates wanted");
119 "FundFreqDet::Detection:Number of candidates wanted bigger "
120 "than the maximum configured on the algorithm");
135 if(!peaks.HasIndexArray())
137 peaks.AddIndexArray();
146 DataArray& peakMagnitudes=peaks.GetMagBuffer();
147 DataArray& peakFrequencies=peaks.GetFreqBuffer();
148 DataArray& peakBinPosBuffer=peaks.GetBinPosBuffer();
150 const TData spectralRes = peakFrequencies[nMaxMagPeak]/peakBinPosBuffer[nMaxMagPeak];
152 TData lowestFundFreqBinPos = mLowestFundFreq/spectralRes;
154 while((z<peaks.
GetnPeaks()) && (peakBinPosBuffer[z]<lowestFundFreqBinPos))
161 for(
int i=z; i<nMaxMagPeak; i++)
163 if(peakMagnitudes[i] < maxMag - 30)
168 TData peaklimitBinPos = 3000.0/spectralRes;
169 for (
int i=peaks.
GetnPeaks()-1; i > nMaxMagPeak; i--)
171 if (peakBinPosBuffer[i] <= peaklimitBinPos)
break;
177 a = - 10*spectralRes/
TData(1000.0);
178 b = maxMag - 50 - a*(
TData)peakBinPosBuffer[nMaxMagPeak];
179 for(
int i=nMaxMagPeak+1; i<z; i++)
181 y = peakMagnitudes[i];
182 x = peakBinPosBuffer[i];
189 const IndexArray & peakIndexes = peaks.GetIndexArray();
191 if (peakIndexes.
Size() <= 0)
199 maxMag = peakMagnitudes[peakIndexes[nMaxMagPeak]];
204 tmpFreq.AddCandidatesFreq();
205 tmpFreq.AddCandidatesErr();
210 DataArray & candidatesFrequency = tmpFreq.GetCandidatesFreq();
211 DataArray & candidatesError = tmpFreq.GetCandidatesErr();
214 if( IsGoodCandidate(mReferenceFundFreq) )
215 tmpFreq.
AddElem(mReferenceFundFreq);
218 TIndex nMaxMagPeak2 = nMaxMagPeak;
219 TIndex nMaxMagPeak3 = nMaxMagPeak;
220 if(peakIndexes.Size() >= 2)
223 peakMagnitudes[peakIndexes[nMaxMagPeak]]=-2000;
225 if(peakIndexes.Size() >= 3)
229 aux = peakMagnitudes[peakIndexes[nMaxMagPeak2]];
230 peakMagnitudes[peakIndexes[nMaxMagPeak2]]=-2000;
233 if ( IsGoodCandidate(peakFrequencies[peakIndexes[nMaxMagPeak3]]) )
234 tmpFreq.
AddElem(peakFrequencies[peakIndexes[nMaxMagPeak3]]);
236 peakMagnitudes[peakIndexes[nMaxMagPeak2]]=aux;
239 if ( IsGoodCandidate(peakFrequencies[peakIndexes[nMaxMagPeak2]]) )
240 tmpFreq.
AddElem(peakFrequencies[peakIndexes[nMaxMagPeak2]]);
243 peakMagnitudes[peakIndexes[nMaxMagPeak]]=maxMag;
246 if ( IsGoodCandidate(peakFrequencies[peakIndexes[nMaxMagPeak]]) )
247 tmpFreq.
AddElem(peakFrequencies[peakIndexes[nMaxMagPeak]]);
250 for (
int i=0; i < nMaxMagPeak; i++ )
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]]);
263 for (
int i = nMaxMagPeak+1; i<peakIndexes.Size(); i++)
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;
274 for (
int i = 0; i<peakIndexes.Size(); i++ )
276 if (i==nMaxMagPeak)
continue;
277 for (
int j = i+1; j<peakIndexes.Size(); j++)
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))
289 for (
int i=0; i<peakIndexes.Size(); i++ )
291 for (
int j=1; j <= mnInt; j++)
294 if (candidatesFrequency.
Size() >= mnMaxCandidates)
break;
295 freq = peakFrequencies[peakIndexes[i]]/j;
296 if (freq < peakFrequencies[peakIndexes[nMaxMagPeak]]*1.1)
297 if (IsGoodCandidate(freq))
302 if(candidatesFrequency.
Size() <= 0)
309 TData oneOverSeventyFive = 0.013333333333f;
311 const int nPeaks = peaks.GetIndexArray().Size();
323 for (i=0; i<nPeaks; i++)
325 TData Mag = peakMagnitudes[peakIndexes[i]];
327 tmpMagFactor =
TData(1.0) - tmpMagFactor*oneOverSeventyFive;
328 if (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];
338 int maxNMP =
CLAM_min(mMPnPeaks,nPeaks);
343 maxNPM =
CLAM_min(mPMnPeaks,nPeaks);
345 for (
int i=0; i<candidatesFrequency.
Size(); i++)
347 TData myFrequency = candidatesFrequency[i];
348 TData myError = WeightCandidate(myFrequency,peaks, magFactor, magFactor34q, magFactor4r, maxNMP, maxNPM);
349 candidatesError[i] = myError;
357 DataArray & candidates2Frequency = tmpFreq2.GetCandidatesFreq();
358 DataArray & candidates2Error = tmpFreq2.GetCandidatesErr();
360 for (
int i=0;i<candidatesFrequency.
Size();i++)
364 tmpFreq2.
AddElem(candidatesFrequency[i],candidatesError[i]);
367 bool addedNearOne=
false;
368 for(
int j=0; j<candidates2Frequency.
Size(); j++)
370 if (candidatesFrequency[i]<=0.95*candidates2Frequency[j])
continue;
371 if (candidatesFrequency[i]>=1.1*candidates2Frequency[j])
continue;
373 if(candidates2Error[j] > candidatesError[i])
375 candidates2Frequency[j] = candidatesFrequency[i];
376 candidates2Error[j] = candidatesError[i];
380 tmpFreq2.
AddElem(candidatesFrequency[i],candidatesError[i]);
384 TData nMinimum = std::min(3,candidates2Frequency.
Size());
385 for(
int i=0; i<nMinimum; i++)
387 TData & myFrequency = candidates2Frequency[i];
389 TData High = myFrequency*
TData(1.1);
390 TData Incr = std::max(
TData(1.0), myFrequency*
TData(.008));
392 TData FinalPitch = candidates2Frequency[i];
393 TData FinalError = candidates2Error[i];
394 for(TData lPitch = Low; lPitch <=High; lPitch += Incr)
396 TData lErr = WeightCandidate(lPitch,peaks, magFactor, magFactor34q, magFactor4r, maxNMP, maxNPM);
397 if (lPitch > peakFrequencies[peakIndexes[nMaxMagPeak]]*1.1)
399 if (lErr < FinalError)
405 candidates2Frequency[i] = FinalPitch;
406 candidates2Error[i] = FinalError;
413 for(
int i=0; i<nCandidates; i++)
414 if(candidates2Error[i] <= mMaxFundFreqError)
415 outFreq.
AddElem(candidates2Frequency[i], candidates2Error[i]);
425 mReferenceFundFreq = outFreq.
GetFreq(0);
432 const DataArray& magFactor4r,
int maxNMP,
int maxNPM)
const
435 const int nPeaks = peaks.GetIndexArray().Size();
436 const IndexArray & peakIndexes = peaks.GetIndexArray();
437 DataArray& peakFrequencies=peaks.GetFreqBuffer();
443 TData HarmonicPm = freq;
445 TData lastFreq=peakFrequencies[peakIndexes[nPeaks-1]];
448 TData HarmonicMp = freq;
454 bool finishedPM =
false;
455 bool finishedMP =
false;
457 TData tenTimesFreq = freq*10.f;
458 bool isFreqHigh = (freq>500.);
459 TData oneOverFundFreq = 1./freq;
461 for (i=0; i<nPeaks; i++)
467 if (HarmonicPm > lastFreq)
471 if (finishedMP)
break;
475 Peak = GetClosestPeak(HarmonicPm,Peak, peakIndexes, peakFrequencies);
477 TData Freq = peakFrequencies[peakIndexes[Peak]];
480 TData FreqDistance =
Abs(Freq - HarmonicPm);
483 Tmp = FreqDistance /
CLAM_sqrt(HarmonicPm);
485 Tmp = FreqDistance *
CLAM_pow(HarmonicPm, -mPMp);
487 ErrorPM += (Tmp +magFactor[Peak] * (mPMq * Tmp - mPMr));
495 TData Freq =
TData(peakFrequencies[peakIndexes[i]]);
497 if ( (isFreqHigh) && (Freq < 100))
500 HarmonicMp = GetClosestHarmonic(Freq,freq, oneOverFundFreq);
501 TData FreqDistance =
Abs(Freq - HarmonicMp);
506 Tmp = FreqDistance *
CLAM_pow(Freq, -mMPp);
508 ErrorMP += magFactor34q[i] * Tmp - magFactor4r[i];
509 if (Freq > tenTimesFreq){
514 if (finishedPM)
break;
523 ErrorPM = 20 + (ErrorPM-20)*(ErrorPM-20);
525 ErrorMP = 20 + (ErrorMP-20)*(ErrorMP-20);
527 return (mPMCont * ErrorPM/nPM + mMPCont * ErrorMP/nMP);
533 int FundFreqDetect::GetClosestPeak(
TData freq,
int firstPeak,
const IndexArray& peakIndexes,
const DataArray & peakFrequencies)
const
535 const int size = peakIndexes.Size();
536 int bestpeak = firstPeak;
538 for (
int peak=firstPeak; peak < size; peak++)
540 TData nextdistance =
Abs(freq - peakFrequencies[peakIndexes[peak]]);
541 if (nextdistance >= distance)
544 distance=nextdistance;
550 TData FundFreqDetect::GetClosestHarmonic(
TData peak,
TData fundfreq,
TData oneOverFundfreq)
const
555 return floor(peak*oneOverFundfreq+.5)*fundfreq;
558 bool FundFreqDetect::IsGoodCandidate(
TData freq)
const
560 return (freq >= mLowestFundFreq) && (freq <= mHighestFundFreq);