1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
|
/*
* Copyright (c) 2008 MUSIC TECHNOLOGY GROUP (MTG)
* UNIVERSITAT POMPEU FABRA
*
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/*! \file peakDetection.c
* \brief peak detection algorithm and functions
*/
#include "sms.h"
/*! \brief function used for the parabolic interpolation of the spectral peaks
*
* it performs the interpolation in a log scale and
* stores the location in pFDiffFromMax and
*
* \param fMaxVal value of max bin
* \param fLeftBinVal value for left bin
* \param fRightBinVal value for right bin
* \param pFDiffFromMax location of the tip as the difference from the top bin
* \return the peak height
*/
static sfloat PeakInterpolation (sfloat fMaxVal, sfloat fLeftBinVal,
sfloat fRightBinVal, sfloat *pFDiffFromMax)
{
/* get the location of the tip of the parabola */
*pFDiffFromMax = (.5 * (fLeftBinVal - fRightBinVal) /
(fLeftBinVal - (2*fMaxVal) + fRightBinVal));
/* return the value at the tip */
return(fMaxVal - (.25 * (fLeftBinVal - fRightBinVal) *
*pFDiffFromMax));
}
/*! \brief detect the next local maximum in the spectrum
*
* stores the value in pFMaxVal
*
* \todo export this to sms.h and wrap in pysms
*
* \param pFMagSpectrum magnitude spectrum
* \param iHighBinBound highest bin to search
* \param pICurrentLoc current bin location
* \param pFMaxVal value of the maximum found
* \param fMinPeakMag minimum magnitude to accept a peak
* \return the bin location of the maximum
*/
static int FindNextMax ( sfloat *pFMagSpectrum, int iHighBinBound,
int *pICurrentLoc, sfloat *pFMaxVal, sfloat fMinPeakMag)
{
int iCurrentBin = *pICurrentLoc;
sfloat fPrevVal = pFMagSpectrum[iCurrentBin - 1];
sfloat fCurrentVal = pFMagSpectrum[iCurrentBin];
sfloat fNextVal = (iCurrentBin >= iHighBinBound)
? 0 : pFMagSpectrum[iCurrentBin + 1];
/* try to find a local maximum */
while (iCurrentBin <= iHighBinBound)
{
if (fCurrentVal > fMinPeakMag &&
fCurrentVal >= fPrevVal &&
fCurrentVal >= fNextVal)
break;
iCurrentBin++;
fPrevVal = fCurrentVal;
fCurrentVal = fNextVal;
fNextVal = pFMagSpectrum[1+iCurrentBin];
}
/* save the current location, value of maximum and return */
/* location of max */
*pICurrentLoc = iCurrentBin + 1;
*pFMaxVal = fCurrentVal;
return(iCurrentBin);
}
/*! \brief function to detect the next spectral peak
*
* \param pFMagSpectrum magnitude spectrum
* \param iHighestBin highest bin to search
* \param pICurrentLoc current bin location
* \param pFPeakMag magnitude value of peak
* \param pFPeakLoc location of peak
* \param fMinPeakMag minimum magnitude to accept a peak
* \return 1 if found, 0 if not
*/
static int FindNextPeak (sfloat *pFMagSpectrum, int iHighestBin,
int *pICurrentLoc, sfloat *pFPeakMag, sfloat *pFPeakLoc,
sfloat fMinPeakMag)
{
int iPeakBin = 0; /* location of the local peak */
sfloat fPeakMag = 0; /* value of local peak */
/* keep trying to find a good peak while inside the freq range */
while ((iPeakBin = FindNextMax(pFMagSpectrum, iHighestBin,
pICurrentLoc, &fPeakMag, fMinPeakMag))
<= iHighestBin)
{
/* get the neighboring samples */
sfloat fDiffFromMax = 0;
sfloat fLeftBinVal = pFMagSpectrum[iPeakBin - 1];
sfloat fRightBinVal = pFMagSpectrum[iPeakBin + 1];
if (fLeftBinVal <= 0 || fRightBinVal <= 0) //ahah! there you are!
continue;
/* interpolate the spectral samples to obtain
a more accurate magnitude and freq */
*pFPeakMag = PeakInterpolation (fPeakMag, fLeftBinVal,
fRightBinVal, &fDiffFromMax);
*pFPeakLoc = iPeakBin + fDiffFromMax;
return (1);
}
/* if it does not find a peak return 0 */
return (0);
}
/*! \brief get the corresponding phase value for a given peak
*
* performs linear interpolation for a more accurate phase
* \param pPhaseSpectrum phase spectrum
* \param fPeakLoc location of peak
* \return the phase value
*/
static sfloat GetPhaseVal (sfloat *pPhaseSpectrum, sfloat fPeakLoc)
{
int bin = (int) fPeakLoc;
sfloat fFraction = fPeakLoc - bin,
fLeftPha = pPhaseSpectrum[bin],
fRightPha = pPhaseSpectrum[bin+1];
/* check for phase wrapping */
if (fLeftPha - fRightPha > 1.5 * PI)
fRightPha += TWO_PI;
else if (fRightPha - fLeftPha > 1.5 * PI)
fLeftPha += TWO_PI;
/* return interpolated phase */
return (fLeftPha + fFraction * (fRightPha - fLeftPha));
}
/*! \brief find the prominent spectral peaks
*
* uses a dB spectrum
*
* \param sizeSpec size of magnitude spectrum
* \param pMag pointer to power spectrum
* \param pPhase pointer to phase spectrum
* \param pSpectralPeaks pointer to array of peaks
* \param pPeakParams peak detection parameters
* \return the number of peaks found
*/
int sms_detectPeaks (int sizeSpec, sfloat *pMag, sfloat *pPhase,
SMS_Peak *pSpectralPeaks, SMS_PeakParams *pPeakParams)
{
static int iFirstBin, iHighestBin, sizeFft;
static sfloat fInvSizeFft;
static int sizeSpecStatic = 0;
/* allocate memory if sizeSpec has changed or this is the first run */
if(sizeSpecStatic != sizeSpec)
{
sizeSpecStatic = sizeSpec;
//printf("sizeSpecStatic: %d \n", sizeSpecStatic);
sizeFft = sizeSpec << 1;
fInvSizeFft = 1.0 / sizeFft;
/* make sure to start on the 2nd bin so interpolation is possible QUESTION: why not allow a peak in bin 1 or 0? */
/* rte: changed the first argument of MAX from 2 to 1 */
iFirstBin = MAX (1, sizeFft * pPeakParams->fLowestFreq /
pPeakParams->iSamplingRate);
iHighestBin = MIN (sizeSpec-1,
sizeFft * pPeakParams->fHighestFreq /
pPeakParams->iSamplingRate);
}
/* clear peak structure */
memset (pSpectralPeaks, 0, pPeakParams->iMaxPeaks * sizeof(SMS_Peak));
/* set starting search values */
int iCurrentLoc = iFirstBin;
int iPeak = 0; /* index for spectral search */
sfloat fPeakMag = 0.0; /* magnitude of peak */
sfloat fPeakLoc = 0.0; /* location of peak */
/* find peaks */
while ((iPeak < pPeakParams->iMaxPeaks) &&
(FindNextPeak(pMag, iHighestBin,
&iCurrentLoc, &fPeakMag, &fPeakLoc, pPeakParams->fMinPeakMag) == 1))
{
/* store peak values */
pSpectralPeaks[iPeak].fFreq = pPeakParams->iSamplingRate * fPeakLoc * fInvSizeFft;
pSpectralPeaks[iPeak].fMag = fPeakMag;
pSpectralPeaks[iPeak].fPhase = GetPhaseVal(pPhase, fPeakLoc);
iPeak++;
}
/* return the number of peaks found */
return (iPeak);
}
|