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

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce1a28c9f Fri Aug 23 08:02:09 2019 +0200 at vie ago 23 08:10:11 CEST 2019