-
Notifications
You must be signed in to change notification settings - Fork 17
/
PeakFinder.cpp
256 lines (220 loc) · 6.48 KB
/
PeakFinder.cpp
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
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
// PeakFinder.cpp
#include "PeakFinder.h"
#include <iostream>
#include <vector>
#include <algorithm>
#include <cmath>
void diff(std::vector<float> in, std::vector<float>& out)
{
out = std::vector<float>(in.size()-1);
for(int i=1; i<in.size(); ++i)
out[i-1] = in[i] - in[i-1];
}
void vectorElementsProduct(std::vector<float> a, std::vector<float> b, std::vector<float>& out)
{
out = std::vector<float>(a.size());
for(int i=0; i<a.size(); ++i)
out[i] = a[i] * b[i];
}
void findIndicesLessThan(std::vector<float> in, float threshold, std::vector<int>& indices)
{
for(int i=0; i<in.size(); ++i)
if(in[i]<threshold)
indices.push_back(i+1);
}
void selectElementsFromIndices(std::vector<float> in, std::vector<int> indices, std::vector<float>& out)
{
for(int i=0; i<indices.size(); ++i)
out.push_back(in[indices[i]]);
}
void selectElementsFromIndices(std::vector<int> in, std::vector<int> indices, std::vector<int>& out)
{
for(int i=0; i<indices.size(); ++i)
out.push_back(in[indices[i]]);
}
void signVector(std::vector<float> in, std::vector<int>& out)
{
out = std::vector<int>(in.size());
for(int i=0; i<in.size(); ++i)
{
if(in[i]>0)
out[i]=1;
else if(in[i]<0)
out[i]=-1;
else
out[i]=0;
}
}
void scalarProduct(float scalar, std::vector<float> in, std::vector<float>& out)
{
out = std::vector<float>(in.size());
for(int i=0; i<in.size(); ++i)
out[i] = scalar * in[i];
}
void PeakFinder::findPeaks(std::vector<float> x0, std::vector<int>& peakInds, bool includeEndpoints, float extrema)
{
int minIdx = distance(x0.begin(), min_element(x0.begin(), x0.end()));
int maxIdx = distance(x0.begin(), max_element(x0.begin(), x0.end()));
float sel = (x0[maxIdx]-x0[minIdx])/4.0;
int len0 = x0.size();
scalarProduct(extrema, x0, x0);
std::vector<float> dx;
diff(x0, dx);
replace(dx.begin(), dx.end(), 0.0f, -PeakFinder::EPS);
std::vector<float> dx0(dx.begin(), dx.end()-1);
std::vector<float> dx0_1(dx.begin()+1, dx.end());
std::vector<float> dx0_2;
vectorElementsProduct(dx0, dx0_1, dx0_2);
std::vector<int> ind;
findIndicesLessThan(dx0_2, 0, ind); // Find where the derivative changes sign
std::vector<float> x;
float leftMin;
int minMagIdx;
float minMag;
if(includeEndpoints)
{
//x = [x0(1);x0(ind);x0(end)];
selectElementsFromIndices(x0, ind, x);
x.insert(x.begin(), x0[0]);
x.insert(x.end(), x0[x0.size()-1]);
//ind = [1;ind;len0];
ind.insert(ind.begin(), 1);
ind.insert(ind.end(), len0);
minMagIdx = distance(x.begin(), std::min_element(x.begin(), x.end()));
minMag = x[minMagIdx];
//std::cout<<"Hola"<<std::endl;
leftMin = minMag;
}
else
{
selectElementsFromIndices(x0, ind, x);
if(x.size()>2)
{
minMagIdx = distance(x.begin(), std::min_element(x.begin(), x.end()));
minMag = x[minMagIdx];
leftMin = x[0]<x0[0]?x[0]:x0[0];
}
}
int len = x.size();
if(len>2)
{
float tempMag = minMag;
bool foundPeak = false;
int ii;
if(includeEndpoints)
{
// Deal with first point a little differently since tacked it on
// Calculate the sign of the derivative since we tacked the first
// point on it does not neccessarily alternate like the rest.
std::vector<float> xSub0(x.begin(), x.begin()+3);//tener cuidado subvector
std::vector<float> xDiff;//tener cuidado subvector
diff(xSub0, xDiff);
std::vector<int> signDx;
signVector(xDiff, signDx);
if (signDx[0] <= 0) // The first point is larger or equal to the second
{
if (signDx[0] == signDx[1]) // Want alternating signs
{
x.erase(x.begin()+1);
ind.erase(ind.begin()+1);
len = len-1;
}
}
else // First point is smaller than the second
{
if (signDx[0] == signDx[1]) // Want alternating signs
{
x.erase(x.begin());
ind.erase(ind.begin());
len = len-1;
}
}
}
//Skip the first point if it is smaller so we always start on maxima
if ( x[0] >= x[1] )
ii = 0;
else
ii = 1;
//Preallocate max number of maxima
float maxPeaks = ceil((float)len/2.0);
std::vector<int> peakLoc(maxPeaks,0);
std::vector<float> peakMag(maxPeaks,0.0);
int cInd = 1;
int tempLoc;
while(ii < len)
{
ii = ii+1;//This is a peak
//Reset peak finding if we had a peak and the next peak is bigger
//than the last or the left min was small enough to reset.
if(foundPeak)
{
tempMag = minMag;
foundPeak = false;
}
//Found new peak that was lager than temp mag and selectivity larger
//than the minimum to its left.
if( x[ii-1] > tempMag && x[ii-1] > leftMin + sel )
{
tempLoc = ii-1;
tempMag = x[ii-1];
}
//Make sure we don't iterate past the length of our vector
if(ii == len)
break; //We assign the last point differently out of the loop
ii = ii+1; // Move onto the valley
//Come down at least sel from peak
if(!foundPeak && tempMag > sel + x[ii-1])
{
foundPeak = true; //We have found a peak
leftMin = x[ii-1];
peakLoc[cInd-1] = tempLoc; // Add peak to index
peakMag[cInd-1] = tempMag;
cInd = cInd+1;
}
else if(x[ii-1] < leftMin) // New left minima
leftMin = x[ii-1];
}
// Check end point
if(includeEndpoints)
{
if ( x[x.size()-1] > tempMag && x[x.size()-1] > leftMin + sel )
{
peakLoc[cInd-1] = len-1;
peakMag[cInd-1] = x[x.size()-1];
cInd = cInd + 1;
}
else if( !foundPeak && tempMag > minMag )// Check if we still need to add the last point
{
peakLoc[cInd-1] = tempLoc;
peakMag[cInd-1] = tempMag;
cInd = cInd + 1;
}
}
else if(!foundPeak)
{
float minAux = x0[x0.size()-1]<x[x.size()-1]?x0[x0.size()-1]:x[x.size()-1];
if ( x[x.size()-1] > tempMag && x[x.size()-1] > leftMin + sel )
{
peakLoc[cInd-1] = len-1;
peakMag[cInd-1] = x[x.size()-1];
cInd = cInd + 1;
}
else if( !tempMag > minAux + sel)// Check if we still need to add the last point
{
peakLoc[cInd-1] = tempLoc;
peakMag[cInd-1] = tempMag;
cInd = cInd + 1;
}
}
//Create output
if( cInd > 0 )
{
std::vector<int> peakLocTmp(peakLoc.begin(), peakLoc.begin()+cInd-1);
selectElementsFromIndices(ind, peakLocTmp, peakInds);
}
}
//else
//{
//input signal length <= 2
//}
}