GSaha567/seq_level_training_data
052
1text,length,is_long_context,metric_val,label_metric2"/* +------------------------------------------------------------------------+3 | Mobile Robot Programming Toolkit (MRPT) |4 | http://www.mrpt.org/ |5 | |6 | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |7 | See: http://www.mrpt.org/Authors - All rights reserved. |8 | Released under BSD License. See details in http://www.mrpt.org/License |9 +------------------------------------------------------------------------+ */10 11#include ""math-precomp.h"" // Precompiled headers12 13#include <mrpt/math/fourier.h>14#include <algorithm>15#include <mrpt/core/bits_math.h>16#include <cmath>17 18using namespace mrpt;19using namespace std;20using namespace mrpt::math;21 22// Next we declare some auxiliary functions:23namespace mrpt::math24{25// Replaces data[1..2*nn] by its discrete Fourier transform, if isign is input26// as 1; or replaces27// data[1..2*nn] by nn times its inverse discrete Fourier transform, if isign is28// input as -1.29// data is a complex array of length nn or, equivalently, a real array of length30// 2*nn. nn MUST31// be an integer power of 2 (this is not checked for!).32static 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 (which92// is stored in array data[1..n]) by the positive frequency half of its complex93// Fourier transform.94// The real-valued first and last components of the complex transform are95// returned as elements96// data[1] and data[2], respectively. n must be a power of 2. This routine also97// calculates the98// inverse transform of a complex data array if it is the transform of real99// data. (Result in this case100// must be multiplied by 2/n.)101static 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 are122 // 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 to128 // form the true transform of129 // 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 the139 // 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 and146 without fee. You may distribute this ORIGINAL package.147 */148using FFT_TYPE = float;149 150static void makewt(int nw, int* ip, FFT_TYPE* w);151static void bitrv2(int n, int* ip, FFT_TYPE* a);152static void cftbsub(int n, FFT_TYPE* a, FFT_TYPE* w);153static void cftfsub(int n, FFT_TYPE* a, FFT_TYPE* w);154static void rftfsub(int n, FFT_TYPE* a, int nc, FFT_TYPE* c);155static void rftbsub(int n, FFT_TYPE* a, int nc, FFT_TYPE* c);156 157static 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 277static 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 397static 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 and429 without fee. You may distribute this ORIGINAL package.430 */431static 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 and454 without fee. You may distribute this ORIGINAL package.455 */456static 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 else491 {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 and521 without fee. You may distribute this ORIGINAL package.522 */523static 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 else538 {539 cftbsub(n, a, w);540 }541}542 543static 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 567static 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 else596 {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 612static 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 and639 without fee. You may distribute this ORIGINAL package.640 641-------- Real DFT / Inverse of Real DFT --------642 [definition]643 <case1> RDFT644 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<n2647 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<n2650 <case2> IRDFT (excluding scale)651 a[k1][k2] = (1/2) * sum_j1=0^n1-1 sum_j2=0^n2-1652 (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<n2657 (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 only667 rdft2d(n1, n2, 1, a, t, ip, w);668 <case2>669 ip[0] = 0; // first time only670 rdft2d(n1, n2, -1, a, t, ip, w);671 [parameters]672 n1 :data length (int)673 n1 >= 2, n1 = power of 2674 n2 :data length (int)675 n2 >= 2, n2 = power of 2676 a[0...n1-1][0...n2-1]677 :input/output data (FFT_TYPE **)678 <case1>679 output data680 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 data697 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 == 0717 2+sqrt(n/2); otherwise718 (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/4723 w[],ip[] are initialized if ip[0] == 0.724 [remark]725 Inverse of726 rdft2d(n1, n2, 1, a, t, ip, w);727 is728 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 */735static 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 else793 {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 and828 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<n2836 <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<n2840 (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 only844 cdft2d(n1, 2*n2, 1, a, t, ip, w);845 <case2>846 ip[0] = 0; // first time only847 cdft2d(n1, 2*n2, -1, a, t, ip, w);848 [parameters]849 n1 :data length (int)850 n1 >= 1, n1 = power of 2851 2*n2 :data length (int)852 n2 >= 1, n2 = power of 2853 a[0...n1-1][0...2*n2-1]854 :input/output data (double **)855 input data856 a[j1][2*j2] = Re(x[j1][j2]),857 a[j1][2*j2+1] = Im(x[j1][j2]),858 0<=j1<n1, 0<=j2<n2859 output data860 a[k1][2*k2] = Re(X[k1][k2]),861 a[k1][2*k2+1] = Im(X[k1][k2]),862 0<=k1<n1, 0<=k2<n2863 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 == 0868 2+sqrt(n/2); otherwise869 (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 of877 cdft2d(n1, 2*n2, -1, a, t, ip, w);878 is879 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*/887static 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::math926 927void mrpt::math::fft_real(928 CVectorFloat& in_realData, CVectorFloat& out_FFT_Re,929 CVectorFloat& out_FFT_Im, CVectorFloat& out_FFT_Mag)930{931 MRPT_START932 933 unsigned long 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 else954 out_FFT_Re[i] = auxVect[1 + i * 2];955 956 if (i == 0 || i == (n_2 - 1))957 out_FFT_Im[i] = 0;958 else959 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_END966}967 968void math::dft2_real(969 const CMatrixFloat& in_data, CMatrixFloat& out_real, CMatrixFloat& out_imag)970{971 MRPT_START972 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.get_unsafe(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.set_unsafe(i, j, (float)a[i][j * 2]);1017 out_real.set_unsafe(dim1 - i, dim2 - j, (float)a[i][j * 2]);1018 out_imag.set_unsafe(i, j, (float)-a[i][j * 2 + 1]);1019 out_imag.set_unsafe(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.set_unsafe(0, j, (float)a[0][j * 2]);1027 out_real.set_unsafe(0, dim2 - j, (float)a[0][j * 2]);1028 out_imag.set_unsafe(0, j, (float)-a[0][j * 2 + 1]);1029 out_imag.set_unsafe(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.set_unsafe(i, 0, (float)a[i][0]);1040 out_real.set_unsafe(dim1 - i, 0, (float)a[i][0]);1041 out_imag.set_unsafe(i, 0, (float)-a[i][1]);1042 out_imag.set_unsafe(dim1 - i, 0, (float)a[i][1]);1043 out_real.set_unsafe(i, dim2 / 2, (float)a[dim1 - i][1]);1044 out_real.set_unsafe(dim1 - i, dim2 / 2, (float)a[dim1 - i][1]);1045 out_imag.set_unsafe(i, dim2 / 2, (float)a[dim1 - i][0]);1046 out_imag.set_unsafe(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.set_unsafe(0, 0, (float)a[0][0]);1054 out_real.set_unsafe(0, dim2 / 2, (float)a[0][1]);1055 out_real.set_unsafe(dim1 / 2, 0, (float)a[dim1 / 2][0]);1056 out_real.set_unsafe(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_END1066}1067 1068void math::idft2_real(1069 const CMatrixFloat& in_real, const CMatrixFloat& in_imag,1070 CMatrixFloat& out_data)1071{1072 MRPT_START1073 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.get_unsafe(i, j);1106 a[i][2 * j + 1] = -in_imag.get_unsafe(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.get_unsafe(0, j);1115 a[0][2 * j + 1] = -in_imag.get_unsafe(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.get_unsafe(i, 0);1126 a[i][1] = -in_imag.get_unsafe(i, 0);1127 a[dim1 - i][1] = in_real.get_unsafe(i, dim2 / 2);1128 a[dim1 - i][0] = in_imag.get_unsafe(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.get_unsafe(0, 0);1136 a[0][1] = in_real.get_unsafe(0, dim2 / 2);1137 a[dim1 / 2][0] = in_real.get_unsafe(dim1 / 2, 0);1138 a[dim1 / 2][1] = in_real.get_unsafe(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++)1157 out_data.set_unsafe(i, j, (float)(a[i][j] * scale));1158 1159 // Free temporary memory:1160 for (i = 0; i < dim1; i++) delete[] a[i];1161 delete[] a;1162 delete[] t;1163 delete[] ip;1164 delete[] w;1165 1166 MRPT_END1167}1168 1169/*---------------------------------------------------------------1170 myGeneralDFT1171 1172 sign -> -1: DFT, 1:IDFT1173 ---------------------------------------------------------------*/1174static void myGeneralDFT(1175 int sign, const CMatrixFloat& in_real, const CMatrixFloat& in_imag,1176 CMatrixFloat& out_real, CMatrixFloat& out_imag)1177{1178 ASSERT_(in_real.rows() == in_imag.rows());1179 ASSERT_(in_real.cols() == in_imag.cols());1180 1181 // The dimensions:1182 size_t dim1 = in_real.rows();1183 size_t dim2 = in_real.cols();1184 1185 size_t k1, k2, n1, n2;1186 float w_r, w_i;1187 float ang1 = (float)(sign * M_2PI / dim1);1188 float ang2 = (float)(sign * M_2PI / dim2);1189 float phase;1190 float R, I;1191 float scale = sign == 1 ? (1.0f / (dim1 * dim2)) : 1;1192 1193 out_real.setSize(dim1, dim2);1194 out_imag.setSize(dim1, dim2);1195 1196 for (k1 = 0; k1 < dim1; k1++)1197 {1198 for (k2 = 0; k2 < dim2; k2++)1199 {1200 R = I = 0; // Accum: