45 void four1(
float data[],
unsigned long nn,
int isign)
47 unsigned long n, mmax, m, j, i;
48 double wtemp, wr, wpr, wpi, wi,
64 while (m >= 2 && j > m)
75 unsigned long istep = mmax << 1;
76 theta = isign * (6.28318530717959 /
78 wtemp = sin(0.5 * theta);
79 wpr = -2.0 * wtemp * wtemp;
83 for (m = 1; m < mmax; m += 2)
85 for (i = m; i <=
n; i += istep)
88 tempr = (float)(wr *
data[j] - wi *
data[j + 1]);
89 tempi = (float)(wr *
data[j + 1] + wi *
data[j]);
95 wr = (wtemp = wr) * wpr - wi * wpi +
97 wi = wi * wpr + wtemp * wpi + wi;
116 unsigned long i, i1, i2, i3, i4, np3;
117 float c1 = 0.5, c2, h1r, h1i, h2r, h2i;
118 double wr, wi, wpr, wpi, wtemp,
120 theta = 3.141592653589793 / (double)(
n >> 1);
125 wtemp = sin(0.5 * theta);
126 wpr = -2.0 * wtemp * wtemp;
131 for (i = 2; i <= (n >> 2); i++)
133 i4 = 1 + (i3 = np3 - (i2 = 1 + (i1 = i + i - 1)));
140 (float)(h1r + wr * h2r - wi * h2i);
143 data[i2] = (float)(h1i + wr * h2i + wi * h2r);
144 data[i3] = (float)(h1r - wr * h2r + wi * h2i);
145 data[i4] = (float)(-h1i + wr * h2i + wi * h2r);
146 wr = (wtemp = wr) * wpr - wi * wpi + wr;
147 wi = wi * wpr + wtemp * wpi + wi;
175 int j, j1, j2, j3, k, k1, ks, l, m;
176 FFT_TYPE wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
177 FFT_TYPE x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
183 for (j = 0; j <= l - 2; j += 2)
189 x0i =
a[j + 1] +
a[j1 + 1];
191 x1i =
a[j + 1] -
a[j1 + 1];
193 x2i =
a[j2 + 1] +
a[j3 + 1];
195 x3i =
a[j2 + 1] -
a[j3 + 1];
197 a[j + 1] = x0i + x2i;
199 a[j2 + 1] = x0i - x2i;
201 a[j1 + 1] = x1i + x3r;
203 a[j3 + 1] = x1i - x3r;
208 for (j = m; j <= l + m - 2; j += 2)
214 x0i =
a[j + 1] +
a[j1 + 1];
216 x1i =
a[j + 1] -
a[j1 + 1];
218 x2i =
a[j2 + 1] +
a[j3 + 1];
220 x3i =
a[j2 + 1] -
a[j3 + 1];
222 a[j + 1] = x0i + x2i;
224 a[j2 + 1] = x0r - x2r;
227 a[j1] = wk1r * (x0r - x0i);
228 a[j1 + 1] = wk1r * (x0r + x0i);
231 a[j3] = wk1r * (x0i - x0r);
232 a[j3 + 1] = wk1r * (x0i + x0r);
236 for (k = (m << 1); k <=
n - m; k += m)
241 wk1i =
w[(k1 << 1) + 1];
244 wk3r = wk1r - 2 * wk2i * wk1i;
245 wk3i = 2 * wk2i * wk1r - wk1i;
246 for (j = k; j <= l + k - 2; j += 2)
252 x0i =
a[j + 1] +
a[j1 + 1];
254 x1i =
a[j + 1] -
a[j1 + 1];
256 x2i =
a[j2 + 1] +
a[j3 + 1];
258 x3i =
a[j2 + 1] -
a[j3 + 1];
260 a[j + 1] = x0i + x2i;
263 a[j2] = wk2r * x0r - wk2i * x0i;
264 a[j2 + 1] = wk2r * x0i + wk2i * x0r;
267 a[j1] = wk1r * x0r - wk1i * x0i;
268 a[j1 + 1] = wk1r * x0i + wk1i * x0r;
271 a[j3] = wk3r * x0r - wk3i * x0i;
272 a[j3 + 1] = wk3r * x0i + wk3i * x0r;
280 for (j = 0; j <= l - 2; j += 2)
284 x0i =
a[j + 1] -
a[j1 + 1];
286 a[j + 1] +=
a[j1 + 1];
295 int j, j1, j2, j3, k, k1, ks, l, m;
296 FFT_TYPE wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
297 FFT_TYPE x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
303 for (j = 0; j <= l - 2; j += 2)
309 x0i =
a[j + 1] +
a[j1 + 1];
311 x1i =
a[j + 1] -
a[j1 + 1];
313 x2i =
a[j2 + 1] +
a[j3 + 1];
315 x3i =
a[j2 + 1] -
a[j3 + 1];
317 a[j + 1] = x0i + x2i;
319 a[j2 + 1] = x0i - x2i;
321 a[j1 + 1] = x1i - x3r;
323 a[j3 + 1] = x1i + x3r;
328 for (j = m; j <= l + m - 2; j += 2)
334 x0i =
a[j + 1] +
a[j1 + 1];
336 x1i =
a[j + 1] -
a[j1 + 1];
338 x2i =
a[j2 + 1] +
a[j3 + 1];
340 x3i =
a[j2 + 1] -
a[j3 + 1];
342 a[j + 1] = x0i + x2i;
344 a[j2 + 1] = x2r - x0r;
347 a[j1] = wk1r * (x0i + x0r);
348 a[j1 + 1] = wk1r * (x0i - x0r);
351 a[j3] = wk1r * (x0r + x0i);
352 a[j3 + 1] = wk1r * (x0r - x0i);
356 for (k = (m << 1); k <=
n - m; k += m)
361 wk1i =
w[(k1 << 1) + 1];
364 wk3r = wk1r - 2 * wk2i * wk1i;
365 wk3i = 2 * wk2i * wk1r - wk1i;
366 for (j = k; j <= l + k - 2; j += 2)
372 x0i =
a[j + 1] +
a[j1 + 1];
374 x1i =
a[j + 1] -
a[j1 + 1];
376 x2i =
a[j2 + 1] +
a[j3 + 1];
378 x3i =
a[j2 + 1] -
a[j3 + 1];
380 a[j + 1] = x0i + x2i;
383 a[j2] = wk2r * x0r + wk2i * x0i;
384 a[j2 + 1] = wk2r * x0i - wk2i * x0r;
387 a[j1] = wk1r * x0r + wk1i * x0i;
388 a[j1 + 1] = wk1r * x0i - wk1i * x0r;
391 a[j3] = wk3r * x0r + wk3i * x0i;
392 a[j3 + 1] = wk3r * x0i - wk3i * x0r;
400 for (j = 0; j <= l - 2; j += 2)
404 x0i =
a[j + 1] -
a[j1 + 1];
406 a[j + 1] +=
a[j1 + 1];
424 delta = atan(1.0f) / nwh;
427 w[nwh] = cos(delta * nwh);
429 for (j = 2; j <= nwh - 2; j += 2)
456 delta = atan(1.0f) / nch;
458 c[nch] = 0.5f * cos(delta * nch);
459 for (j = 1; j <= nch - 1; j++)
461 c[j] = 0.5f * cos(delta * j);
462 c[nc - j] = 0.5f * sin(delta * j);
474 int j, j1, k, k1, l, m, m2;
483 for (j = 0; j <= m - 1; j++)
485 ip[m + j] = ip[j] + l;
491 for (k = 1; k <= m - 1; k++)
493 for (j = 0; j <= k - 1; j++)
495 j1 = (j << 1) + ip[k];
496 k1 = (k << 1) + ip[j];
500 a[j1 + 1] =
a[k1 + 1];
509 for (k = 1; k <= m - 1; k++)
511 for (j = 0; j <= k - 1; j++)
513 j1 = (j << 1) + ip[k];
514 k1 = (k << 1) + ip[j];
518 a[j1 + 1] =
a[k1 + 1];
526 a[j1 + 1] =
a[k1 + 1];
541 if (
n > (ip[0] << 2))
566 for (k = (
n >> 1) - 2; k >= 2; k -= 2)
573 xi =
a[k + 1] +
a[j + 1];
574 yr = wkr * xr + wki * xi;
575 yi = wkr * xi - wki * xr;
591 for (k = 1; k <= m - 1; k++)
594 wkr =
c[kk] -
c[nc - kk];
595 wki =
c[kk] +
c[nc - kk];
597 xr = wki *
a[k] - wkr *
a[j];
598 a[k] = wkr *
a[k] + wki *
a[j];
612 for (k = 1; k <= m - 1; k++)
615 wkr =
c[kk] -
c[nc - kk];
616 wki =
c[kk] +
c[nc - kk];
618 xr = wki *
a[j] - wkr *
a[k];
619 a[j] = wkr *
a[j] + wki *
a[k];
644 a[1] = 0.5f * (
a[0] -
a[1]);
677 for (k = (
n >> 1) - 2; k >= 2; k -= 2)
684 xi =
a[k + 1] +
a[j + 1];
685 yr = wkr * xr - wki * xi;
686 yi = wkr * xi + wki * xr;
796 int n, nw, nc, n1h, i, j, i2;
819 for (i = 1; i <= n1h - 1; i++)
822 xi =
a[i][0] -
a[j][0];
825 xi =
a[j][1] -
a[i][1];
829 for (j = 0; j <= n2 - 2; j += 2)
831 for (i = 0; i <= n1 - 1; i++)
835 t[i2 + 1] =
a[i][j + 1];
837 cdft(n1 << 1, isgn,
t, ip,
w);
838 for (i = 0; i <= n1 - 1; i++)
842 a[i][j + 1] =
t[i2 + 1];
845 for (i = 0; i <= n1 - 1; i++)
847 rdft(n2, isgn,
a[i], ip,
w);
852 for (i = 0; i <= n1 - 1; i++)
854 rdft(n2, isgn,
a[i], ip,
w);
856 for (j = 0; j <= n2 - 2; j += 2)
858 for (i = 0; i <= n1 - 1; i++)
862 t[i2 + 1] =
a[i][j + 1];
864 cdft(n1 << 1, isgn,
t, ip,
w);
865 for (i = 0; i <= n1 - 1; i++)
869 a[i][j + 1] =
t[i2 + 1];
872 for (i = 1; i <= n1h - 1; i++)
875 a[j][0] = 0.5f * (
a[i][0] -
a[j][0]);
877 a[j][1] = 0.5f * (
a[i][1] +
a[j][1]);
957 if (
n > (ip[0] << 2))
961 for (i = 0; i <= n1 - 1; i++)
963 cdft(n2, isgn,
a[i], ip,
w);
965 for (j = 0; j <= n2 - 2; j += 2)
967 for (i = 0; i <= n1 - 1; i++)
971 t[i2 + 1] =
a[i][j + 1];
973 cdft(n1 << 1, isgn,
t, ip,
w);
974 for (i = 0; i <= n1 - 1; i++)
978 a[i][j + 1] =
t[i2 + 1];
992 (
std * 2.506628274631000502415765284811);
1004 return dim * pow(1.0 - 2.0 / (9 * dim) +
1016 for (
unsigned int i = 2; i <=
n; i++) ret *= i;
1028 for (
unsigned int i = 2; i <=
n; i++) retLog += ::log((
double)
n);
1030 return ::exp(retLog);
1042 unsigned long n = (
unsigned long)in_realData.size();
1048 memcpy(&auxVect[1], &in_realData[0],
n *
sizeof(auxVect[0]));
1052 unsigned int n_2 = 1 + (
n / 2);
1054 out_FFT_Re.resize(n_2);
1055 out_FFT_Im.resize(n_2);
1056 out_FFT_Mag.resize(n_2);
1058 for (
unsigned int i = 0; i < n_2; i++)
1061 out_FFT_Re[i] = auxVect[2];
1063 out_FFT_Re[i] = auxVect[1 + i * 2];
1065 if (i == 0 || i == (n_2 - 1))
1068 out_FFT_Im[i] = auxVect[1 + i * 2 + 1];
1070 out_FFT_Mag[i] = sqrt(
square(out_FFT_Re[i]) +
square(out_FFT_Im[i]));
1083 static const double a[6] = {-3.969683028665376e+01, 2.209460984245205e+02,
1084 -2.759285104469687e+02, 1.383577518672690e+02,
1085 -3.066479806614716e+01, 2.506628277459239e+00};
1086 static const double b[5] = {-5.447609879822406e+01, 1.615858368580409e+02,
1087 -1.556989798598866e+02, 6.680131188771972e+01,
1088 -1.328068155288572e+01};
1089 static const double c[6] = {-7.784894002430293e-03, -3.223964580411365e-01,
1090 -2.400758277161838e+00, -2.549732539343734e+00,
1091 4.374664141464968e+00, 2.938163982698783e+00};
1092 static const double d[4] = {7.784695709041462e-03, 3.224671290700398e-01,
1093 2.445134137142996e+00, 3.754408661907416e+00};
1105 u = u * (((((
a[0] *
t +
a[1]) *
t +
a[2]) *
t +
a[3]) *
t +
a[4]) *
t +
1107 (((((
b[0] *
t +
b[1]) *
t +
b[2]) *
t +
b[3]) *
t +
b[4]) *
t + 1);
1112 t = sqrt(-2 * ::log(
q));
1113 u = (((((
c[0] *
t +
c[1]) *
t +
c[2]) *
t +
c[3]) *
t +
c[4]) *
t +
1115 ((((d[0] *
t + d[1]) *
t + d[2]) *
t + d[3]) *
t + 1);
1122 t =
t * 2.506628274631000502415765284811 *
1124 u = u -
t / (1 + u *
t / 2);
1126 return (
p > 0.5 ? -u : u);
1134 static const double a[5] = {1.161110663653770e-002, 3.951404679838207e-001,
1135 2.846603853776254e+001, 1.887426188426510e+002,
1136 3.209377589138469e+003};
1137 static const double b[5] = {1.767766952966369e-001, 8.344316438579620e+000,
1138 1.725514762600375e+002, 1.813893686502485e+003,
1139 8.044716608901563e+003};
1140 static const double c[9] = {
1141 2.15311535474403846e-8, 5.64188496988670089e-1, 8.88314979438837594e00,
1142 6.61191906371416295e01, 2.98635138197400131e02, 8.81952221241769090e02,
1143 1.71204761263407058e03, 2.05107837782607147e03, 1.23033935479799725E03};
1144 static const double d[9] = {
1145 1.00000000000000000e00, 1.57449261107098347e01, 1.17693950891312499e02,
1146 5.37181101862009858e02, 1.62138957456669019e03, 3.29079923573345963e03,
1147 4.36261909014324716e03, 3.43936767414372164e03, 1.23033935480374942e03};
1148 static const double p[6] = {1.63153871373020978e-2, 3.05326634961232344e-1,
1149 3.60344899949804439e-1, 1.25781726111229246e-1,
1150 1.60837851487422766e-2, 6.58749161529837803e-4};
1151 static const double q[6] = {1.00000000000000000e00, 2.56852019228982242e00,
1152 1.87295284992346047e00, 5.27905102951428412e-1,
1153 6.05183413124413191e-2, 2.33520497626869185e-3};
1160 if (
y <= 0.46875 * 1.4142135623730950488016887242097)
1164 y = u * ((((
a[0] *
z +
a[1]) *
z +
a[2]) *
z +
a[3]) *
z +
a[4]) /
1165 ((((
b[0] *
z +
b[1]) *
z +
b[2]) *
z +
b[3]) *
z +
b[4]);
1169 z = ::exp(-
y *
y / 2) / 2;
1173 y =
y / 1.4142135623730950488016887242097;
1174 y = ((((((((
c[0] *
y +
c[1]) *
y +
c[2]) *
y +
c[3]) *
y +
c[4]) *
y +
1183 / ((((((((d[0] *
y + d[1]) *
y + d[2]) *
y + d[3]) *
y + d[4]) *
y +
1197 z =
z * 1.4142135623730950488016887242097 /
y;
1199 y =
y * (((((
p[0] *
y +
p[1]) *
y +
p[2]) *
y +
p[3]) *
y +
p[4]) *
y +
1201 (((((
q[0] *
y +
q[1]) *
y +
q[2]) *
y +
q[3]) *
y +
q[4]) *
y +
1203 y =
z * (0.564189583547756286948 -
y);
1205 return (u < 0.0 ?
y : 1 -
y);
1220 size_t dim1 = in_data.getRowCount();
1221 size_t dim2 = in_data.getColCount();
1232 a =
new float_ptr[dim1];
1233 for (i = 0; i < dim1; i++)
1236 for (j = 0; j < dim2; j++)
a[i][j] = in_data.get_unsafe(i, j);
1240 ip =
new int[(int)ceil(20 + 2 + sqrt((
FFT_TYPE)max(dim1, dim2 / 2)))];
1242 w =
new FFT_TYPE[max(dim1 / 2, dim2 / 4) + dim2 / 4 + 20];
1246 rdft2d((
int)dim1, (
int)dim2, 1,
a,
t, ip,
w);
1250 out_real.setSize(dim1, dim2);
1251 out_imag.setSize(dim1, dim2);
1256 for (i = 1; i < dim1; i++)
1257 for (j = 1; j < dim2 / 2; j++)
1259 out_real.set_unsafe(i, j, (
float)
a[i][j * 2]);
1260 out_real.set_unsafe(dim1 - i, dim2 - j, (
float)
a[i][j * 2]);
1261 out_imag.set_unsafe(i, j, (
float)-
a[i][j * 2 + 1]);
1262 out_imag.set_unsafe(dim1 - i, dim2 - j, (
float)
a[i][j * 2 + 1]);
1267 for (j = 1; j < dim2 / 2; j++)
1269 out_real.set_unsafe(0, j, (
float)
a[0][j * 2]);
1270 out_real.set_unsafe(0, dim2 - j, (
float)
a[0][j * 2]);
1271 out_imag.set_unsafe(0, j, (
float)-
a[0][j * 2 + 1]);
1272 out_imag.set_unsafe(0, dim2 - j, (
float)
a[0][j * 2 + 1]);
1280 for (i = 1; i < dim1 / 2; i++)
1282 out_real.set_unsafe(i, 0, (
float)
a[i][0]);
1283 out_real.set_unsafe(dim1 - i, 0, (
float)
a[i][0]);
1284 out_imag.set_unsafe(i, 0, (
float)-
a[i][1]);
1285 out_imag.set_unsafe(dim1 - i, 0, (
float)
a[i][1]);
1286 out_real.set_unsafe(i, dim2 / 2, (
float)
a[dim1 - i][1]);
1287 out_real.set_unsafe(dim1 - i, dim2 / 2, (
float)
a[dim1 - i][1]);
1288 out_imag.set_unsafe(i, dim2 / 2, (
float)
a[dim1 - i][0]);
1289 out_imag.set_unsafe(dim1 - i, dim2 / 2, (
float)-
a[dim1 - i][0]);
1296 out_real.set_unsafe(0, 0, (
float)
a[0][0]);
1297 out_real.set_unsafe(0, dim2 / 2, (
float)
a[0][1]);
1298 out_real.set_unsafe(dim1 / 2, 0, (
float)
a[dim1 / 2][0]);
1299 out_real.set_unsafe(dim1 / 2, dim2 / 2, (
float)
a[dim1 / 2][1]);
1302 for (i = 0; i < dim1; i++)
delete[]
a[i];
1323 ASSERT_(in_real.getColCount() == in_imag.getColCount());
1324 ASSERT_(in_real.getRowCount() == in_imag.getRowCount());
1327 size_t dim1 = in_real.getRowCount();
1328 size_t dim2 = in_real.getColCount();
1342 a =
new float_ptr[dim1];
1343 for (i = 0; i < dim1; i++)
a[i] =
new FFT_TYPE[dim2];
1348 for (i = 1; i < dim1; i++)
1349 for (j = 1; j < dim2 / 2; j++)
1351 a[i][2 * j] = in_real.get_unsafe(i, j);
1352 a[i][2 * j + 1] = -in_imag.get_unsafe(i, j);
1358 for (j = 1; j < dim2 / 2; j++)
1360 a[0][2 * j] = in_real.get_unsafe(0, j);
1361 a[0][2 * j + 1] = -in_imag.get_unsafe(0, j);
1369 for (i = 1; i < dim1 / 2; i++)
1371 a[i][0] = in_real.get_unsafe(i, 0);
1372 a[i][1] = -in_imag.get_unsafe(i, 0);
1373 a[dim1 - i][1] = in_real.get_unsafe(i, dim2 / 2);
1374 a[dim1 - i][0] = in_imag.get_unsafe(i, dim2 / 2);
1381 a[0][0] = in_real.get_unsafe(0, 0);
1382 a[0][1] = in_real.get_unsafe(0, dim2 / 2);
1383 a[dim1 / 2][0] = in_real.get_unsafe(dim1 / 2, 0);
1384 a[dim1 / 2][1] = in_real.get_unsafe(dim1 / 2, dim2 / 2);
1387 ip =
new int[(int)ceil(20 + 2 + sqrt((
FFT_TYPE)max(dim1, dim2 / 2)))];
1389 w =
new FFT_TYPE[max(dim1 / 2, dim2 / 4) + dim2 / 4 + 20];
1393 rdft2d((
int)dim1, (
int)dim2, -1,
a,
t, ip,
w);
1397 out_data.setSize(dim1, dim2);
1401 for (i = 0; i < dim1; i++)
1402 for (j = 0; j < dim2; j++)
1403 out_data.set_unsafe(i, j, (
float)(
a[i][j] *
scale));
1406 for (i = 0; i < dim1; i++)
delete[]
a[i];
1424 ASSERT_(in_real.getRowCount() == in_imag.getRowCount())
1425 ASSERT_(in_real.getColCount() == in_imag.getColCount())
1428 size_t dim1 = in_real.getRowCount();
1429 size_t dim2 = in_real.getColCount();
1431 size_t k1, k2, n1, n2;
1433 float ang1 = (float)(
sign *
M_2PI / dim1);
1434 float ang2 = (float)(
sign *
M_2PI / dim2);
1437 float scale =
sign == 1 ? (1.0f / (dim1 * dim2)) : 1;
1439 out_real.setSize(dim1, dim2);
1440 out_imag.setSize(dim1, dim2);
1442 for (k1 = 0; k1 < dim1; k1++)
1444 for (k2 = 0; k2 < dim2; k2++)
1448 for (n1 = 0; n1 < dim1; n1++)
1450 phase = ang1 * n1 * k1;
1451 for (n2 = 0; n2 < dim2; n2++)
1456 R += w_r * in_real.get_unsafe(n1, n2) -
1457 w_i * in_imag.get_unsafe(n1, n2);
1458 I += w_i * in_real.get_unsafe(n1, n2) +
1459 w_r * in_imag.get_unsafe(n1, n2);
1466 out_real.set_unsafe(k1, k2,
R *
scale);
1467 out_imag.set_unsafe(k1, k2, I *
scale);
1482 ASSERT_(in_real.getRowCount() == in_imag.getRowCount())
1483 ASSERT_(in_real.getColCount() == in_imag.getColCount())
1486 size_t dim1 = in_real.getRowCount();
1487 size_t dim2 = in_real.getColCount();
1494 if (dim1IsPower2 && dim2IsPower2)
1505 static int* ip =
nullptr;
1510 static int alreadyInitSize1 = -1, alreadyInitSize2 = -1;
1512 if (alreadyInitSize1 != (
int)dim1 || alreadyInitSize2 != (int)dim2)
1517 for (i = 0; i < dim1; i++)
delete[]
a[i];
1520 if (ip)
delete[] ip;
1524 alreadyInitSize1 = (int)dim1;
1525 alreadyInitSize2 = (int)dim2;
1527 a =
new float_ptr[dim1];
1528 for (i = 0; i < dim1; i++)
a[i] =
new FFT_TYPE[2 * dim2];
1531 ip =
new int[(int)ceil(
1532 20 + 2 + sqrt((
FFT_TYPE)max(dim1, dim2 / 2)))];
1534 w =
new FFT_TYPE[max(dim1 / 2, dim2 / 4) + dim2 / 4 + 20];
1539 for (i = 0; i < dim1; i++)
1540 for (j = 0; j < dim2; j++)
1542 a[i][2 * j + 0] = in_real.get_unsafe(i, j);
1543 a[i][2 * j + 1] = in_imag.get_unsafe(i, j);
1548 cdft2d((
int)dim1, (
int)(2 * dim2), 1,
a,
t, ip,
w);
1552 out_real.setSize(dim1, dim2);
1553 out_imag.setSize(dim1, dim2);
1558 for (i = 0; i < dim1; i++)
1559 for (j = 0; j < dim2; j++)
1561 out_real.set_unsafe(i, j, (
float)
a[i][j * 2 + 0]);
1562 out_imag.set_unsafe(i, j, (
float)
a[i][j * 2 + 1]);
1571 printf(
"Using general DFT...\n");
1572 myGeneralDFT(-1, in_real, in_imag, out_real, out_imag);
1587 ASSERT_(in_real.getRowCount() == in_imag.getRowCount())
1588 ASSERT_(in_real.getColCount() == in_imag.getColCount())
1591 size_t dim1 = in_real.getRowCount();
1592 size_t dim2 = in_real.getColCount();
1599 if (dim1IsPower2 && dim2IsPower2)
1610 static int* ip =
nullptr;
1618 static int alreadyInitSize1 = -1, alreadyInitSize2 = -1;
1620 if (alreadyInitSize1 != (
int)dim1 || alreadyInitSize2 != (int)dim2)
1625 for (i = 0; i < dim1; i++)
delete[]
a[i];
1628 if (ip)
delete[] ip;
1632 alreadyInitSize1 = (int)dim1;
1633 alreadyInitSize2 = (int)dim2;
1635 a =
new float_ptr[dim1];
1636 for (i = 0; i < dim1; i++)
a[i] =
new FFT_TYPE[2 * dim2];
1638 ip =
new int[(int)ceil(
1639 20 + 2 + sqrt((
FFT_TYPE)max(dim1, dim2 / 2)))];
1641 w =
new FFT_TYPE[max(dim1 / 2, dim2 / 4) + dim2 / 4 + 20];
1646 for (i = 0; i < dim1; i++)
1647 for (j = 0; j < dim2; j++)
1649 a[i][2 * j + 0] = in_real.get_unsafe(i, j);
1650 a[i][2 * j + 1] = in_imag.get_unsafe(i, j);
1655 cdft2d((
int)dim1, (
int)(2 * dim2), -1,
a,
t, ip,
w);
1659 out_real.setSize(dim1, dim2);
1660 out_imag.setSize(dim1, dim2);
1667 for (i = 0; i < dim1; i++)
1668 for (j = 0; j < dim2; j++)
1672 out_real.set_unsafe(i, j, (
float)(
a[i][j * 2 + 0]));
1673 out_imag.set_unsafe(i, j, (
float)(
a[i][j * 2 + 1]));
1680 out_imag.set_unsafe(0, 0, 0);
1688 printf(
"Using general DFT...\n");
1701 if (!f.
readLine(str))
return false;
1703 const char*
s = str.c_str();
1705 char *nextTok, *context;
1706 const char* delim =
" \t";
1710 while (nextTok !=
nullptr)
1712 d.push_back(atoi(nextTok));
1725 if (!f.
readLine(str))
return false;
1727 const char*
s = str.c_str();
1729 char *nextTok, *context;
1730 const char* delim =
" \t";
1734 while (nextTok !=
nullptr)
1736 d.push_back(atof(nextTok));
1752 ASSERT_(logWeights.size() == logLikelihoods.size());
1754 if (!logWeights.size())
1760 double SUM1 = 0, SUM2 = 0, tmpVal;
1762 for (itLW = logWeights.begin(), itLL = logLikelihoods.begin();
1763 itLW != logWeights.end(); itLW++, itLL++)
1765 tmpVal = *itLW - lw_max;
1766 SUM1 += std::exp(tmpVal);
1767 SUM2 += std::exp(tmpVal + *itLL - ll_max);
1770 double res = -std::log(SUM1) + std::log(SUM2) + ll_max;
1783 size_t N = logLikelihoods.size();
1789 for (
size_t i = 0; i < N; i++) SUM1 += exp(logLikelihoods[i] - ll_max);
1791 double res = log(SUM1) - log(static_cast<double>(N)) + ll_max;
1801 if (angles.empty())
return 0;
1803 int W_phi_R = 0, W_phi_L = 0;
1804 double phi_R = 0, phi_L = 0;
1808 for (CVectorDouble::Index i = 0; i < angles.size(); i++)
1810 double phi = angles[i];
1811 if (abs(phi) > 1.5707963267948966192313216916398)
1814 if (phi < 0) phi = (
M_2PI + phi);
1830 if (W_phi_L) phi_L /=
static_cast<double>(W_phi_L);
1831 if (W_phi_R) phi_R /=
static_cast<double>(W_phi_R);
1837 return ((phi_L * W_phi_L + phi_R * W_phi_R) / (W_phi_L + W_phi_R));
1842 const string& style,
const size_t& nEllipsePoints)
1851 cov2, mean2, stdCount, style, nEllipsePoints);
1858 const string& style,
const size_t& nEllipsePoints)
1867 std::vector<float> X, Y, COS, SIN;
1873 X.resize(nEllipsePoints);
1874 Y.resize(nEllipsePoints);
1875 COS.resize(nEllipsePoints);
1876 SIN.resize(nEllipsePoints);
1879 for (Cos = COS.begin(), Sin = SIN.begin(), ang = 0; Cos != COS.end();
1880 ++Cos, ++Sin, ang += (
M_2PI / (nEllipsePoints - 1)))
1882 *Cos = (float)cos(ang);
1883 *Sin = (float)sin(ang);
1886 cov.eigenVectors(eigVec, eigVal);
1887 eigVal = eigVal.array().sqrt().matrix();
1888 M = eigVal * eigVec.adjoint();
1892 for (
x = X.begin(),
y = Y.begin(), Cos = COS.begin(), Sin = SIN.begin();
1893 x != X.end(); ++
x, ++
y, ++Cos, ++Sin)
1895 *
x = (float)(
mean[0] + stdCount * (*Cos * M(0, 0) + *Sin * M(1, 0)));
1896 *
y = (float)(
mean[1] + stdCount * (*Cos * M(0, 1) + *Sin * M(1, 1)));
1901 str +=
string(
"plot([ ");
1902 for (
x = X.begin();
x != X.end(); ++
x)
1905 if (
x != (X.end() - 1)) str +=
format(
",");
1908 for (
y = Y.begin();
y != Y.end(); ++
y)
1911 if (
y != (Y.end() - 1)) str +=
format(
",");
1914 str +=
format(
"],'%s');\n", style.c_str());
1918 cerr <<
"The matrix that led to error was: " << endl
1937 const size_t lx =
size(A, 2);
1938 const size_t ly =
size(A, 1);
1949 for (
y = 0;
y < ly;
y++)
1950 for (
x = 0;
x < lx;
x++)
1952 float r1 = I1_R.get_unsafe(
y,
x);
1953 float r2 = I2_R.get_unsafe(
y,
x);
1955 float ii1 = I1_I.get_unsafe(
y,
x);
1956 float ii2 = I2_I.get_unsafe(
y,
x);
1959 I2_R.set_unsafe(
y,
x, (r1 * r2 + ii1 * ii2) / den);
1960 I2_I.set_unsafe(
y,
x, (ii2 * r1 - r2 * ii1) / den);
1967 out_corr.setSize(ly, lx);
1968 for (
y = 0;
y < ly;
y++)
1969 for (
x = 0;
x < lx;
x++)
1976 const double x,
const double x0,
const double y0,
const double x1,
1977 const double y1,
bool wrap2pi)
1983 const double Ax = x1 - x0;
1984 const double Ay = y1 - y0;
1986 double r = y0 + Ay * (
x - x0) / Ax;
2000 const std::vector<double>& inV, std::vector<double>& outV,
2001 const int& _winSize,
const int& numberOfSigmas)
2004 ASSERT_((
int)inV.size() >= _winSize);
2006 size_t winSize = _winSize;
2011 size_t sz = inV.size();
2014 std::vector<double> aux(winSize);
2015 size_t mpoint = winSize / 2;
2016 for (
size_t k = 0; k < sz; ++k)
2020 size_t idx_to_start =
2021 std::max(
size_t(0), k - mpoint);
2025 aux.resize(n_elements);
2026 for (
size_t m = idx_to_start,
n = 0; m < idx_to_start + n_elements;
2030 std::sort(aux.begin(), aux.end());
2032 size_t auxSz = aux.size();
2033 size_t auxMPoint = auxSz / 2;
2034 outV[k] = (auxSz % 2) ? (aux[auxMPoint])
2035 : (0.5 * (aux[auxMPoint - 1] +
2050 T arg, T& lans, T& dans, T& pans,
unsigned int& j)
2055 lans = lans + std::log(arg / j);
2056 dans = std::exp(lans);
2060 dans = dans * arg / j;
2067 unsigned int degreesOfFreedom,
double noncentrality,
double arg,
double eps)
2070 noncentrality >= 0.0 && arg >= 0.0 &&
eps > 0.0,
2071 "noncentralChi2PDF_CDF(): parameters must be positive.")
2073 if (arg == 0.0 && degreesOfFreedom > 0)
return std::make_pair(0.0, 0.0);
2076 double b1 = 0.5 * noncentrality, ao = std::exp(-
b1), eps2 =
eps / ao,
2077 lnrtpi2 = 0.22579135264473, probability, density, lans, dans, pans,
2079 unsigned int maxit = 500, i, m;
2080 if (degreesOfFreedom % 2)
2083 lans = -0.5 * (arg + std::log(arg)) - lnrtpi2;
2084 dans = std::exp(lans);
2085 pans = std::erf(std::sqrt(arg / 2.0));
2091 dans = std::exp(lans);
2096 if (degreesOfFreedom == 0)
2099 degreesOfFreedom = 2;
2101 sum = 1.0 / ao - 1.0 - am;
2102 density = am * dans;
2103 probability = 1.0 + am * pans;
2108 degreesOfFreedom = degreesOfFreedom - 1;
2110 sum = 1.0 / ao - 1.0;
2111 while (i < degreesOfFreedom)
2113 degreesOfFreedom = degreesOfFreedom + 1;
2118 for (++m; m < maxit; ++m)
2123 density = density + am * dans;
2125 probability = probability + hold;
2126 if ((pans *
sum < eps2) && (hold < eps2))
break;
2128 if (m == maxit)
THROW_EXCEPTION(
"noncentralChi2PDF_CDF(): no convergence.");
2129 return std::make_pair(
2130 0.5 * ao * density,
std::min(1.0, std::max(0.0, ao * probability)));
2134 unsigned int degreesOfFreedom,
double arg,
double accuracy)
2137 degreesOfFreedom, 0.0, arg, accuracy)
2142 unsigned int degreesOfFreedom,
double noncentrality,
double arg)
2144 const double a = degreesOfFreedom + noncentrality;
2145 const double b = (
a + noncentrality) /
square(
a);
2147 (std::pow((
double)arg /
a, 1.0 / 3.0) - (1.0 - 2.0 / 9.0 *
b)) /
2148 std::sqrt(2.0 / 9.0 *
b);
2149 return 0.5 * (1.0 + std::erf(
t / std::sqrt(2.0)));
bool readLine(std::string &str)
Reads one string line from the file (until a new-line character)
double averageWrap2Pi(const CVectorDouble &angles)
Computes the average of a sequence of angles in radians taking into account the correct wrapping in t...
double chi2CDF(unsigned int degreesOfFreedom, double arg)
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, const float &stdCount, const std::string &style=std::string("b"), const size_t &nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
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).
#define MRPT_END_WITH_CLEAN_UP(stuff)
void bitrv2(int n, int *ip, FFT_TYPE *a)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
void rdft(int n, int isgn, FFT_TYPE *a, int *ip, FFT_TYPE *w)
GLenum GLenum GLenum GLenum GLenum scale
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...
#define MRPT_CHECK_NORMAL_NUMBER(val)
double normalCDF(double p)
Evaluates the Gaussian cumulative density function.
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
This CStream derived class allow using a file as a read/write binary stream, creating it if the file ...
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...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
const Scalar * const_iterator
void myGeneralDFT(int sign, const CMatrixFloat &in_real, const CMatrixFloat &in_imag, CMatrixFloat &out_real, CMatrixFloat &out_imag)
void cftbsub(int n, FFT_TYPE *a, FFT_TYPE *w)
GLubyte GLubyte GLubyte GLubyte w
void makect(int nc, int *ip, FFT_TYPE *c)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
T square(const T x)
Inline function for the square of a number.
double factorial(unsigned int n)
Computes the factorial of an integer number and returns it as a double value (internally it uses loga...
double normalQuantile(double p)
Evaluates the Gaussian distribution quantile for the probability value p=[0,1].
void makewt(int nw, int *ip, FFT_TYPE *w)
This base provides a set of functions for maths stuff.
T round2up(T val)
Round up to the nearest power of two of a given number.
void four1(float data[], unsigned long nn, int isign)
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
double averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
void cross_correlation_FFT(const CMatrixFloat &A, const CMatrixFloat &B, CMatrixFloat &out_corr)
Correlation of two matrixes using 2D FFT.
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).
char * strtok(char *str, const char *strDelimit, char **context) noexcept
An OS-independent method for tokenizing a string.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
uint64_t factorial64(unsigned int n)
Computes the factorial of an integer number and returns it as a 64-bit integer number.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
void dstsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
CONTAINER::Scalar maximum(const CONTAINER &v)
void rftfsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
GLsizei const GLchar ** string
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
void realft(float data[], unsigned long n)
int sign(T x)
Returns the sign of X as "1" or "-1".
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).
void noncentralChi2OneIteration(T arg, T &lans, T &dans, T &pans, unsigned int &j)
unsigned __int64 uint64_t
void cftfsub(int n, FFT_TYPE *a, FFT_TYPE *w)
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)
double chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
GLdouble GLdouble GLdouble r
std::pair< double, double > noncentralChi2PDF_CDF(unsigned int degreesOfFreedom, double noncentrality, double arg, double eps=1e-7)
Returns the 'exact' PDF (first) and CDF (second) of a Non-central chi-squared probability distributio...
A matrix of dynamic size.
void rftbsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
void medianFilter(const std::vector< double > &inV, std::vector< double > &outV, const int &winSize, const int &numberOfSigmas=2)
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
bool loadVector(utils::CFileStream &f, std::vector< int > &d)
Loads one row of a text file as a numerical std::vector.
double interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi=false)
Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).
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...
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
#define ASSERTMSG_(f, __ERROR_MSG)
GLsizei GLsizei GLenum GLenum const GLvoid * data
GLubyte GLubyte GLubyte a
double chi2PDF(unsigned int degreesOfFreedom, double arg, double accuracy=1e-7)
void dctsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
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).
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
double noncentralChi2CDF(unsigned int degreesOfFreedom, double noncentrality, double arg)
float FFT_TYPE
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).