GestureRecognitionToolkit  Version: 1.0 Revision: 04-03-15
The Gesture Recognition Toolkit (GRT) is a cross-platform, open-source, c++ machine learning library for real-time gesture recognition.
FastFourierTransform.cpp
1 /*
2  GRT MIT License
3  Copyright (c) <2012> <Nicholas Gillian, Media Lab, MIT>
4 
5  Permission is hereby granted, free of charge, to any person obtaining a copy of this software
6  and associated documentation files (the "Software"), to deal in the Software without restriction,
7  including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
8  and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
9  subject to the following conditions:
10 
11  The above copyright notice and this permission notice shall be included in all copies or substantial
12  portions of the Software.
13 
14  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
15  LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
16  IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
17  WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
18  SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
19  */
20 
21 /*
22  This code is based on Dominic Mazzoni's c++ wrapper, which is based on a free implementation of an FFT
23  by Don Cross (http://www.intersrv.com/~dcross/fft.html) and the FFT algorithms from Numerical Recipes.
24  */
25 
26 #include "FastFourierTransform.h"
27 
28 
29 namespace GRT{
30 
31 FastFourierTransform::FastFourierTransform(){
32  initialized = false;
33  computeMagnitude = true;
34  computePhase = true;
35  windowSize = 0;
36  windowFunction = RECTANGULAR_WINDOW;
37  averagePower = 0;
38 
39  infoLog.setProceedingText("[FastFourierTransform]");
40  warningLog.setProceedingText("[WARNING FastFourierTransform]");
41  errorLog.setProceedingText("[ERROR FastFourierTransform]");
42 
43  initFFT();
44 }
45 
46 FastFourierTransform::FastFourierTransform(const FastFourierTransform &rhs){
47 
48  this->initialized = rhs.initialized;
49  this->computeMagnitude = rhs.computeMagnitude;
50  this->computePhase = rhs.computePhase;
51  this->windowSize = rhs.windowSize;
52  this->windowFunction = rhs.windowFunction;
53  this->averagePower = 0;
54  this->initFFT();
55  this->infoLog = rhs.infoLog;
56  this->warningLog = rhs.warningLog;
57  this->errorLog = rhs.errorLog;
58 
59  if( rhs.initialized ){
60  this->init(rhs.windowSize,rhs.windowFunction,rhs.computeMagnitude,rhs.computePhase);
61 
62  //Copy the fft results
63  for(UINT i=0; i<this->windowSize; i++){
64  this->fftReal[i] = rhs.fftReal[i];
65  this->fftImag[i] = rhs.fftImag[i];
66  this->magnitude[i] = rhs.magnitude[i];
67  this->phase[i] = rhs.phase[i];
68  this->power[i] = rhs.power[i];
69  }
70  }
71 }
72 
73 FastFourierTransform::~FastFourierTransform() {
74 
75 }
76 
77 FastFourierTransform& FastFourierTransform::operator=(const FastFourierTransform &rhs){
78 
79  if( this != &rhs ){
80  this->initialized = rhs.initialized;
81  this->computeMagnitude = rhs.computeMagnitude;
82  this->computePhase = rhs.computePhase;
83  this->windowSize = rhs.windowSize;
84  this->windowFunction = rhs.windowFunction;
85  this->averagePower = 0;
86  this->initFFT();
87 
88  if( rhs.initialized ){
89  this->init(rhs.windowSize,rhs.windowFunction,rhs.computeMagnitude,rhs.computePhase);
90 
91  //Copy the fft results
92  for(UINT i=0; i<this->windowSize; i++){
93  this->fftReal[i] = rhs.fftReal[i];
94  this->fftImag[i] = rhs.fftImag[i];
95  this->magnitude[i] = rhs.magnitude[i];
96  this->phase[i] = rhs.phase[i];
97  this->power[i] = rhs.power[i];
98  }
99  }
100  }
101  return *this;
102 }
103 
104 bool FastFourierTransform::init(const unsigned int windowSize,const unsigned int windowFunction,const bool computeMagnitude,const bool computePhase){
105 
106  initialized = false;
107  averagePower = 0;
108 
109  //Validate the window size
110  if( !isPowerOfTwo( windowSize ) ){
111  return false;
112  }
113 
114  if( windowFunction != RECTANGULAR_WINDOW && windowFunction != BARTLETT_WINDOW &&
115  windowFunction != HAMMING_WINDOW && windowFunction != HANNING_WINDOW ){
116  return false;
117  }
118 
119  initFFT();
120 
121  this->windowSize = windowSize;
122  this->windowFunction = windowFunction;
123  this->computeMagnitude = computeMagnitude;
124  this->computePhase = computePhase;
125 
126  //Init the memory
127  fftReal.resize( windowSize );
128  fftImag.resize( windowSize );
129  tmpReal.resize( windowSize/2 );
130  tmpImag.resize( windowSize/2 );
131  magnitude.resize( windowSize );
132  phase.resize( windowSize );
133  power.resize( windowSize );
134  averagePower = 0;
135 
136  //Zero the memory
137  for(UINT i=0; i<windowSize/2; i++){
138  tmpReal[i] = 0;
139  tmpImag[i] = 0;
140  }
141  for(UINT i=0; i<windowSize; i++){
142  fftReal[i] = 0;
143  fftImag[i] = 0;
144  magnitude[i] = 0;
145  phase[i] = 0;
146  power[i] = 0;
147  }
148 
149  //Flag that the FFT has been initialized
150  initialized = true;
151 
152  return true;
153 }
154 
155 bool FastFourierTransform::computeFFT( VectorDouble &data ){
156 
157  if( !initialized ){
158  return false;
159  }
160 
161  //Window the input data
162  if( !windowData( data ) ){
163  return false;
164  }
165 
166  //Perform the FFT
167  realFFT(data, &fftReal[0], &fftImag[0]);
168 
169  averagePower = 0;
170 
171  for(unsigned int i = 0; i<windowSize/2; i++){
172 
173  if( computeMagnitude ){
174  power[i] = fftReal[i]*fftReal[i] + fftImag[i]*fftImag[i];
175  averagePower += power[i];
176  magnitude[i] = 2.0*sqrt( power[i] );
177  }
178 
179  if( computePhase ){
180  phase[i] = atan2(fftImag[i],fftReal[i]);
181  }
182  }
183 
184  //Compute the average power
185  averagePower = averagePower / (double)(windowSize/2);
186 
187  return true;
188 }
189 
190 bool FastFourierTransform::windowData( VectorDouble &data ){
191 
192  if( data.size() != windowSize ){
193  errorLog << "The size of the data vector (" << data.size() << ") does not match the windowSize: " << windowSize << endl;
194  return false;
195  }
196 
197  switch( windowFunction ){
198  case RECTANGULAR_WINDOW:
199  return true;
200  break;
201  case BARTLETT_WINDOW:
202  for(unsigned int i=0; i<windowSize/2; i++) {
203  data[i] *= (i / (double) (windowSize / 2));
204  data[i + (windowSize/2)] *= (1.0 - (i / (double) (windowSize/2)));
205  }
206  return true;
207  break;
208  case HAMMING_WINDOW:
209  for(unsigned int i=0; i<windowSize; i++)
210  data[i] *= 0.54 - 0.46 * cos(2 * PI * i / (windowSize - 1));
211  return true;
212  break;
213  case HANNING_WINDOW:
214  for(unsigned int i=0; i <windowSize; i++)
215  data[i] *= 0.50 - 0.50 * cos(2 * PI * i / (windowSize - 1));
216  return true;
217  break;
218  default:
219  return false;
220  break;
221  }
222 
223  return false;
224 }
225 
226 VectorDouble FastFourierTransform::getMagnitudeData(){
227 
228  if( !initialized ) return VectorDouble();
229 
230  VectorDouble magnitudeData(windowSize/2);
231 
232  for(unsigned int i=0; i<windowSize/2; i++){
233  magnitudeData[i] = magnitude[i];
234  }
235 
236  return magnitudeData;
237 }
238 
239 VectorDouble FastFourierTransform::getPhaseData(){
240  if( !initialized ) return VectorDouble();
241 
242  VectorDouble phaseData(windowSize/2);
243 
244  for(unsigned int i=0; i<windowSize/2; i++){
245  phaseData[i] = phase[i];
246  }
247 
248  return phaseData;
249 }
250 
251 VectorDouble FastFourierTransform::getPowerData(){
252  if( !initialized ) return VectorDouble();
253 
254  VectorDouble powerData(windowSize/2);
255 
256  for(unsigned int i=0; i<windowSize/2; i++){
257  powerData[i] = power[i];
258  }
259 
260  return powerData;
261 }
262 
263 double FastFourierTransform::getAveragePower(){
264  return averagePower;
265 }
266 
267 double* FastFourierTransform::getMagnitudeDataPtr(){
268  return &magnitude[0];
269 }
270 
271 double* FastFourierTransform::getPhaseDataPtr(){
272  return &phase[0];
273 }
274 
275 double* FastFourierTransform::getPowerDataPtr(){
276  return &power[0];
277 }
278 
279 /*
280  * Real Fast Fourier Transform
281  *
282  * This function was based on the code in Numerical Recipes in C.
283  * In Num. Rec., the inner loop is based on a single 1-based array
284  * of interleaved real and imaginary numbers. Because we have two
285  * separate zero-based arrays, our indices are quite different.
286  * Here is the correspondence between Num. Rec. indices and our indices:
287  *
288  * i1 <-> real[i]
289  * i2 <-> imag[i]
290  * i3 <-> real[n/2-i]
291  * i4 <-> imag[n/2-i]
292  */
293 
294 bool FastFourierTransform::realFFT( const VectorDouble &realIn, double *realOut, double *imagOut ){
295  int NumSamples = (int)windowSize;
296  int Half = NumSamples / 2;
297  int i;
298 
299  double theta = PI / Half;
300 
301  for (i = 0; i < Half; i++) {
302  tmpReal[i] = realIn[2 * i];
303  tmpImag[i] = realIn[2 * i + 1];
304  }
305 
306  if( !FFT(Half, 0, &tmpReal[0], &tmpImag[0], realOut, imagOut) ){
307  return false;
308  }
309 
310  double wtemp = double(sin(0.5 * theta));
311 
312  double wpr = -2.0 * wtemp * wtemp;
313  double wpi = double (sin(theta));
314  double wr = 1.0 + wpr;
315  double wi = wpi;
316 
317  int i3;
318 
319  double h1r, h1i, h2r, h2i;
320 
321  for (i = 1; i < Half / 2; i++) {
322 
323  i3 = Half - i;
324 
325  h1r = 0.5 * (realOut[i] + realOut[i3]);
326  h1i = 0.5 * (imagOut[i] - imagOut[i3]);
327  h2r = 0.5 * (imagOut[i] + imagOut[i3]);
328  h2i = -0.5 * (realOut[i] - realOut[i3]);
329 
330  realOut[i] = h1r + wr * h2r - wi * h2i;
331  imagOut[i] = h1i + wr * h2i + wi * h2r;
332  realOut[i3] = h1r - wr * h2r + wi * h2i;
333  imagOut[i3] = -h1i + wr * h2i + wi * h2r;
334 
335  wr = (wtemp = wr) * wpr - wi * wpi + wr;
336  wi = wi * wpr + wtemp * wpi + wi;
337  }
338 
339  realOut[0] = (h1r = realOut[0]) + imagOut[0];
340  imagOut[0] = h1r - imagOut[0];
341 
342  return true;
343 }
344 
345 bool FastFourierTransform::FFT(int numSamples,bool inverseTransform,double *realIn, double *imagIn, double *realOut, double *imagOut){
346  int NumBits; /* Number of bits needed to store indices */
347  int i, j, k, n;
348  int BlockSize, BlockEnd;
349 
350  double angle_numerator = 2.0 * PI;
351  double tr, ti; /* temp real, temp imaginary */
352 
353  if( !isPowerOfTwo(numSamples) ) {
354  fprintf(stderr, "%d is not a power of two\n", numSamples);
355  return false;
356  }
357 
358  if( bitTable.size() == 0 ) initFFT();
359 
360  if( inverseTransform ) angle_numerator = -angle_numerator;
361 
362  NumBits = numberOfBitsNeeded(numSamples);
363 
364  //Simultaneously data copy and bit-reversal ordering into outputs...
365  for(i = 0; i < numSamples; i++) {
366  j = fastReverseBits(i, NumBits);
367  realOut[j] = realIn[i];
368  imagOut[j] = (imagIn == NULL) ? 0.0 : imagIn[i];
369  }
370 
371  //Do the FFT
372  BlockEnd = 1;
373  for (BlockSize = 2; BlockSize <= numSamples; BlockSize <<= 1) {
374 
375  double delta_angle = angle_numerator / (double) BlockSize;
376 
377  double sm2 = sin(-2 * delta_angle);
378  double sm1 = sin(-delta_angle);
379  double cm2 = cos(-2 * delta_angle);
380  double cm1 = cos(-delta_angle);
381  double w = 2 * cm1;
382  double ar0, ar1, ar2, ai0, ai1, ai2;
383 
384  for (i = 0; i < numSamples; i += BlockSize) {
385  ar2 = cm2;
386  ar1 = cm1;
387 
388  ai2 = sm2;
389  ai1 = sm1;
390 
391  for (j = i, n = 0; n < BlockEnd; j++, n++) {
392  ar0 = w * ar1 - ar2;
393  ar2 = ar1;
394  ar1 = ar0;
395 
396  ai0 = w * ai1 - ai2;
397  ai2 = ai1;
398  ai1 = ai0;
399 
400  k = j + BlockEnd;
401  tr = ar0 * realOut[k] - ai0 * imagOut[k];
402  ti = ar0 * imagOut[k] + ai0 * realOut[k];
403 
404  realOut[k] = realOut[j] - tr;
405  imagOut[k] = imagOut[j] - ti;
406 
407  realOut[j] += tr;
408  imagOut[j] += ti;
409  }
410  }
411 
412  BlockEnd = BlockSize;
413  }
414 
415  //Need to normalize the results if we are computing the inverse transform
416  if( inverseTransform ){
417  double denom = (double) numSamples;
418 
419  for(i = 0; i < numSamples; i++) {
420  realOut[i] /= denom;
421  imagOut[i] /= denom;
422  }
423  }
424 
425  return true;
426 }
427 
428 int FastFourierTransform::numberOfBitsNeeded(int powerOfTwo)
429 {
430  for (int i = 0;; i++){
431  if (powerOfTwo & (1 << i)){
432  return i;
433  }
434  }
435  return 0;
436 }
437 
438 int FastFourierTransform::reverseBits(int index, int numBits)
439 {
440  int i, rev;
441 
442  for (i = rev = 0; i < numBits; i++) {
443  rev = (rev << 1) | (index & 1);
444  index >>= 1;
445  }
446 
447  return rev;
448 }
449 
450 void FastFourierTransform::initFFT()
451 {
452  bitTable.resize( MAX_FAST_BITS );
453 
454  int len = 2;
455  for (int b = 1; b <= MAX_FAST_BITS; b++) {
456 
457  bitTable[b - 1].resize(len);
458 
459  for (int i = 0; i < len; i++)
460  bitTable[b - 1][i] = reverseBits(i, b);
461 
462  len <<= 1;
463  }
464 }
465 
466 inline int FastFourierTransform::fastReverseBits(const int i, const int numBits)
467 {
468  if (numBits <= MAX_FAST_BITS)
469  return bitTable[numBits - 1][i];
470  else
471  return reverseBits(i, numBits);
472 }
473 
474 inline bool FastFourierTransform::isPowerOfTwo(const unsigned int x){
475  if (x < 2) return false;
476  if (x & (x - 1)) return false;
477  return true;
478 }
479 
480 }//End of namespace GRT
Definition: AdaBoost.cpp:25