Main MRPT website > C++ reference for MRPT 1.9.9
fourier.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/fourier.h>
13 #include <algorithm>
14 #include <mrpt/core/bits_math.h>
15 #include <cmath>
16 
17 using namespace mrpt;
18 using namespace std;
19 using namespace mrpt::math;
20 
21 // Next we declare some auxiliary functions:
22 namespace mrpt
23 {
24 namespace math
25 {
26 // Replaces data[1..2*nn] by its discrete Fourier transform, if isign is input
27 // as 1; or replaces
28 // data[1..2*nn] by nn times its inverse discrete Fourier transform, if isign is
29 // input as -1.
30 // data is a complex array of length nn or, equivalently, a real array of length
31 // 2*nn. nn MUST
32 // be an integer power of 2 (this is not checked for!).
33 static void four1(float data[], unsigned long nn, int isign)
34 {
35  unsigned long n, mmax, m, j, i;
36  double wtemp, wr, wpr, wpi, wi,
37  theta; // Double precision for the trigonometric recurrences.
38  float tempr, tempi;
39 
40  n = nn << 1;
41  j = 1;
42 
43  for (i = 1; i < n;
44  i += 2) // This is the bit-reversal section of the routine.
45  {
46  if (j > i)
47  {
48  std::swap(data[j], data[i]); // Exchange the two complex numbers.
49  std::swap(data[j + 1], data[i + 1]);
50  }
51  m = nn;
52  while (m >= 2 && j > m)
53  {
54  j -= m;
55  m >>= 1;
56  }
57  j += m;
58  }
59  // Here begins the Danielson-Lanczos section of the routine.
60  mmax = 2;
61  while (n > mmax) // Outer loop executed log2 nn times.
62  {
63  unsigned long istep = mmax << 1;
64  theta = isign * (6.28318530717959 /
65  mmax); // Initialize the trigonometric recurrence.
66  wtemp = sin(0.5 * theta);
67  wpr = -2.0 * wtemp * wtemp;
68  wpi = sin(theta);
69  wr = 1.0;
70  wi = 0.0;
71  for (m = 1; m < mmax; m += 2) // Here are the two nested inner loops.
72  {
73  for (i = m; i <= n; i += istep)
74  {
75  j = i + mmax; // This is the Danielson-Lanczos formula:
76  tempr = (float)(wr * data[j] - wi * data[j + 1]);
77  tempi = (float)(wr * data[j + 1] + wi * data[j]);
78  data[j] = data[i] - tempr;
79  data[j + 1] = data[i + 1] - tempi;
80  data[i] += tempr;
81  data[i + 1] += tempi;
82  }
83  wr = (wtemp = wr) * wpr - wi * wpi +
84  wr; // Trigonometric recurrence.
85  wi = wi * wpr + wtemp * wpi + wi;
86  }
87  mmax = istep;
88  }
89 }
90 
91 // Calculates the Fourier transform of a set of n real-valued data points.
92 // Replaces this data (which
93 // is stored in array data[1..n]) by the positive frequency half of its complex
94 // Fourier transform.
95 // The real-valued first and last components of the complex transform are
96 // returned as elements
97 // data[1] and data[2], respectively. n must be a power of 2. This routine also
98 // calculates the
99 // inverse transform of a complex data array if it is the transform of real
100 // data. (Result in this case
101 // must be multiplied by 2/n.)
102 static void realft(float data[], unsigned long n)
103 {
104  unsigned long i, i1, i2, i3, i4, np3;
105  float c1 = 0.5, c2, h1r, h1i, h2r, h2i;
106  double wr, wi, wpr, wpi, wtemp,
107  theta; // Double precision for the trigonometric recurrences.
108  theta = 3.141592653589793 / (double)(n >> 1); // Initialize the recurrence.
109 
110  c2 = -0.5;
111  four1(data, n >> 1, 1); // The forward transform is here.
112 
113  wtemp = sin(0.5 * theta);
114  wpr = -2.0 * wtemp * wtemp;
115  wpi = sin(theta);
116  wr = 1.0 + wpr;
117  wi = wpi;
118  np3 = n + 3;
119  for (i = 2; i <= (n >> 2); i++) // Case i=1 done separately below.
120  {
121  i4 = 1 + (i3 = np3 - (i2 = 1 + (i1 = i + i - 1)));
122  h1r = c1 * (data[i1] + data[i3]); // The two separate transforms are
123  // separated out of data.
124  h1i = c1 * (data[i2] - data[i4]);
125  h2r = -c2 * (data[i2] + data[i4]);
126  h2i = c2 * (data[i1] - data[i3]);
127  data[i1] =
128  (float)(h1r + wr * h2r - wi * h2i); // Here they are recombined to
129  // form the true transform of
130  // the original real data.
131  data[i2] = (float)(h1i + wr * h2i + wi * h2r);
132  data[i3] = (float)(h1r - wr * h2r + wi * h2i);
133  data[i4] = (float)(-h1i + wr * h2i + wi * h2r);
134  wr = (wtemp = wr) * wpr - wi * wpi + wr; // The recurrence.
135  wi = wi * wpr + wtemp * wpi + wi;
136  }
137 
138  data[1] = (h1r = data[1]) + data[2];
139  // Squeeze the first and last data together to get them all within the
140  // original array.
141  data[2] = h1r - data[2];
142 }
143 
144 /**
145  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
146  You may use, copy, modify this code for any purpose and
147  without fee. You may distribute this ORIGINAL package.
148  */
149 using FFT_TYPE = float;
150 
151 static void makewt(int nw, int* ip, FFT_TYPE* w);
152 static void bitrv2(int n, int* ip, FFT_TYPE* a);
153 static void cftbsub(int n, FFT_TYPE* a, FFT_TYPE* w);
154 static void cftfsub(int n, FFT_TYPE* a, FFT_TYPE* w);
155 static void rftfsub(int n, FFT_TYPE* a, int nc, FFT_TYPE* c);
156 static void rftbsub(int n, FFT_TYPE* a, int nc, FFT_TYPE* c);
157 
158 static void cftbsub(int n, FFT_TYPE* a, FFT_TYPE* w)
159 {
160  int j, j1, j2, j3, k, k1, ks, l, m;
161  FFT_TYPE wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
162  FFT_TYPE x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
163 
164  l = 2;
165  while ((l << 1) < n)
166  {
167  m = l << 2;
168  for (j = 0; j <= l - 2; j += 2)
169  {
170  j1 = j + l;
171  j2 = j1 + l;
172  j3 = j2 + l;
173  x0r = a[j] + a[j1];
174  x0i = a[j + 1] + a[j1 + 1];
175  x1r = a[j] - a[j1];
176  x1i = a[j + 1] - a[j1 + 1];
177  x2r = a[j2] + a[j3];
178  x2i = a[j2 + 1] + a[j3 + 1];
179  x3r = a[j2] - a[j3];
180  x3i = a[j2 + 1] - a[j3 + 1];
181  a[j] = x0r + x2r;
182  a[j + 1] = x0i + x2i;
183  a[j2] = x0r - x2r;
184  a[j2 + 1] = x0i - x2i;
185  a[j1] = x1r - x3i;
186  a[j1 + 1] = x1i + x3r;
187  a[j3] = x1r + x3i;
188  a[j3 + 1] = x1i - x3r;
189  }
190  if (m < n)
191  {
192  wk1r = w[2];
193  for (j = m; j <= l + m - 2; j += 2)
194  {
195  j1 = j + l;
196  j2 = j1 + l;
197  j3 = j2 + l;
198  x0r = a[j] + a[j1];
199  x0i = a[j + 1] + a[j1 + 1];
200  x1r = a[j] - a[j1];
201  x1i = a[j + 1] - a[j1 + 1];
202  x2r = a[j2] + a[j3];
203  x2i = a[j2 + 1] + a[j3 + 1];
204  x3r = a[j2] - a[j3];
205  x3i = a[j2 + 1] - a[j3 + 1];
206  a[j] = x0r + x2r;
207  a[j + 1] = x0i + x2i;
208  a[j2] = x2i - x0i;
209  a[j2 + 1] = x0r - x2r;
210  x0r = x1r - x3i;
211  x0i = x1i + x3r;
212  a[j1] = wk1r * (x0r - x0i);
213  a[j1 + 1] = wk1r * (x0r + x0i);
214  x0r = x3i + x1r;
215  x0i = x3r - x1i;
216  a[j3] = wk1r * (x0i - x0r);
217  a[j3 + 1] = wk1r * (x0i + x0r);
218  }
219  k1 = 1;
220  ks = -1;
221  for (k = (m << 1); k <= n - m; k += m)
222  {
223  k1++;
224  ks = -ks;
225  wk1r = w[k1 << 1];
226  wk1i = w[(k1 << 1) + 1];
227  wk2r = ks * w[k1];
228  wk2i = w[k1 + ks];
229  wk3r = wk1r - 2 * wk2i * wk1i;
230  wk3i = 2 * wk2i * wk1r - wk1i;
231  for (j = k; j <= l + k - 2; j += 2)
232  {
233  j1 = j + l;
234  j2 = j1 + l;
235  j3 = j2 + l;
236  x0r = a[j] + a[j1];
237  x0i = a[j + 1] + a[j1 + 1];
238  x1r = a[j] - a[j1];
239  x1i = a[j + 1] - a[j1 + 1];
240  x2r = a[j2] + a[j3];
241  x2i = a[j2 + 1] + a[j3 + 1];
242  x3r = a[j2] - a[j3];
243  x3i = a[j2 + 1] - a[j3 + 1];
244  a[j] = x0r + x2r;
245  a[j + 1] = x0i + x2i;
246  x0r -= x2r;
247  x0i -= x2i;
248  a[j2] = wk2r * x0r - wk2i * x0i;
249  a[j2 + 1] = wk2r * x0i + wk2i * x0r;
250  x0r = x1r - x3i;
251  x0i = x1i + x3r;
252  a[j1] = wk1r * x0r - wk1i * x0i;
253  a[j1 + 1] = wk1r * x0i + wk1i * x0r;
254  x0r = x1r + x3i;
255  x0i = x1i - x3r;
256  a[j3] = wk3r * x0r - wk3i * x0i;
257  a[j3 + 1] = wk3r * x0i + wk3i * x0r;
258  }
259  }
260  }
261  l = m;
262  }
263  if (l < n)
264  {
265  for (j = 0; j <= l - 2; j += 2)
266  {
267  j1 = j + l;
268  x0r = a[j] - a[j1];
269  x0i = a[j + 1] - a[j1 + 1];
270  a[j] += a[j1];
271  a[j + 1] += a[j1 + 1];
272  a[j1] = x0r;
273  a[j1 + 1] = x0i;
274  }
275  }
276 }
277 
278 static void cftfsub(int n, FFT_TYPE* a, FFT_TYPE* w)
279 {
280  int j, j1, j2, j3, k, k1, ks, l, m;
281  FFT_TYPE wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
282  FFT_TYPE x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
283 
284  l = 2;
285  while ((l << 1) < n)
286  {
287  m = l << 2;
288  for (j = 0; j <= l - 2; j += 2)
289  {
290  j1 = j + l;
291  j2 = j1 + l;
292  j3 = j2 + l;
293  x0r = a[j] + a[j1];
294  x0i = a[j + 1] + a[j1 + 1];
295  x1r = a[j] - a[j1];
296  x1i = a[j + 1] - a[j1 + 1];
297  x2r = a[j2] + a[j3];
298  x2i = a[j2 + 1] + a[j3 + 1];
299  x3r = a[j2] - a[j3];
300  x3i = a[j2 + 1] - a[j3 + 1];
301  a[j] = x0r + x2r;
302  a[j + 1] = x0i + x2i;
303  a[j2] = x0r - x2r;
304  a[j2 + 1] = x0i - x2i;
305  a[j1] = x1r + x3i;
306  a[j1 + 1] = x1i - x3r;
307  a[j3] = x1r - x3i;
308  a[j3 + 1] = x1i + x3r;
309  }
310  if (m < n)
311  {
312  wk1r = w[2];
313  for (j = m; j <= l + m - 2; j += 2)
314  {
315  j1 = j + l;
316  j2 = j1 + l;
317  j3 = j2 + l;
318  x0r = a[j] + a[j1];
319  x0i = a[j + 1] + a[j1 + 1];
320  x1r = a[j] - a[j1];
321  x1i = a[j + 1] - a[j1 + 1];
322  x2r = a[j2] + a[j3];
323  x2i = a[j2 + 1] + a[j3 + 1];
324  x3r = a[j2] - a[j3];
325  x3i = a[j2 + 1] - a[j3 + 1];
326  a[j] = x0r + x2r;
327  a[j + 1] = x0i + x2i;
328  a[j2] = x0i - x2i;
329  a[j2 + 1] = x2r - x0r;
330  x0r = x1r + x3i;
331  x0i = x1i - x3r;
332  a[j1] = wk1r * (x0i + x0r);
333  a[j1 + 1] = wk1r * (x0i - x0r);
334  x0r = x3i - x1r;
335  x0i = x3r + x1i;
336  a[j3] = wk1r * (x0r + x0i);
337  a[j3 + 1] = wk1r * (x0r - x0i);
338  }
339  k1 = 1;
340  ks = -1;
341  for (k = (m << 1); k <= n - m; k += m)
342  {
343  k1++;
344  ks = -ks;
345  wk1r = w[k1 << 1];
346  wk1i = w[(k1 << 1) + 1];
347  wk2r = ks * w[k1];
348  wk2i = w[k1 + ks];
349  wk3r = wk1r - 2 * wk2i * wk1i;
350  wk3i = 2 * wk2i * wk1r - wk1i;
351  for (j = k; j <= l + k - 2; j += 2)
352  {
353  j1 = j + l;
354  j2 = j1 + l;
355  j3 = j2 + l;
356  x0r = a[j] + a[j1];
357  x0i = a[j + 1] + a[j1 + 1];
358  x1r = a[j] - a[j1];
359  x1i = a[j + 1] - a[j1 + 1];
360  x2r = a[j2] + a[j3];
361  x2i = a[j2 + 1] + a[j3 + 1];
362  x3r = a[j2] - a[j3];
363  x3i = a[j2 + 1] - a[j3 + 1];
364  a[j] = x0r + x2r;
365  a[j + 1] = x0i + x2i;
366  x0r -= x2r;
367  x0i -= x2i;
368  a[j2] = wk2r * x0r + wk2i * x0i;
369  a[j2 + 1] = wk2r * x0i - wk2i * x0r;
370  x0r = x1r + x3i;
371  x0i = x1i - x3r;
372  a[j1] = wk1r * x0r + wk1i * x0i;
373  a[j1 + 1] = wk1r * x0i - wk1i * x0r;
374  x0r = x1r - x3i;
375  x0i = x1i + x3r;
376  a[j3] = wk3r * x0r + wk3i * x0i;
377  a[j3 + 1] = wk3r * x0i - wk3i * x0r;
378  }
379  }
380  }
381  l = m;
382  }
383  if (l < n)
384  {
385  for (j = 0; j <= l - 2; j += 2)
386  {
387  j1 = j + l;
388  x0r = a[j] - a[j1];
389  x0i = a[j + 1] - a[j1 + 1];
390  a[j] += a[j1];
391  a[j + 1] += a[j1 + 1];
392  a[j1] = x0r;
393  a[j1 + 1] = x0i;
394  }
395  }
396 }
397 
398 static void makewt(int nw, int* ip, FFT_TYPE* w)
399 {
400  void bitrv2(int n, int* ip, FFT_TYPE* a);
401  int nwh, j;
402  FFT_TYPE delta, x, y;
403 
404  ip[0] = nw;
405  ip[1] = 1;
406  if (nw > 2)
407  {
408  nwh = nw >> 1;
409  delta = atan(1.0f) / nwh;
410  w[0] = 1;
411  w[1] = 0;
412  w[nwh] = cos(delta * nwh);
413  w[nwh + 1] = w[nwh];
414  for (j = 2; j <= nwh - 2; j += 2)
415  {
416  x = cos(delta * j);
417  y = sin(delta * j);
418  w[j] = x;
419  w[j + 1] = y;
420  w[nw - j] = y;
421  w[nw - j + 1] = x;
422  }
423  bitrv2(nw, ip + 2, w);
424  }
425 }
426 
427 /**
428  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
429  You may use, copy, modify this code for any purpose and
430  without fee. You may distribute this ORIGINAL package.
431  */
432 static void makect(int nc, int* ip, FFT_TYPE* c)
433 {
434  int nch, j;
435  FFT_TYPE delta;
436 
437  ip[1] = nc;
438  if (nc > 1)
439  {
440  nch = nc >> 1;
441  delta = atan(1.0f) / nch;
442  c[0] = 0.5f;
443  c[nch] = 0.5f * cos(delta * nch);
444  for (j = 1; j <= nch - 1; j++)
445  {
446  c[j] = 0.5f * cos(delta * j);
447  c[nc - j] = 0.5f * sin(delta * j);
448  }
449  }
450 }
451 
452 /**
453  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
454  You may use, copy, modify this code for any purpose and
455  without fee. You may distribute this ORIGINAL package.
456  */
457 static void bitrv2(int n, int* ip, FFT_TYPE* a)
458 {
459  int j, j1, k, k1, l, m, m2;
460  FFT_TYPE xr, xi;
461 
462  ip[0] = 0;
463  l = n;
464  m = 1;
465  while ((m << 2) < l)
466  {
467  l >>= 1;
468  for (j = 0; j <= m - 1; j++)
469  {
470  ip[m + j] = ip[j] + l;
471  }
472  m <<= 1;
473  }
474  if ((m << 2) > l)
475  {
476  for (k = 1; k <= m - 1; k++)
477  {
478  for (j = 0; j <= k - 1; j++)
479  {
480  j1 = (j << 1) + ip[k];
481  k1 = (k << 1) + ip[j];
482  xr = a[j1];
483  xi = a[j1 + 1];
484  a[j1] = a[k1];
485  a[j1 + 1] = a[k1 + 1];
486  a[k1] = xr;
487  a[k1 + 1] = xi;
488  }
489  }
490  }
491  else
492  {
493  m2 = m << 1;
494  for (k = 1; k <= m - 1; k++)
495  {
496  for (j = 0; j <= k - 1; j++)
497  {
498  j1 = (j << 1) + ip[k];
499  k1 = (k << 1) + ip[j];
500  xr = a[j1];
501  xi = a[j1 + 1];
502  a[j1] = a[k1];
503  a[j1 + 1] = a[k1 + 1];
504  a[k1] = xr;
505  a[k1 + 1] = xi;
506  j1 += m2;
507  k1 += m2;
508  xr = a[j1];
509  xi = a[j1 + 1];
510  a[j1] = a[k1];
511  a[j1 + 1] = a[k1 + 1];
512  a[k1] = xr;
513  a[k1 + 1] = xi;
514  }
515  }
516  }
517 }
518 
519 /**
520  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
521  You may use, copy, modify this code for any purpose and
522  without fee. You may distribute this ORIGINAL package.
523  */
524 static void cdft(int n, int isgn, FFT_TYPE* a, int* ip, FFT_TYPE* w)
525 {
526  if (n > (ip[0] << 2))
527  {
528  makewt(n >> 2, ip, w);
529  }
530  if (n > 4)
531  {
532  bitrv2(n, ip + 2, a);
533  }
534  if (isgn < 0)
535  {
536  cftfsub(n, a, w);
537  }
538  else
539  {
540  cftbsub(n, a, w);
541  }
542 }
543 
544 static void rftfsub(int n, FFT_TYPE* a, int nc, FFT_TYPE* c)
545 {
546  int j, k, kk, ks;
547  FFT_TYPE wkr, wki, xr, xi, yr, yi;
548 
549  ks = (nc << 2) / n;
550  kk = 0;
551  for (k = (n >> 1) - 2; k >= 2; k -= 2)
552  {
553  j = n - k;
554  kk += ks;
555  wkr = 0.5f - c[kk];
556  wki = c[nc - kk];
557  xr = a[k] - a[j];
558  xi = a[k + 1] + a[j + 1];
559  yr = wkr * xr + wki * xi;
560  yi = wkr * xi - wki * xr;
561  a[k] -= yr;
562  a[k + 1] -= yi;
563  a[j] += yr;
564  a[j + 1] -= yi;
565  }
566 }
567 
568 static void rdft(int n, int isgn, FFT_TYPE* a, int* ip, FFT_TYPE* w)
569 {
570  int nw, nc;
571  FFT_TYPE xi;
572 
573  nw = ip[0];
574  if (n > (nw << 2))
575  {
576  nw = n >> 2;
577  makewt(nw, ip, w);
578  }
579  nc = ip[1];
580  if (n > (nc << 2))
581  {
582  nc = n >> 2;
583  makect(nc, ip, w + nw);
584  }
585  if (isgn < 0)
586  {
587  a[1] = 0.5f * (a[0] - a[1]);
588  a[0] -= a[1];
589  if (n > 4)
590  {
591  rftfsub(n, a, nc, w + nw);
592  bitrv2(n, ip + 2, a);
593  }
594  cftfsub(n, a, w);
595  }
596  else
597  {
598  if (n > 4)
599  {
600  bitrv2(n, ip + 2, a);
601  }
602  cftbsub(n, a, w);
603  if (n > 4)
604  {
605  rftbsub(n, a, nc, w + nw);
606  }
607  xi = a[0] - a[1];
608  a[0] += a[1];
609  a[1] = xi;
610  }
611 }
612 
613 static void rftbsub(int n, FFT_TYPE* a, int nc, FFT_TYPE* c)
614 {
615  int j, k, kk, ks;
616  FFT_TYPE wkr, wki, xr, xi, yr, yi;
617 
618  ks = (nc << 2) / n;
619  kk = 0;
620  for (k = (n >> 1) - 2; k >= 2; k -= 2)
621  {
622  j = n - k;
623  kk += ks;
624  wkr = 0.5f - c[kk];
625  wki = c[nc - kk];
626  xr = a[k] - a[j];
627  xi = a[k + 1] + a[j + 1];
628  yr = wkr * xr - wki * xi;
629  yi = wkr * xi + wki * xr;
630  a[k] -= yr;
631  a[k + 1] -= yi;
632  a[j] += yr;
633  a[j + 1] -= yi;
634  }
635 }
636 
637 /**
638  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
639  You may use, copy, modify this code for any purpose and
640  without fee. You may distribute this ORIGINAL package.
641 
642 -------- Real DFT / Inverse of Real DFT --------
643  [definition]
644  <case1> RDFT
645  R[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 a[j1][j2] *
646  cos(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2),
647  0<=k1<n1, 0<=k2<n2
648  I[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 a[j1][j2] *
649  sin(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2),
650  0<=k1<n1, 0<=k2<n2
651  <case2> IRDFT (excluding scale)
652  a[k1][k2] = (1/2) * sum_j1=0^n1-1 sum_j2=0^n2-1
653  (R[j1][j2] *
654  cos(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2) +
655  I[j1][j2] *
656  sin(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2)),
657  0<=k1<n1, 0<=k2<n2
658  (notes: R[n1-k1][n2-k2] = R[k1][k2],
659  I[n1-k1][n2-k2] = -I[k1][k2],
660  R[n1-k1][0] = R[k1][0],
661  I[n1-k1][0] = -I[k1][0],
662  R[0][n2-k2] = R[0][k2],
663  I[0][n2-k2] = -I[0][k2],
664  0<k1<n1, 0<k2<n2)
665  [usage]
666  <case1>
667  ip[0] = 0; // first time only
668  rdft2d(n1, n2, 1, a, t, ip, w);
669  <case2>
670  ip[0] = 0; // first time only
671  rdft2d(n1, n2, -1, a, t, ip, w);
672  [parameters]
673  n1 :data length (int)
674  n1 >= 2, n1 = power of 2
675  n2 :data length (int)
676  n2 >= 2, n2 = power of 2
677  a[0...n1-1][0...n2-1]
678  :input/output data (FFT_TYPE **)
679  <case1>
680  output data
681  a[k1][2*k2] = R[k1][k2] = R[n1-k1][n2-k2],
682  a[k1][2*k2+1] = I[k1][k2] = -I[n1-k1][n2-k2],
683  0<k1<n1, 0<k2<n2/2,
684  a[0][2*k2] = R[0][k2] = R[0][n2-k2],
685  a[0][2*k2+1] = I[0][k2] = -I[0][n2-k2],
686  0<k2<n2/2,
687  a[k1][0] = R[k1][0] = R[n1-k1][0],
688  a[k1][1] = I[k1][0] = -I[n1-k1][0],
689  a[n1-k1][1] = R[k1][n2/2] = R[n1-k1][n2/2],
690  a[n1-k1][0] = -I[k1][n2/2] = I[n1-k1][n2/2],
691  0<k1<n1/2,
692  a[0][0] = R[0][0],
693  a[0][1] = R[0][n2/2],
694  a[n1/2][0] = R[n1/2][0],
695  a[n1/2][1] = R[n1/2][n2/2]
696  <case2>
697  input data
698  a[j1][2*j2] = R[j1][j2] = R[n1-j1][n2-j2],
699  a[j1][2*j2+1] = I[j1][j2] = -I[n1-j1][n2-j2],
700  0<j1<n1, 0<j2<n2/2,
701  a[0][2*j2] = R[0][j2] = R[0][n2-j2],
702  a[0][2*j2+1] = I[0][j2] = -I[0][n2-j2],
703  0<j2<n2/2,
704  a[j1][0] = R[j1][0] = R[n1-j1][0],
705  a[j1][1] = I[j1][0] = -I[n1-j1][0],
706  a[n1-j1][1] = R[j1][n2/2] = R[n1-j1][n2/2],
707  a[n1-j1][0] = -I[j1][n2/2] = I[n1-j1][n2/2],
708  0<j1<n1/2,
709  a[0][0] = R[0][0],
710  a[0][1] = R[0][n2/2],
711  a[n1/2][0] = R[n1/2][0],
712  a[n1/2][1] = R[n1/2][n2/2]
713  t[0...2*n1-1]
714  :work area (FFT_TYPE *)
715  ip[0...*]
716  :work area for bit reversal (int *)
717  length of ip >= 2+sqrt(n) ; if n % 4 == 0
718  2+sqrt(n/2); otherwise
719  (n = max(n1, n2/2))
720  ip[0],ip[1] are pointers of the cos/sin table.
721  w[0...*]
722  :cos/sin table (FFT_TYPE *)
723  length of w >= max(n1/2, n2/4) + n2/4
724  w[],ip[] are initialized if ip[0] == 0.
725  [remark]
726  Inverse of
727  rdft2d(n1, n2, 1, a, t, ip, w);
728  is
729  rdft2d(n1, n2, -1, a, t, ip, w);
730  for (j1 = 0; j1 <= n1 - 1; j1++) {
731  for (j2 = 0; j2 <= n2 - 1; j2++) {
732  a[j1][j2] *= 2.0 / (n1 * n2);
733  }
734  }
735  */
736 static void rdft2d(
737  int n1, int n2, int isgn, FFT_TYPE** a, FFT_TYPE* t, int* ip, FFT_TYPE* w)
738 {
739  int n, nw, nc, n1h, i, j, i2;
740  FFT_TYPE xi;
741 
742  n = n1 << 1;
743  if (n < n2)
744  {
745  n = n2;
746  }
747  nw = ip[0];
748  if (n > (nw << 2))
749  {
750  nw = n >> 2;
751  makewt(nw, ip, w);
752  }
753  nc = ip[1];
754  if (n2 > (nc << 2))
755  {
756  nc = n2 >> 2;
757  makect(nc, ip, w + nw);
758  }
759  n1h = n1 >> 1;
760  if (isgn < 0)
761  {
762  for (i = 1; i <= n1h - 1; i++)
763  {
764  j = n1 - i;
765  xi = a[i][0] - a[j][0];
766  a[i][0] += a[j][0];
767  a[j][0] = xi;
768  xi = a[j][1] - a[i][1];
769  a[i][1] += a[j][1];
770  a[j][1] = xi;
771  }
772  for (j = 0; j <= n2 - 2; j += 2)
773  {
774  for (i = 0; i <= n1 - 1; i++)
775  {
776  i2 = i << 1;
777  t[i2] = a[i][j];
778  t[i2 + 1] = a[i][j + 1];
779  }
780  cdft(n1 << 1, isgn, t, ip, w);
781  for (i = 0; i <= n1 - 1; i++)
782  {
783  i2 = i << 1;
784  a[i][j] = t[i2];
785  a[i][j + 1] = t[i2 + 1];
786  }
787  }
788  for (i = 0; i <= n1 - 1; i++)
789  {
790  rdft(n2, isgn, a[i], ip, w);
791  }
792  }
793  else
794  {
795  for (i = 0; i <= n1 - 1; i++)
796  {
797  rdft(n2, isgn, a[i], ip, w);
798  }
799  for (j = 0; j <= n2 - 2; j += 2)
800  {
801  for (i = 0; i <= n1 - 1; i++)
802  {
803  i2 = i << 1;
804  t[i2] = a[i][j];
805  t[i2 + 1] = a[i][j + 1];
806  }
807  cdft(n1 << 1, isgn, t, ip, w);
808  for (i = 0; i <= n1 - 1; i++)
809  {
810  i2 = i << 1;
811  a[i][j] = t[i2];
812  a[i][j + 1] = t[i2 + 1];
813  }
814  }
815  for (i = 1; i <= n1h - 1; i++)
816  {
817  j = n1 - i;
818  a[j][0] = 0.5f * (a[i][0] - a[j][0]);
819  a[i][0] -= a[j][0];
820  a[j][1] = 0.5f * (a[i][1] + a[j][1]);
821  a[i][1] -= a[j][1];
822  }
823  }
824 }
825 
826 /**
827  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
828  You may use, copy, modify this code for any purpose and
829  without fee. You may distribute this ORIGINAL package.
830 
831 -------- Complex DFT (Discrete Fourier Transform) --------
832  [definition]
833  <case1>
834  X[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 x[j1][j2] *
835  exp(2*pi*i*j1*k1/n1) *
836  exp(2*pi*i*j2*k2/n2), 0<=k1<n1, 0<=k2<n2
837  <case2>
838  X[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 x[j1][j2] *
839  exp(-2*pi*i*j1*k1/n1) *
840  exp(-2*pi*i*j2*k2/n2), 0<=k1<n1, 0<=k2<n2
841  (notes: sum_j=0^n-1 is a summation from j=0 to n-1)
842  [usage]
843  <case1>
844  ip[0] = 0; // first time only
845  cdft2d(n1, 2*n2, 1, a, t, ip, w);
846  <case2>
847  ip[0] = 0; // first time only
848  cdft2d(n1, 2*n2, -1, a, t, ip, w);
849  [parameters]
850  n1 :data length (int)
851  n1 >= 1, n1 = power of 2
852  2*n2 :data length (int)
853  n2 >= 1, n2 = power of 2
854  a[0...n1-1][0...2*n2-1]
855  :input/output data (double **)
856  input data
857  a[j1][2*j2] = Re(x[j1][j2]),
858  a[j1][2*j2+1] = Im(x[j1][j2]),
859  0<=j1<n1, 0<=j2<n2
860  output data
861  a[k1][2*k2] = Re(X[k1][k2]),
862  a[k1][2*k2+1] = Im(X[k1][k2]),
863  0<=k1<n1, 0<=k2<n2
864  t[0...2*n1-1]
865  :work area (double *)
866  ip[0...*]
867  :work area for bit reversal (int *)
868  length of ip >= 2+sqrt(n) ; if n % 4 == 0
869  2+sqrt(n/2); otherwise
870  (n = max(n1, n2))
871  ip[0],ip[1] are pointers of the cos/sin table.
872  w[0...*]
873  :cos/sin table (double *)
874  length of w >= max(n1/2, n2/2)
875  w[],ip[] are initialized if ip[0] == 0.
876  [remark]
877  Inverse of
878  cdft2d(n1, 2*n2, -1, a, t, ip, w);
879  is
880  cdft2d(n1, 2*n2, 1, a, t, ip, w);
881  for (j1 = 0; j1 <= n1 - 1; j1++) {
882  for (j2 = 0; j2 <= 2 * n2 - 1; j2++) {
883  a[j1][j2] *= 1.0 / (n1 * n2);
884  }
885  }
886 
887 */
888 static void cdft2d(
889  int n1, int n2, int isgn, FFT_TYPE** a, FFT_TYPE* t, int* ip, FFT_TYPE* w)
890 {
891  void makewt(int nw, int* ip, FFT_TYPE* w);
892  void cdft(int n, int isgn, FFT_TYPE* a, int* ip, FFT_TYPE* w);
893  int n, i, j, i2;
894 
895  n = n1 << 1;
896  if (n < n2)
897  {
898  n = n2;
899  }
900  if (n > (ip[0] << 2))
901  {
902  makewt(n >> 2, ip, w);
903  }
904  for (i = 0; i <= n1 - 1; i++)
905  {
906  cdft(n2, isgn, a[i], ip, w);
907  }
908  for (j = 0; j <= n2 - 2; j += 2)
909  {
910  for (i = 0; i <= n1 - 1; i++)
911  {
912  i2 = i << 1;
913  t[i2] = a[i][j];
914  t[i2 + 1] = a[i][j + 1];
915  }
916  cdft(n1 << 1, isgn, t, ip, w);
917  for (i = 0; i <= n1 - 1; i++)
918  {
919  i2 = i << 1;
920  a[i][j] = t[i2];
921  a[i][j + 1] = t[i2 + 1];
922  }
923  }
924 }
925 
926 } // namespace math
927 } // namespace mrpt
928 
929 void mrpt::math::fft_real(
930  CVectorFloat& in_realData, CVectorFloat& out_FFT_Re,
931  CVectorFloat& out_FFT_Im, CVectorFloat& out_FFT_Mag)
932 {
933  MRPT_START
934 
935  unsigned long n = (unsigned long)in_realData.size();
936 
937  // TODO: Test data lenght is 2^N...
938 
939  CVectorFloat auxVect(n + 1);
940 
941  memcpy(&auxVect[1], &in_realData[0], n * sizeof(auxVect[0]));
942 
943  realft(&auxVect[0], n);
944 
945  unsigned int n_2 = 1 + (n / 2);
946 
947  out_FFT_Re.resize(n_2);
948  out_FFT_Im.resize(n_2);
949  out_FFT_Mag.resize(n_2);
950 
951  for (unsigned int i = 0; i < n_2; i++)
952  {
953  if (i == (n_2 - 1))
954  out_FFT_Re[i] = auxVect[2];
955  else
956  out_FFT_Re[i] = auxVect[1 + i * 2];
957 
958  if (i == 0 || i == (n_2 - 1))
959  out_FFT_Im[i] = 0;
960  else
961  out_FFT_Im[i] = auxVect[1 + i * 2 + 1];
962 
963  out_FFT_Mag[i] =
964  std::sqrt(square(out_FFT_Re[i]) + square(out_FFT_Im[i]));
965  }
966 
967  MRPT_END
968 }
969 
970 void math::dft2_real(
971  const CMatrixFloat& in_data, CMatrixFloat& out_real, CMatrixFloat& out_imag)
972 {
973  MRPT_START
974 
975  size_t i, j;
976  using float_ptr = FFT_TYPE*;
977 
978  // The dimensions:
979  size_t dim1 = in_data.rows();
980  size_t dim2 = in_data.cols();
981 
982  // Transform to format compatible with C routines:
983  // ------------------------------------------------------------
984  FFT_TYPE** a;
985  FFT_TYPE* t;
986  int* ip;
987  FFT_TYPE* w;
988 
989  // Reserve memory and copy data:
990  // --------------------------------------
991  a = new float_ptr[dim1];
992  for (i = 0; i < dim1; i++)
993  {
994  a[i] = new FFT_TYPE[dim2];
995  for (j = 0; j < dim2; j++) a[i][j] = in_data.get_unsafe(i, j);
996  }
997 
998  t = new FFT_TYPE[2 * dim1 + 20];
999  ip = new int[(int)ceil(20 + 2 + sqrt((FFT_TYPE)max(dim1, dim2 / 2)))];
1000  ip[0] = 0;
1001  w = new FFT_TYPE[max(dim1 / 2, dim2 / 4) + dim2 / 4 + 20];
1002 
1003  // Do the job!
1004  // --------------------------------------
1005  rdft2d((int)dim1, (int)dim2, 1, a, t, ip, w);
1006 
1007  // Transform back to MRPT matrix format:
1008  // --------------------------------------
1009  out_real.setSize(dim1, dim2);
1010  out_imag.setSize(dim1, dim2);
1011 
1012  // a[k1][2*k2] = R[k1][k2] = R[n1-k1][n2-k2],
1013  // a[k1][2*k2+1] = I[k1][k2] = -I[n1-k1][n2-k2],
1014  // 0<k1<n1, 0<k2<n2/2,
1015  for (i = 1; i < dim1; i++)
1016  for (j = 1; j < dim2 / 2; j++)
1017  {
1018  out_real.set_unsafe(i, j, (float)a[i][j * 2]);
1019  out_real.set_unsafe(dim1 - i, dim2 - j, (float)a[i][j * 2]);
1020  out_imag.set_unsafe(i, j, (float)-a[i][j * 2 + 1]);
1021  out_imag.set_unsafe(dim1 - i, dim2 - j, (float)a[i][j * 2 + 1]);
1022  }
1023  // a[0][2*k2] = R[0][k2] = R[0][n2-k2],
1024  // a[0][2*k2+1] = I[0][k2] = -I[0][n2-k2],
1025  // 0<k2<n2/2,
1026  for (j = 1; j < dim2 / 2; j++)
1027  {
1028  out_real.set_unsafe(0, j, (float)a[0][j * 2]);
1029  out_real.set_unsafe(0, dim2 - j, (float)a[0][j * 2]);
1030  out_imag.set_unsafe(0, j, (float)-a[0][j * 2 + 1]);
1031  out_imag.set_unsafe(0, dim2 - j, (float)a[0][j * 2 + 1]);
1032  }
1033 
1034  // a[k1][0] = R[k1][0] = R[n1-k1][0],
1035  // a[k1][1] = I[k1][0] = -I[n1-k1][0],
1036  // a[n1-k1][1] = R[k1][n2/2] = R[n1-k1][n2/2],
1037  // a[n1-k1][0] = -I[k1][n2/2] = I[n1-k1][n2/2],
1038  // 0<k1<n1/2,
1039  for (i = 1; i < dim1 / 2; i++)
1040  {
1041  out_real.set_unsafe(i, 0, (float)a[i][0]);
1042  out_real.set_unsafe(dim1 - i, 0, (float)a[i][0]);
1043  out_imag.set_unsafe(i, 0, (float)-a[i][1]);
1044  out_imag.set_unsafe(dim1 - i, 0, (float)a[i][1]);
1045  out_real.set_unsafe(i, dim2 / 2, (float)a[dim1 - i][1]);
1046  out_real.set_unsafe(dim1 - i, dim2 / 2, (float)a[dim1 - i][1]);
1047  out_imag.set_unsafe(i, dim2 / 2, (float)a[dim1 - i][0]);
1048  out_imag.set_unsafe(dim1 - i, dim2 / 2, (float)-a[dim1 - i][0]);
1049  }
1050 
1051  // a[0][0] = R[0][0],
1052  // a[0][1] = R[0][n2/2],
1053  // a[n1/2][0] = R[n1/2][0],
1054  // a[n1/2][1] = R[n1/2][n2/2]
1055  out_real.set_unsafe(0, 0, (float)a[0][0]);
1056  out_real.set_unsafe(0, dim2 / 2, (float)a[0][1]);
1057  out_real.set_unsafe(dim1 / 2, 0, (float)a[dim1 / 2][0]);
1058  out_real.set_unsafe(dim1 / 2, dim2 / 2, (float)a[dim1 / 2][1]);
1059 
1060  // Free temporary memory:
1061  for (i = 0; i < dim1; i++) delete[] a[i];
1062  delete[] a;
1063  delete[] t;
1064  delete[] ip;
1065  delete[] w;
1066 
1067  MRPT_END
1068 }
1069 
1070 void math::idft2_real(
1071  const CMatrixFloat& in_real, const CMatrixFloat& in_imag,
1072  CMatrixFloat& out_data)
1073 {
1074  MRPT_START
1075 
1076  size_t i, j;
1077  using float_ptr = FFT_TYPE*;
1078 
1079  ASSERT_(in_real.rows() == in_imag.rows());
1080  ASSERT_(in_real.cols() == in_imag.cols());
1081 
1082  // The dimensions:
1083  size_t dim1 = in_real.rows();
1084  size_t dim2 = in_real.cols();
1085 
1086  if (mrpt::round2up(dim1) != dim1 || mrpt::round2up(dim2) != dim2)
1087  THROW_EXCEPTION("Matrix sizes are not a power of two!");
1088 
1089  // Transform to format compatible with C routines:
1090  // ------------------------------------------------------------
1091  FFT_TYPE** a;
1092  FFT_TYPE* t;
1093  int* ip;
1094  FFT_TYPE* w;
1095 
1096  // Reserve memory and copy data:
1097  // --------------------------------------
1098  a = new float_ptr[dim1];
1099  for (i = 0; i < dim1; i++) a[i] = new FFT_TYPE[dim2];
1100 
1101  // a[j1][2*j2] = R[j1][j2] = R[n1-j1][n2-j2],
1102  // a[j1][2*j2+1] = I[j1][j2] = -I[n1-j1][n2-j2],
1103  // 0<j1<n1, 0<j2<n2/2,
1104  for (i = 1; i < dim1; i++)
1105  for (j = 1; j < dim2 / 2; j++)
1106  {
1107  a[i][2 * j] = in_real.get_unsafe(i, j);
1108  a[i][2 * j + 1] = -in_imag.get_unsafe(i, j);
1109  }
1110 
1111  // a[0][2*j2] = R[0][j2] = R[0][n2-j2],
1112  // a[0][2*j2+1] = I[0][j2] = -I[0][n2-j2],
1113  // 0<j2<n2/2,
1114  for (j = 1; j < dim2 / 2; j++)
1115  {
1116  a[0][2 * j] = in_real.get_unsafe(0, j);
1117  a[0][2 * j + 1] = -in_imag.get_unsafe(0, j);
1118  }
1119 
1120  // a[j1][0] = R[j1][0] = R[n1-j1][0],
1121  // a[j1][1] = I[j1][0] = -I[n1-j1][0],
1122  // a[n1-j1][1] = R[j1][n2/2] = R[n1-j1][n2/2],
1123  // a[n1-j1][0] = -I[j1][n2/2] = I[n1-j1][n2/2],
1124  // 0<j1<n1/2,
1125  for (i = 1; i < dim1 / 2; i++)
1126  {
1127  a[i][0] = in_real.get_unsafe(i, 0);
1128  a[i][1] = -in_imag.get_unsafe(i, 0);
1129  a[dim1 - i][1] = in_real.get_unsafe(i, dim2 / 2);
1130  a[dim1 - i][0] = in_imag.get_unsafe(i, dim2 / 2);
1131  }
1132 
1133  // a[0][0] = R[0][0],
1134  // a[0][1] = R[0][n2/2],
1135  // a[n1/2][0] = R[n1/2][0],
1136  // a[n1/2][1] = R[n1/2][n2/2]
1137  a[0][0] = in_real.get_unsafe(0, 0);
1138  a[0][1] = in_real.get_unsafe(0, dim2 / 2);
1139  a[dim1 / 2][0] = in_real.get_unsafe(dim1 / 2, 0);
1140  a[dim1 / 2][1] = in_real.get_unsafe(dim1 / 2, dim2 / 2);
1141 
1142  t = new FFT_TYPE[2 * dim1 + 20];
1143  ip = new int[(int)ceil(20 + 2 + sqrt((FFT_TYPE)max(dim1, dim2 / 2)))];
1144  ip[0] = 0;
1145  w = new FFT_TYPE[max(dim1 / 2, dim2 / 4) + dim2 / 4 + 20];
1146 
1147  // Do the job!
1148  // --------------------------------------
1149  rdft2d((int)dim1, (int)dim2, -1, a, t, ip, w);
1150 
1151  // Transform back to MRPT matrix format:
1152  // --------------------------------------
1153  out_data.setSize(dim1, dim2);
1154 
1155  FFT_TYPE scale = 2.0f / (dim1 * dim2);
1156 
1157  for (i = 0; i < dim1; i++)
1158  for (j = 0; j < dim2; j++)
1159  out_data.set_unsafe(i, j, (float)(a[i][j] * scale));
1160 
1161  // Free temporary memory:
1162  for (i = 0; i < dim1; i++) delete[] a[i];
1163  delete[] a;
1164  delete[] t;
1165  delete[] ip;
1166  delete[] w;
1167 
1168  MRPT_END
1169 }
1170 
1171 /*---------------------------------------------------------------
1172  myGeneralDFT
1173 
1174  sign -> -1: DFT, 1:IDFT
1175  ---------------------------------------------------------------*/
1176 static void myGeneralDFT(
1177  int sign, const CMatrixFloat& in_real, const CMatrixFloat& in_imag,
1178  CMatrixFloat& out_real, CMatrixFloat& out_imag)
1179 {
1180  ASSERT_(in_real.rows() == in_imag.rows());
1181  ASSERT_(in_real.cols() == in_imag.cols());
1182 
1183  // The dimensions:
1184  size_t dim1 = in_real.rows();
1185  size_t dim2 = in_real.cols();
1186 
1187  size_t k1, k2, n1, n2;
1188  float w_r, w_i;
1189  float ang1 = (float)(sign * M_2PI / dim1);
1190  float ang2 = (float)(sign * M_2PI / dim2);
1191  float phase;
1192  float R, I;
1193  float scale = sign == 1 ? (1.0f / (dim1 * dim2)) : 1;
1194 
1195  out_real.setSize(dim1, dim2);
1196  out_imag.setSize(dim1, dim2);
1197 
1198  for (k1 = 0; k1 < dim1; k1++)
1199  {
1200  for (k2 = 0; k2 < dim2; k2++)
1201  {
1202  R = I = 0; // Accum:
1203 
1204  for (n1 = 0; n1 < dim1; n1++)
1205  {
1206  phase = ang1 * n1 * k1;
1207  for (n2 = 0; n2 < dim2; n2++)
1208  {
1209  w_r = cos(phase);
1210  w_i = sin(phase);
1211 
1212  R += w_r * in_real.get_unsafe(n1, n2) -
1213  w_i * in_imag.get_unsafe(n1, n2);
1214  I += w_i * in_real.get_unsafe(n1, n2) +
1215  w_r * in_imag.get_unsafe(n1, n2);
1216 
1217  phase += ang2 * k2;
1218  } // end for k2
1219  } // end for k1
1220 
1221  // Save result:
1222  out_real.set_unsafe(k1, k2, R * scale);
1223  out_imag.set_unsafe(k1, k2, I * scale);
1224 
1225  } // end for k2
1226  } // end for k1
1227 }
1228 
1229 /*---------------------------------------------------------------
1230  dft2_complex
1231  ---------------------------------------------------------------*/
1232 void math::dft2_complex(
1233  const CMatrixFloat& in_real, const CMatrixFloat& in_imag,
1234  CMatrixFloat& out_real, CMatrixFloat& out_imag)
1235 {
1236  MRPT_START
1237 
1238  ASSERT_(in_real.rows() == in_imag.rows());
1239  ASSERT_(in_real.cols() == in_imag.cols());
1240 
1241  // The dimensions:
1242  size_t dim1 = in_real.rows();
1243  size_t dim2 = in_real.cols();
1244  size_t i, j;
1245 
1246  bool dim1IsPower2 = (mrpt::round2up(dim1) == dim1);
1247  bool dim2IsPower2 = (mrpt::round2up(dim2) == dim2);
1248 
1249  // FFT or DFT??
1250  if (dim1IsPower2 && dim2IsPower2)
1251  {
1252  // ----------------------------------------
1253  // Optimized FFT:
1254  // ----------------------------------------
1255  using float_ptr = FFT_TYPE*;
1256 
1257  // Transform to format compatible with C routines:
1258  // ------------------------------------------------------------
1259  static FFT_TYPE** a = nullptr;
1260  static FFT_TYPE* t = nullptr;
1261  static int* ip = nullptr;
1262  static FFT_TYPE* w = nullptr;
1263 
1264  // Reserve memory
1265  // --------------------------------------
1266  static int alreadyInitSize1 = -1, alreadyInitSize2 = -1;
1267 
1268  if (alreadyInitSize1 != (int)dim1 || alreadyInitSize2 != (int)dim2)
1269  {
1270  // Create/realloc buffers:
1271  if (a)
1272  {
1273  for (i = 0; i < dim1; i++) delete[] a[i];
1274  delete[] a;
1275  }
1276  if (ip) delete[] ip;
1277  if (t) delete[] t;
1278  if (w) delete[] w;
1279 
1280  alreadyInitSize1 = (int)dim1;
1281  alreadyInitSize2 = (int)dim2;
1282 
1283  a = new float_ptr[dim1];
1284  for (i = 0; i < dim1; i++) a[i] = new FFT_TYPE[2 * dim2];
1285 
1286  t = new FFT_TYPE[2 * dim1 + 20];
1287  ip = new int[(int)ceil(
1288  20 + 2 + sqrt((FFT_TYPE)max(dim1, dim2 / 2)))];
1289  ip[0] = 0;
1290  w = new FFT_TYPE[max(dim1 / 2, dim2 / 4) + dim2 / 4 + 20];
1291  }
1292 
1293  // and copy data:
1294  // --------------------------------------
1295  for (i = 0; i < dim1; i++)
1296  for (j = 0; j < dim2; j++)
1297  {
1298  a[i][2 * j + 0] = in_real.get_unsafe(i, j);
1299  a[i][2 * j + 1] = in_imag.get_unsafe(i, j);
1300  }
1301 
1302  // Do the job!
1303  // --------------------------------------
1304  cdft2d((int)dim1, (int)(2 * dim2), 1, a, t, ip, w);
1305 
1306  // Transform back to MRPT matrix format:
1307  // --------------------------------------
1308  out_real.setSize(dim1, dim2);
1309  out_imag.setSize(dim1, dim2);
1310 
1311  // a[k1][2*k2] = Re(X[k1][k2]),
1312  // a[k1][2*k2+1] = Im(X[k1][k2]),
1313  // 0<=k1<n1, 0<=k2<n2
1314  for (i = 0; i < dim1; i++)
1315  for (j = 0; j < dim2; j++)
1316  {
1317  out_real.set_unsafe(i, j, (float)a[i][j * 2 + 0]);
1318  out_imag.set_unsafe(i, j, (float)a[i][j * 2 + 1]);
1319  }
1320 
1321  } // end FFT
1322  else
1323  {
1324  // ----------------------------------------
1325  // General DFT:
1326  // ----------------------------------------
1327  printf("Using general DFT...\n");
1328  myGeneralDFT(-1, in_real, in_imag, out_real, out_imag);
1329  }
1330 
1331  MRPT_END
1332 }
1333 
1334 /*---------------------------------------------------------------
1335  idft2_complex
1336  ---------------------------------------------------------------*/
1337 void math::idft2_complex(
1338  const CMatrixFloat& in_real, const CMatrixFloat& in_imag,
1339  CMatrixFloat& out_real, CMatrixFloat& out_imag)
1340 {
1341  MRPT_START
1342 
1343  ASSERT_(in_real.rows() == in_imag.rows());
1344  ASSERT_(in_real.cols() == in_imag.cols());
1345 
1346  // The dimensions:
1347  size_t dim1 = in_real.rows();
1348  size_t dim2 = in_real.cols();
1349  size_t i, j;
1350 
1351  bool dim1IsPower2 = (mrpt::round2up(dim1) == dim1);
1352  bool dim2IsPower2 = (mrpt::round2up(dim2) == dim2);
1353 
1354  // FFT or DFT??
1355  if (dim1IsPower2 && dim2IsPower2)
1356  {
1357  using float_ptr = FFT_TYPE*;
1358  // ----------------------------------------
1359  // Optimized FFT:
1360  // ----------------------------------------
1361 
1362  // Transform to format compatible with C routines:
1363  // ------------------------------------------------------------
1364  static FFT_TYPE** a = nullptr;
1365  static FFT_TYPE* t = nullptr;
1366  static int* ip = nullptr;
1367  static FFT_TYPE* w = nullptr;
1368 
1369  // Reserve memory
1370  // --------------------------------------
1371 
1372  // and copy data:
1373  // --------------------------------------
1374  static int alreadyInitSize1 = -1, alreadyInitSize2 = -1;
1375 
1376  if (alreadyInitSize1 != (int)dim1 || alreadyInitSize2 != (int)dim2)
1377  {
1378  // Create/realloc buffers:
1379  if (a)
1380  {
1381  for (i = 0; i < dim1; i++) delete[] a[i];
1382  delete[] a;
1383  }
1384  if (ip) delete[] ip;
1385  if (t) delete[] t;
1386  if (w) delete[] w;
1387 
1388  alreadyInitSize1 = (int)dim1;
1389  alreadyInitSize2 = (int)dim2;
1390 
1391  a = new float_ptr[dim1];
1392  for (i = 0; i < dim1; i++) a[i] = new FFT_TYPE[2 * dim2];
1393  t = new FFT_TYPE[2 * dim1 + 20];
1394  ip = new int[(int)ceil(
1395  20 + 2 + sqrt((FFT_TYPE)max(dim1, dim2 / 2)))];
1396  ip[0] = 0;
1397  w = new FFT_TYPE[max(dim1 / 2, dim2 / 4) + dim2 / 4 + 20];
1398  }
1399 
1400  // and copy data:
1401  // --------------------------------------
1402  for (i = 0; i < dim1; i++)
1403  for (j = 0; j < dim2; j++)
1404  {
1405  a[i][2 * j + 0] = in_real.get_unsafe(i, j);
1406  a[i][2 * j + 1] = in_imag.get_unsafe(i, j);
1407  }
1408 
1409  // Do the job!
1410  // --------------------------------------
1411  cdft2d((int)dim1, (int)(2 * dim2), -1, a, t, ip, w);
1412 
1413  // Transform back to MRPT matrix format:
1414  // --------------------------------------
1415  out_real.setSize(dim1, dim2);
1416  out_imag.setSize(dim1, dim2);
1417 
1418  FFT_TYPE scale = 1.0f / (dim1 * dim2);
1419 
1420  // a[k1][2*k2] = Re(X[k1][k2]),
1421  // a[k1][2*k2+1] = Im(X[k1][k2]),
1422  // 0<=k1<n1, 0<=k2<n2
1423  for (i = 0; i < dim1; i++)
1424  for (j = 0; j < dim2; j++)
1425  {
1426  // out_real.set_unsafe(i,j,(float)(a[i][j*2+0]*scale));
1427  // out_imag.set_unsafe(i,j,(float)(a[i][j*2+1]*scale));
1428  out_real.set_unsafe(i, j, (float)(a[i][j * 2 + 0]));
1429  out_imag.set_unsafe(i, j, (float)(a[i][j * 2 + 1]));
1430  }
1431 
1432  out_real *= scale;
1433  out_imag *= scale;
1434 
1435  // The element (0,0) is purely real!
1436  out_imag.set_unsafe(0, 0, 0);
1437 
1438  } // end FFT
1439  else
1440  {
1441  // ----------------------------------------
1442  // General DFT:
1443  // ----------------------------------------
1444  printf("Using general DFT...\n");
1445  myGeneralDFT(1, in_real, in_imag, out_real, out_imag);
1446  }
1447 
1448  MRPT_END
1449 }
1450 
1451 void mrpt::math::cross_correlation_FFT(
1452  const CMatrixFloat& A, const CMatrixFloat& B, CMatrixFloat& out_corr)
1453 {
1454  MRPT_START
1455 
1456  ASSERT_(A.cols() == B.cols() && A.rows() == B.rows());
1457  if (mrpt::round2up(A.rows()) != A.rows() ||
1458  mrpt::round2up(A.cols()) != A.cols())
1459  THROW_EXCEPTION("Size of input matrices must be powers of two.");
1460 
1461  // Find smallest valid size:
1462  size_t x, y;
1463  const size_t lx = A.cols();
1464  const size_t ly = A.rows();
1465 
1466  const CMatrixFloat& i1 = A;
1467  const CMatrixFloat& i2 = B;
1468 
1469  // FFT:
1470  CMatrixFloat I1_R, I1_I, I2_R, I2_I, ZEROS(ly, lx);
1471  math::dft2_complex(i1, ZEROS, I1_R, I1_I);
1472  math::dft2_complex(i2, ZEROS, I2_R, I2_I);
1473 
1474  // Compute the COMPLEX division of I2 by I1:
1475  for (y = 0; y < ly; y++)
1476  for (x = 0; x < lx; x++)
1477  {
1478  float r1 = I1_R.get_unsafe(y, x);
1479  float r2 = I2_R.get_unsafe(y, x);
1480 
1481  float ii1 = I1_I.get_unsafe(y, x);
1482  float ii2 = I2_I.get_unsafe(y, x);
1483 
1484  float den = square(r1) + square(ii1);
1485  I2_R.set_unsafe(y, x, (r1 * r2 + ii1 * ii2) / den);
1486  I2_I.set_unsafe(y, x, (ii2 * r1 - r2 * ii1) / den);
1487  }
1488 
1489  // IFFT:
1490  CMatrixFloat res_R, res_I;
1491  math::idft2_complex(I2_R, I2_I, res_R, res_I);
1492 
1493  out_corr.setSize(ly, lx);
1494  for (y = 0; y < ly; y++)
1495  for (x = 0; x < lx; x++)
1496  out_corr(y, x) = sqrt(square(res_R(y, x)) + square(res_I(y, x)));
1497 
1498  MRPT_END
1499 }
n
GLenum GLsizei n
Definition: glext.h:5074
mrpt::math::FFT_TYPE
float FFT_TYPE
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
Definition: fourier.cpp:149
mrpt::math::dft2_real
void dft2_real(const CMatrixFloat &in_data, CMatrixFloat &out_real, CMatrixFloat &out_imag)
Compute the 2D Discrete Fourier Transform (DFT) of a real matrix, returning the real and imaginary pa...
Definition: fourier.cpp:970
fourier.h
mrpt::math::idft2_complex
void idft2_complex(const CMatrixFloat &in_real, const CMatrixFloat &in_imag, CMatrixFloat &out_real, CMatrixFloat &out_imag)
Compute the 2D inverse Discrete Fourier Transform (DFT).
Definition: fourier.cpp:1337
t
GLdouble GLdouble t
Definition: glext.h:3689
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: eigen_frwds.h:44
mrpt::math::rdft
static void rdft(int n, int isgn, FFT_TYPE *a, int *ip, FFT_TYPE *w)
Definition: fourier.cpp:568
mrpt::math::rdft2d
static void rdft2d(int n1, int n2, int isgn, FFT_TYPE **a, FFT_TYPE *t, int *ip, FFT_TYPE *w)
Definition: fourier.cpp:736
c
const GLubyte * c
Definition: glext.h:6313
mrpt::math::makewt
static void makewt(int nw, int *ip, FFT_TYPE *w)
Definition: fourier.cpp:398
math-precomp.h
mrpt::math::realft
static void realft(float data[], unsigned long n)
Definition: fourier.cpp:102
mrpt::math::makect
static void makect(int nc, int *ip, FFT_TYPE *c)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
Definition: fourier.cpp:432
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
w
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
R
const float R
Definition: CKinematicChain.cpp:138
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::square
T square(const T x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:18
mrpt::math::CMatrixTemplateNumeric
A matrix of dynamic size.
Definition: CMatrixTemplateNumeric.h:37
mrpt::math::cdft
static void cdft(int n, int isgn, FFT_TYPE *a, int *ip, FFT_TYPE *w)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
Definition: fourier.cpp:524
data
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3547
mrpt::math::cross_correlation_FFT
void cross_correlation_FFT(const CMatrixFloat &A, const CMatrixFloat &B, CMatrixFloat &out_corr)
Correlation of two matrixes using 2D FFT.
Definition: fourier.cpp:1451
mrpt::math::four1
static void four1(float data[], unsigned long nn, int isign)
Definition: fourier.cpp:33
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
mrpt::math::fft_real
void fft_real(CVectorFloat &in_realData, CVectorFloat &out_FFT_Re, CVectorFloat &out_FFT_Im, CVectorFloat &out_FFT_Mag)
Computes the FFT of a 2^N-size vector of real numbers, and returns the Re+Im+Magnitude parts.
Definition: fourier.cpp:929
mrpt::round2up
T round2up(T val)
Round up to the nearest power of two of a given number.
Definition: core/include/mrpt/core/bits_math.h:156
scale
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:6503
bits_math.h
mrpt::math::idft2_real
void idft2_real(const CMatrixFloat &in_real, const CMatrixFloat &in_imag, CMatrixFloat &out_data)
Compute the 2D inverse Discrete Fourier Transform (DFT)
Definition: fourier.cpp:1070
mrpt::math::rftfsub
static void rftfsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
Definition: fourier.cpp:544
mrpt::math::dft2_complex
void dft2_complex(const CMatrixFloat &in_real, const CMatrixFloat &in_imag, CMatrixFloat &out_real, CMatrixFloat &out_imag)
Compute the 2D Discrete Fourier Transform (DFT) of a complex matrix, returning the real and imaginary...
Definition: fourier.cpp:1232
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
myGeneralDFT
static void myGeneralDFT(int sign, const CMatrixFloat &in_real, const CMatrixFloat &in_imag, CMatrixFloat &out_real, CMatrixFloat &out_imag)
Definition: fourier.cpp:1176
mrpt::math::cdft2d
static void cdft2d(int n1, int n2, int isgn, FFT_TYPE **a, FFT_TYPE *t, int *ip, FFT_TYPE *w)
Definition: fourier.cpp:888
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
mrpt::math::rftbsub
static void rftbsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
Definition: fourier.cpp:613
M_2PI
#define M_2PI
Definition: common.h:58
mrpt::math::cftbsub
static void cftbsub(int n, FFT_TYPE *a, FFT_TYPE *w)
Definition: fourier.cpp:158
mrpt::sign
int sign(T x)
Returns the sign of X as "1" or "-1".
Definition: core/include/mrpt/core/bits_math.h:68
y
GLenum GLint GLint y
Definition: glext.h:3538
mrpt::math::cftfsub
static void cftfsub(int n, FFT_TYPE *a, FFT_TYPE *w)
Definition: fourier.cpp:278
x
GLenum GLint x
Definition: glext.h:3538
a
GLubyte GLubyte GLubyte a
Definition: glext.h:6279
mrpt::math::bitrv2
static void bitrv2(int n, int *ip, FFT_TYPE *a)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
Definition: fourier.cpp:457
mrpt::system::os::memcpy
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:356


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST