Main MRPT website > C++ reference for MRPT 1.5.7
math.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "base-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/utils.h>
13 #include <mrpt/math/wrap2pi.h>
14 #include <mrpt/math/interp_fit.hpp>
15 #include <mrpt/math/data_utils.h>
16 #include <mrpt/math/distributions.h>
17 #include <mrpt/math/fourier.h>
18 
19 #include <mrpt/system/string_utils.h>
20 #include <mrpt/utils/CFileStream.h>
21 #include <mrpt/math/CMatrixD.h>
22 #include <mrpt/utils/types_math.h>
23 #include <mrpt/math/ops_matrices.h>
24 
25 #include <algorithm>
26 
27 using namespace mrpt;
28 using namespace mrpt::utils;
29 using namespace mrpt::math;
30 using namespace std;
31 
32 // Next we declare some auxiliary functions:
33 namespace mrpt
34 {
35  namespace math
36  {
37  //Replaces data[1..2*nn] by its discrete Fourier transform, if isign is input as 1; or replaces
38  //data[1..2*nn] by nn times its inverse discrete Fourier transform, if isign is input as -1.
39  //data is a complex array of length nn or, equivalently, a real array of length 2*nn. nn MUST
40  //be an integer power of 2 (this is not checked for!).
41  void four1(float data[], unsigned long nn, int isign)
42  {
43  unsigned long n,mmax,m,j,i;
44  double wtemp,wr,wpr,wpi,wi,theta; // Double precision for the trigonometric recurrences.
45  float tempr,tempi;
46 
47  n=nn << 1;
48  j=1;
49 
50  for (i=1;i<n;i+=2) // This is the bit-reversal section of the routine.
51  {
52  if (j > i)
53  {
54  swap(data[j],data[i]); // Exchange the two complex numbers.
55  swap(data[j+1],data[i+1]);
56  }
57  m=nn;
58  while (m >= 2 && j > m)
59  {
60  j -= m;
61  m >>= 1;
62  }
63  j += m;
64  }
65  // Here begins the Danielson-Lanczos section of the routine.
66  mmax=2;
67  while (n > mmax) // Outer loop executed log2 nn times.
68  {
69  unsigned long istep=mmax << 1;
70  theta=isign*(6.28318530717959/mmax); // Initialize the trigonometric recurrence.
71  wtemp=sin(0.5*theta);
72  wpr = -2.0*wtemp*wtemp;
73  wpi=sin(theta);
74  wr=1.0;
75  wi=0.0;
76  for (m=1;m<mmax;m+=2) // Here are the two nested inner loops.
77  {
78  for (i=m;i<=n;i+=istep)
79  {
80  j=i+mmax; // This is the Danielson-Lanczos formula:
81  tempr=(float) (wr*data[j]-wi*data[j+1]);
82  tempi=(float) (wr*data[j+1]+wi*data[j]);
83  data[j]=data[i]-tempr;
84  data[j+1]=data[i+1]-tempi;
85  data[i] += tempr;
86  data[i+1] += tempi;
87  }
88  wr=(wtemp=wr)*wpr-wi*wpi+wr; // Trigonometric recurrence.
89  wi=wi*wpr+wtemp*wpi+wi;
90  }
91  mmax=istep;
92  }
93  }
94 
95 
96  //Calculates the Fourier transform of a set of n real-valued data points. Replaces this data (which
97  //is stored in array data[1..n]) by the positive frequency half of its complex Fourier transform.
98  //The real-valued first and last components of the complex transform are returned as elements
99  //data[1] and data[2], respectively. n must be a power of 2. This routine also calculates the
100  //inverse transform of a complex data array if it is the transform of real data. (Result in this case
101  //must be multiplied by 2/n.)
102  void realft(float data[], unsigned long n)
103  {
104  unsigned long i,i1,i2,i3,i4,np3;
105  float c1=0.5,c2,h1r,h1i,h2r,h2i;
106  double wr,wi,wpr,wpi,wtemp,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 separated out of data.
122  h1i=c1*(data[i2]-data[i4]);
123  h2r = -c2*(data[i2]+data[i4]);
124  h2i=c2*(data[i1]-data[i3]);
125  data[i1]=(float)(h1r+wr*h2r-wi*h2i); // Here they are recombined to form the true transform of the original real data.
126  data[i2]=(float)(h1i+wr*h2i+wi*h2r);
127  data[i3]=(float)(h1r-wr*h2r+wi*h2i);
128  data[i4]=(float)( -h1i+wr*h2i+wi*h2r);
129  wr=(wtemp=wr)*wpr-wi*wpi+wr; // The recurrence.
130  wi=wi*wpr+wtemp*wpi+wi;
131  }
132 
133  data[1] = (h1r=data[1])+data[2];
134  // Squeeze the first and last data together to get them all within the original array.
135  data[2] = h1r-data[2];
136  }
137 
138  /**
139  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
140  You may use, copy, modify this code for any purpose and
141  without fee. You may distribute this ORIGINAL package.
142  */
143  typedef float FFT_TYPE;
144 
145  void makewt(int nw, int *ip, FFT_TYPE *w);
146  void bitrv2(int n, int *ip, FFT_TYPE *a);
147  void cftbsub(int n, FFT_TYPE *a, FFT_TYPE *w);
148  void cftfsub(int n, FFT_TYPE *a, FFT_TYPE *w);
149  void dctsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c);
150  void dstsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c);
151  void dctsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c);
152  void rftfsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c);
153  void rftbsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c);
154 
155 
156  void cftbsub(int n, FFT_TYPE *a, FFT_TYPE *w)
157  {
158  int j, j1, j2, j3, k, k1, ks, l, m;
159  FFT_TYPE wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
160  FFT_TYPE x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
161 
162  l = 2;
163  while ((l << 1) < n) {
164  m = l << 2;
165  for (j = 0; j <= l - 2; j += 2) {
166  j1 = j + l;
167  j2 = j1 + l;
168  j3 = j2 + l;
169  x0r = a[j] + a[j1];
170  x0i = a[j + 1] + a[j1 + 1];
171  x1r = a[j] - a[j1];
172  x1i = a[j + 1] - a[j1 + 1];
173  x2r = a[j2] + a[j3];
174  x2i = a[j2 + 1] + a[j3 + 1];
175  x3r = a[j2] - a[j3];
176  x3i = a[j2 + 1] - a[j3 + 1];
177  a[j] = x0r + x2r;
178  a[j + 1] = x0i + x2i;
179  a[j2] = x0r - x2r;
180  a[j2 + 1] = x0i - x2i;
181  a[j1] = x1r - x3i;
182  a[j1 + 1] = x1i + x3r;
183  a[j3] = x1r + x3i;
184  a[j3 + 1] = x1i - x3r;
185  }
186  if (m < n) {
187  wk1r = w[2];
188  for (j = m; j <= l + m - 2; j += 2) {
189  j1 = j + l;
190  j2 = j1 + l;
191  j3 = j2 + l;
192  x0r = a[j] + a[j1];
193  x0i = a[j + 1] + a[j1 + 1];
194  x1r = a[j] - a[j1];
195  x1i = a[j + 1] - a[j1 + 1];
196  x2r = a[j2] + a[j3];
197  x2i = a[j2 + 1] + a[j3 + 1];
198  x3r = a[j2] - a[j3];
199  x3i = a[j2 + 1] - a[j3 + 1];
200  a[j] = x0r + x2r;
201  a[j + 1] = x0i + x2i;
202  a[j2] = x2i - x0i;
203  a[j2 + 1] = x0r - x2r;
204  x0r = x1r - x3i;
205  x0i = x1i + x3r;
206  a[j1] = wk1r * (x0r - x0i);
207  a[j1 + 1] = wk1r * (x0r + x0i);
208  x0r = x3i + x1r;
209  x0i = x3r - x1i;
210  a[j3] = wk1r * (x0i - x0r);
211  a[j3 + 1] = wk1r * (x0i + x0r);
212  }
213  k1 = 1;
214  ks = -1;
215  for (k = (m << 1); k <= n - m; k += m) {
216  k1++;
217  ks = -ks;
218  wk1r = w[k1 << 1];
219  wk1i = w[(k1 << 1) + 1];
220  wk2r = ks * w[k1];
221  wk2i = w[k1 + ks];
222  wk3r = wk1r - 2 * wk2i * wk1i;
223  wk3i = 2 * wk2i * wk1r - wk1i;
224  for (j = k; j <= l + k - 2; j += 2) {
225  j1 = j + l;
226  j2 = j1 + l;
227  j3 = j2 + l;
228  x0r = a[j] + a[j1];
229  x0i = a[j + 1] + a[j1 + 1];
230  x1r = a[j] - a[j1];
231  x1i = a[j + 1] - a[j1 + 1];
232  x2r = a[j2] + a[j3];
233  x2i = a[j2 + 1] + a[j3 + 1];
234  x3r = a[j2] - a[j3];
235  x3i = a[j2 + 1] - a[j3 + 1];
236  a[j] = x0r + x2r;
237  a[j + 1] = x0i + x2i;
238  x0r -= x2r;
239  x0i -= x2i;
240  a[j2] = wk2r * x0r - wk2i * x0i;
241  a[j2 + 1] = wk2r * x0i + wk2i * x0r;
242  x0r = x1r - x3i;
243  x0i = x1i + x3r;
244  a[j1] = wk1r * x0r - wk1i * x0i;
245  a[j1 + 1] = wk1r * x0i + wk1i * x0r;
246  x0r = x1r + x3i;
247  x0i = x1i - x3r;
248  a[j3] = wk3r * x0r - wk3i * x0i;
249  a[j3 + 1] = wk3r * x0i + wk3i * x0r;
250  }
251  }
252  }
253  l = m;
254  }
255  if (l < n) {
256  for (j = 0; j <= l - 2; j += 2) {
257  j1 = j + l;
258  x0r = a[j] - a[j1];
259  x0i = a[j + 1] - a[j1 + 1];
260  a[j] += a[j1];
261  a[j + 1] += a[j1 + 1];
262  a[j1] = x0r;
263  a[j1 + 1] = x0i;
264  }
265  }
266  }
267 
268  void cftfsub(int n, FFT_TYPE *a, FFT_TYPE *w)
269  {
270  int j, j1, j2, j3, k, k1, ks, l, m;
271  FFT_TYPE wk1r, wk1i, wk2r, wk2i, wk3r, wk3i;
272  FFT_TYPE x0r, x0i, x1r, x1i, x2r, x2i, x3r, x3i;
273 
274  l = 2;
275  while ((l << 1) < n) {
276  m = l << 2;
277  for (j = 0; j <= l - 2; j += 2) {
278  j1 = j + l;
279  j2 = j1 + l;
280  j3 = j2 + l;
281  x0r = a[j] + a[j1];
282  x0i = a[j + 1] + a[j1 + 1];
283  x1r = a[j] - a[j1];
284  x1i = a[j + 1] - a[j1 + 1];
285  x2r = a[j2] + a[j3];
286  x2i = a[j2 + 1] + a[j3 + 1];
287  x3r = a[j2] - a[j3];
288  x3i = a[j2 + 1] - a[j3 + 1];
289  a[j] = x0r + x2r;
290  a[j + 1] = x0i + x2i;
291  a[j2] = x0r - x2r;
292  a[j2 + 1] = x0i - x2i;
293  a[j1] = x1r + x3i;
294  a[j1 + 1] = x1i - x3r;
295  a[j3] = x1r - x3i;
296  a[j3 + 1] = x1i + x3r;
297  }
298  if (m < n) {
299  wk1r = w[2];
300  for (j = m; j <= l + m - 2; j += 2) {
301  j1 = j + l;
302  j2 = j1 + l;
303  j3 = j2 + l;
304  x0r = a[j] + a[j1];
305  x0i = a[j + 1] + a[j1 + 1];
306  x1r = a[j] - a[j1];
307  x1i = a[j + 1] - a[j1 + 1];
308  x2r = a[j2] + a[j3];
309  x2i = a[j2 + 1] + a[j3 + 1];
310  x3r = a[j2] - a[j3];
311  x3i = a[j2 + 1] - a[j3 + 1];
312  a[j] = x0r + x2r;
313  a[j + 1] = x0i + x2i;
314  a[j2] = x0i - x2i;
315  a[j2 + 1] = x2r - x0r;
316  x0r = x1r + x3i;
317  x0i = x1i - x3r;
318  a[j1] = wk1r * (x0i + x0r);
319  a[j1 + 1] = wk1r * (x0i - x0r);
320  x0r = x3i - x1r;
321  x0i = x3r + x1i;
322  a[j3] = wk1r * (x0r + x0i);
323  a[j3 + 1] = wk1r * (x0r - x0i);
324  }
325  k1 = 1;
326  ks = -1;
327  for (k = (m << 1); k <= n - m; k += m) {
328  k1++;
329  ks = -ks;
330  wk1r = w[k1 << 1];
331  wk1i = w[(k1 << 1) + 1];
332  wk2r = ks * w[k1];
333  wk2i = w[k1 + ks];
334  wk3r = wk1r - 2 * wk2i * wk1i;
335  wk3i = 2 * wk2i * wk1r - wk1i;
336  for (j = k; j <= l + k - 2; j += 2) {
337  j1 = j + l;
338  j2 = j1 + l;
339  j3 = j2 + l;
340  x0r = a[j] + a[j1];
341  x0i = a[j + 1] + a[j1 + 1];
342  x1r = a[j] - a[j1];
343  x1i = a[j + 1] - a[j1 + 1];
344  x2r = a[j2] + a[j3];
345  x2i = a[j2 + 1] + a[j3 + 1];
346  x3r = a[j2] - a[j3];
347  x3i = a[j2 + 1] - a[j3 + 1];
348  a[j] = x0r + x2r;
349  a[j + 1] = x0i + x2i;
350  x0r -= x2r;
351  x0i -= x2i;
352  a[j2] = wk2r * x0r + wk2i * x0i;
353  a[j2 + 1] = wk2r * x0i - wk2i * x0r;
354  x0r = x1r + x3i;
355  x0i = x1i - x3r;
356  a[j1] = wk1r * x0r + wk1i * x0i;
357  a[j1 + 1] = wk1r * x0i - wk1i * x0r;
358  x0r = x1r - x3i;
359  x0i = x1i + x3r;
360  a[j3] = wk3r * x0r + wk3i * x0i;
361  a[j3 + 1] = wk3r * x0i - wk3i * x0r;
362  }
363  }
364  }
365  l = m;
366  }
367  if (l < n) {
368  for (j = 0; j <= l - 2; j += 2) {
369  j1 = j + l;
370  x0r = a[j] - a[j1];
371  x0i = a[j + 1] - a[j1 + 1];
372  a[j] += a[j1];
373  a[j + 1] += a[j1 + 1];
374  a[j1] = x0r;
375  a[j1 + 1] = x0i;
376  }
377  }
378  }
379 
380 
381 
382  void makewt(int nw, int *ip, FFT_TYPE *w)
383  {
384  void bitrv2(int n, int *ip, FFT_TYPE *a);
385  int nwh, j;
386  FFT_TYPE delta, x, y;
387 
388  ip[0] = nw;
389  ip[1] = 1;
390  if (nw > 2) {
391  nwh = nw >> 1;
392  delta = atan(1.0f) / nwh;
393  w[0] = 1;
394  w[1] = 0;
395  w[nwh] = cos(delta * nwh);
396  w[nwh + 1] = w[nwh];
397  for (j = 2; j <= nwh - 2; j += 2) {
398  x = cos(delta * j);
399  y = sin(delta * j);
400  w[j] = x;
401  w[j + 1] = y;
402  w[nw - j] = y;
403  w[nw - j + 1] = x;
404  }
405  bitrv2(nw, ip + 2, w);
406  }
407  }
408 
409  /**
410  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
411  You may use, copy, modify this code for any purpose and
412  without fee. You may distribute this ORIGINAL package.
413  */
414  void makect(int nc, int *ip, FFT_TYPE *c)
415  {
416  int nch, j;
417  FFT_TYPE delta;
418 
419  ip[1] = nc;
420  if (nc > 1) {
421  nch = nc >> 1;
422  delta = atan(1.0f) / nch;
423  c[0] = 0.5f;
424  c[nch] = 0.5f * cos(delta * nch);
425  for (j = 1; j <= nch - 1; j++) {
426  c[j] = 0.5f * cos(delta * j);
427  c[nc - j] = 0.5f * sin(delta * j);
428  }
429  }
430  }
431 
432  /**
433  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
434  You may use, copy, modify this code for any purpose and
435  without fee. You may distribute this ORIGINAL package.
436  */
437  void bitrv2(int n, int *ip, FFT_TYPE *a)
438  {
439  int j, j1, k, k1, l, m, m2;
440  FFT_TYPE xr, xi;
441 
442  ip[0] = 0;
443  l = n;
444  m = 1;
445  while ((m << 2) < l) {
446  l >>= 1;
447  for (j = 0; j <= m - 1; j++) {
448  ip[m + j] = ip[j] + l;
449  }
450  m <<= 1;
451  }
452  if ((m << 2) > l) {
453  for (k = 1; k <= m - 1; k++) {
454  for (j = 0; j <= k - 1; j++) {
455  j1 = (j << 1) + ip[k];
456  k1 = (k << 1) + ip[j];
457  xr = a[j1];
458  xi = a[j1 + 1];
459  a[j1] = a[k1];
460  a[j1 + 1] = a[k1 + 1];
461  a[k1] = xr;
462  a[k1 + 1] = xi;
463  }
464  }
465  } else {
466  m2 = m << 1;
467  for (k = 1; k <= m - 1; k++) {
468  for (j = 0; j <= k - 1; j++) {
469  j1 = (j << 1) + ip[k];
470  k1 = (k << 1) + ip[j];
471  xr = a[j1];
472  xi = a[j1 + 1];
473  a[j1] = a[k1];
474  a[j1 + 1] = a[k1 + 1];
475  a[k1] = xr;
476  a[k1 + 1] = xi;
477  j1 += m2;
478  k1 += m2;
479  xr = a[j1];
480  xi = a[j1 + 1];
481  a[j1] = a[k1];
482  a[j1 + 1] = a[k1 + 1];
483  a[k1] = xr;
484  a[k1 + 1] = xi;
485  }
486  }
487  }
488  }
489 
490  /**
491  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
492  You may use, copy, modify this code for any purpose and
493  without fee. You may distribute this ORIGINAL package.
494  */
495  void cdft(int n, int isgn, FFT_TYPE *a, int *ip, FFT_TYPE *w)
496  {
497  if (n > (ip[0] << 2)) {
498  makewt(n >> 2, ip, w);
499  }
500  if (n > 4) {
501  bitrv2(n, ip + 2, a);
502  }
503  if (isgn < 0) {
504  cftfsub(n, a, w);
505  } else {
506  cftbsub(n, a, w);
507  }
508  }
509 
510  void rftfsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
511  {
512  int j, k, kk, ks;
513  FFT_TYPE wkr, wki, xr, xi, yr, yi;
514 
515  ks = (nc << 2) / n;
516  kk = 0;
517  for (k = (n >> 1) - 2; k >= 2; k -= 2) {
518  j = n - k;
519  kk += ks;
520  wkr = 0.5f - c[kk];
521  wki = c[nc - kk];
522  xr = a[k] - a[j];
523  xi = a[k + 1] + a[j + 1];
524  yr = wkr * xr + wki * xi;
525  yi = wkr * xi - wki * xr;
526  a[k] -= yr;
527  a[k + 1] -= yi;
528  a[j] += yr;
529  a[j + 1] -= yi;
530  }
531  }
532 
533  void dctsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
534  {
535  int j, k, kk, ks, m;
536  FFT_TYPE wkr, wki, xr;
537 
538  ks = nc / n;
539  kk = ks;
540  m = n >> 1;
541  for (k = 1; k <= m - 1; k++) {
542  j = n - k;
543  wkr = c[kk] - c[nc - kk];
544  wki = c[kk] + c[nc - kk];
545  kk += ks;
546  xr = wki * a[k] - wkr * a[j];
547  a[k] = wkr * a[k] + wki * a[j];
548  a[j] = xr;
549  }
550  a[m] *= 2 * c[kk];
551  }
552 
553  void dstsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
554  {
555  int j, k, kk, ks, m;
556  FFT_TYPE wkr, wki, xr;
557 
558  ks = nc / n;
559  kk = ks;
560  m = n >> 1;
561  for (k = 1; k <= m - 1; k++) {
562  j = n - k;
563  wkr = c[kk] - c[nc - kk];
564  wki = c[kk] + c[nc - kk];
565  kk += ks;
566  xr = wki * a[j] - wkr * a[k];
567  a[j] = wkr * a[j] + wki * a[k];
568  a[k] = xr;
569  }
570  a[m] *= 2 * c[kk];
571  }
572 
573 
574  void rdft(int n, int isgn, FFT_TYPE *a, int *ip, FFT_TYPE *w)
575  {
576  int nw, nc;
577  FFT_TYPE xi;
578 
579  nw = ip[0];
580  if (n > (nw << 2)) {
581  nw = n >> 2;
582  makewt(nw, ip, w);
583  }
584  nc = ip[1];
585  if (n > (nc << 2)) {
586  nc = n >> 2;
587  makect(nc, ip, w + nw);
588  }
589  if (isgn < 0) {
590  a[1] = 0.5f * (a[0] - a[1]);
591  a[0] -= a[1];
592  if (n > 4) {
593  rftfsub(n, a, nc, w + nw);
594  bitrv2(n, ip + 2, a);
595  }
596  cftfsub(n, a, w);
597  } else {
598  if (n > 4) {
599  bitrv2(n, ip + 2, a);
600  }
601  cftbsub(n, a, w);
602  if (n > 4) {
603  rftbsub(n, a, nc, w + nw);
604  }
605  xi = a[0] - a[1];
606  a[0] += a[1];
607  a[1] = xi;
608  }
609  }
610 
611  void rftbsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
612  {
613  int j, k, kk, ks;
614  FFT_TYPE wkr, wki, xr, xi, yr, yi;
615 
616  ks = (nc << 2) / n;
617  kk = 0;
618  for (k = (n >> 1) - 2; k >= 2; k -= 2) {
619  j = n - k;
620  kk += ks;
621  wkr = 0.5f - c[kk];
622  wki = c[nc - kk];
623  xr = a[k] - a[j];
624  xi = a[k + 1] + a[j + 1];
625  yr = wkr * xr - wki * xi;
626  yi = wkr * xi + wki * xr;
627  a[k] -= yr;
628  a[k + 1] -= yi;
629  a[j] += yr;
630  a[j + 1] -= yi;
631  }
632  }
633 
634 
635  /**
636  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
637  You may use, copy, modify this code for any purpose and
638  without fee. You may distribute this ORIGINAL package.
639 
640  -------- Real DFT / Inverse of Real DFT --------
641  [definition]
642  <case1> RDFT
643  R[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 a[j1][j2] *
644  cos(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2),
645  0<=k1<n1, 0<=k2<n2
646  I[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 a[j1][j2] *
647  sin(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2),
648  0<=k1<n1, 0<=k2<n2
649  <case2> IRDFT (excluding scale)
650  a[k1][k2] = (1/2) * sum_j1=0^n1-1 sum_j2=0^n2-1
651  (R[j1][j2] *
652  cos(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2) +
653  I[j1][j2] *
654  sin(2*pi*j1*k1/n1 + 2*pi*j2*k2/n2)),
655  0<=k1<n1, 0<=k2<n2
656  (notes: R[n1-k1][n2-k2] = R[k1][k2],
657  I[n1-k1][n2-k2] = -I[k1][k2],
658  R[n1-k1][0] = R[k1][0],
659  I[n1-k1][0] = -I[k1][0],
660  R[0][n2-k2] = R[0][k2],
661  I[0][n2-k2] = -I[0][k2],
662  0<k1<n1, 0<k2<n2)
663  [usage]
664  <case1>
665  ip[0] = 0; // first time only
666  rdft2d(n1, n2, 1, a, t, ip, w);
667  <case2>
668  ip[0] = 0; // first time only
669  rdft2d(n1, n2, -1, a, t, ip, w);
670  [parameters]
671  n1 :data length (int)
672  n1 >= 2, n1 = power of 2
673  n2 :data length (int)
674  n2 >= 2, n2 = power of 2
675  a[0...n1-1][0...n2-1]
676  :input/output data (FFT_TYPE **)
677  <case1>
678  output data
679  a[k1][2*k2] = R[k1][k2] = R[n1-k1][n2-k2],
680  a[k1][2*k2+1] = I[k1][k2] = -I[n1-k1][n2-k2],
681  0<k1<n1, 0<k2<n2/2,
682  a[0][2*k2] = R[0][k2] = R[0][n2-k2],
683  a[0][2*k2+1] = I[0][k2] = -I[0][n2-k2],
684  0<k2<n2/2,
685  a[k1][0] = R[k1][0] = R[n1-k1][0],
686  a[k1][1] = I[k1][0] = -I[n1-k1][0],
687  a[n1-k1][1] = R[k1][n2/2] = R[n1-k1][n2/2],
688  a[n1-k1][0] = -I[k1][n2/2] = I[n1-k1][n2/2],
689  0<k1<n1/2,
690  a[0][0] = R[0][0],
691  a[0][1] = R[0][n2/2],
692  a[n1/2][0] = R[n1/2][0],
693  a[n1/2][1] = R[n1/2][n2/2]
694  <case2>
695  input data
696  a[j1][2*j2] = R[j1][j2] = R[n1-j1][n2-j2],
697  a[j1][2*j2+1] = I[j1][j2] = -I[n1-j1][n2-j2],
698  0<j1<n1, 0<j2<n2/2,
699  a[0][2*j2] = R[0][j2] = R[0][n2-j2],
700  a[0][2*j2+1] = I[0][j2] = -I[0][n2-j2],
701  0<j2<n2/2,
702  a[j1][0] = R[j1][0] = R[n1-j1][0],
703  a[j1][1] = I[j1][0] = -I[n1-j1][0],
704  a[n1-j1][1] = R[j1][n2/2] = R[n1-j1][n2/2],
705  a[n1-j1][0] = -I[j1][n2/2] = I[n1-j1][n2/2],
706  0<j1<n1/2,
707  a[0][0] = R[0][0],
708  a[0][1] = R[0][n2/2],
709  a[n1/2][0] = R[n1/2][0],
710  a[n1/2][1] = R[n1/2][n2/2]
711  t[0...2*n1-1]
712  :work area (FFT_TYPE *)
713  ip[0...*]
714  :work area for bit reversal (int *)
715  length of ip >= 2+sqrt(n) ; if n % 4 == 0
716  2+sqrt(n/2); otherwise
717  (n = max(n1, n2/2))
718  ip[0],ip[1] are pointers of the cos/sin table.
719  w[0...*]
720  :cos/sin table (FFT_TYPE *)
721  length of w >= max(n1/2, n2/4) + n2/4
722  w[],ip[] are initialized if ip[0] == 0.
723  [remark]
724  Inverse of
725  rdft2d(n1, n2, 1, a, t, ip, w);
726  is
727  rdft2d(n1, n2, -1, a, t, ip, w);
728  for (j1 = 0; j1 <= n1 - 1; j1++) {
729  for (j2 = 0; j2 <= n2 - 1; j2++) {
730  a[j1][j2] *= 2.0 / (n1 * n2);
731  }
732  }
733  */
734  void rdft2d(int n1, int n2, int isgn, FFT_TYPE **a, FFT_TYPE *t, int *ip, FFT_TYPE *w)
735  {
736  int n, nw, nc, n1h, i, j, i2;
737  FFT_TYPE xi;
738 
739  n = n1 << 1;
740  if (n < n2) {
741  n = n2;
742  }
743  nw = ip[0];
744  if (n > (nw << 2)) {
745  nw = n >> 2;
746  makewt(nw, ip, w);
747  }
748  nc = ip[1];
749  if (n2 > (nc << 2)) {
750  nc = n2 >> 2;
751  makect(nc, ip, w + nw);
752  }
753  n1h = n1 >> 1;
754  if (isgn < 0) {
755  for (i = 1; i <= n1h - 1; i++) {
756  j = n1 - i;
757  xi = a[i][0] - a[j][0];
758  a[i][0] += a[j][0];
759  a[j][0] = xi;
760  xi = a[j][1] - a[i][1];
761  a[i][1] += a[j][1];
762  a[j][1] = xi;
763  }
764  for (j = 0; j <= n2 - 2; j += 2) {
765  for (i = 0; i <= n1 - 1; i++) {
766  i2 = i << 1;
767  t[i2] = a[i][j];
768  t[i2 + 1] = a[i][j + 1];
769  }
770  cdft(n1 << 1, isgn, t, ip, w);
771  for (i = 0; i <= n1 - 1; i++) {
772  i2 = i << 1;
773  a[i][j] = t[i2];
774  a[i][j + 1] = t[i2 + 1];
775  }
776  }
777  for (i = 0; i <= n1 - 1; i++) {
778  rdft(n2, isgn, a[i], ip, w);
779  }
780  } else {
781  for (i = 0; i <= n1 - 1; i++) {
782  rdft(n2, isgn, a[i], ip, w);
783  }
784  for (j = 0; j <= n2 - 2; j += 2) {
785  for (i = 0; i <= n1 - 1; i++) {
786  i2 = i << 1;
787  t[i2] = a[i][j];
788  t[i2 + 1] = a[i][j + 1];
789  }
790  cdft(n1 << 1, isgn, t, ip, w);
791  for (i = 0; i <= n1 - 1; i++) {
792  i2 = i << 1;
793  a[i][j] = t[i2];
794  a[i][j + 1] = t[i2 + 1];
795  }
796  }
797  for (i = 1; i <= n1h - 1; i++) {
798  j = n1 - i;
799  a[j][0] = 0.5f * (a[i][0] - a[j][0]);
800  a[i][0] -= a[j][0];
801  a[j][1] = 0.5f * (a[i][1] + a[j][1]);
802  a[i][1] -= a[j][1];
803  }
804  }
805  }
806 
807  /**
808  Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
809  You may use, copy, modify this code for any purpose and
810  without fee. You may distribute this ORIGINAL package.
811 
812  -------- Complex DFT (Discrete Fourier Transform) --------
813  [definition]
814  <case1>
815  X[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 x[j1][j2] *
816  exp(2*pi*i*j1*k1/n1) *
817  exp(2*pi*i*j2*k2/n2), 0<=k1<n1, 0<=k2<n2
818  <case2>
819  X[k1][k2] = sum_j1=0^n1-1 sum_j2=0^n2-1 x[j1][j2] *
820  exp(-2*pi*i*j1*k1/n1) *
821  exp(-2*pi*i*j2*k2/n2), 0<=k1<n1, 0<=k2<n2
822  (notes: sum_j=0^n-1 is a summation from j=0 to n-1)
823  [usage]
824  <case1>
825  ip[0] = 0; // first time only
826  cdft2d(n1, 2*n2, 1, a, t, ip, w);
827  <case2>
828  ip[0] = 0; // first time only
829  cdft2d(n1, 2*n2, -1, a, t, ip, w);
830  [parameters]
831  n1 :data length (int)
832  n1 >= 1, n1 = power of 2
833  2*n2 :data length (int)
834  n2 >= 1, n2 = power of 2
835  a[0...n1-1][0...2*n2-1]
836  :input/output data (double **)
837  input data
838  a[j1][2*j2] = Re(x[j1][j2]),
839  a[j1][2*j2+1] = Im(x[j1][j2]),
840  0<=j1<n1, 0<=j2<n2
841  output data
842  a[k1][2*k2] = Re(X[k1][k2]),
843  a[k1][2*k2+1] = Im(X[k1][k2]),
844  0<=k1<n1, 0<=k2<n2
845  t[0...2*n1-1]
846  :work area (double *)
847  ip[0...*]
848  :work area for bit reversal (int *)
849  length of ip >= 2+sqrt(n) ; if n % 4 == 0
850  2+sqrt(n/2); otherwise
851  (n = max(n1, n2))
852  ip[0],ip[1] are pointers of the cos/sin table.
853  w[0...*]
854  :cos/sin table (double *)
855  length of w >= max(n1/2, n2/2)
856  w[],ip[] are initialized if ip[0] == 0.
857  [remark]
858  Inverse of
859  cdft2d(n1, 2*n2, -1, a, t, ip, w);
860  is
861  cdft2d(n1, 2*n2, 1, a, t, ip, w);
862  for (j1 = 0; j1 <= n1 - 1; j1++) {
863  for (j2 = 0; j2 <= 2 * n2 - 1; j2++) {
864  a[j1][j2] *= 1.0 / (n1 * n2);
865  }
866  }
867 
868  */
869  void cdft2d(int n1, int n2, int isgn, FFT_TYPE **a, FFT_TYPE *t,
870  int *ip, FFT_TYPE *w)
871  {
872  void makewt(int nw, int *ip, FFT_TYPE *w);
873  void cdft(int n, int isgn, FFT_TYPE *a, int *ip, FFT_TYPE *w);
874  int n, i, j, i2;
875 
876  n = n1 << 1;
877  if (n < n2) {
878  n = n2;
879  }
880  if (n > (ip[0] << 2)) {
881  makewt(n >> 2, ip, w);
882  }
883  for (i = 0; i <= n1 - 1; i++) {
884  cdft(n2, isgn, a[i], ip, w);
885  }
886  for (j = 0; j <= n2 - 2; j += 2) {
887  for (i = 0; i <= n1 - 1; i++) {
888  i2 = i << 1;
889  t[i2] = a[i][j];
890  t[i2 + 1] = a[i][j + 1];
891  }
892  cdft(n1 << 1, isgn, t, ip, w);
893  for (i = 0; i <= n1 - 1; i++) {
894  i2 = i << 1;
895  a[i][j] = t[i2];
896  a[i][j + 1] = t[i2 + 1];
897  }
898  }
899  }
900 
901 
902  } // end namespace
903 } // end namespace
904 
905 /*---------------------------------------------------------------
906  normalPDF
907  ---------------------------------------------------------------*/
908 double math::normalPDF(double x, double mu, double std)
909 {
910  return ::exp( -0.5 * square((x-mu)/std) ) / (std*2.506628274631000502415765284811) ;
911 }
912 
913 
914 /*---------------------------------------------------------------
915  erfc
916  ---------------------------------------------------------------*/
917 double math::erfc(const double x)
918 {
919 #if defined(HAVE_ERF)
920  return ::erfc(x);
921 #else
922 // copied from TMath for those platforms which do not have a
923 // C99 compliant compiler
924 
925 // Compute the complementary error function erfc(x).
926 // Erfc(x) = (2/sqrt(pi)) Integral(exp(-t^2))dt between x and infinity
927 //
928 //--- Nve 14-nov-1998 UU-SAP Utrecht
929 
930 // The parameters of the Chebyshev fit
931 const double a1 = -1.26551223, a2 = 1.00002368,
932 a3 = 0.37409196, a4 = 0.09678418,
933 a5 = -0.18628806, a6 = 0.27886807,
934 a7 = -1.13520398, a8 = 1.48851587,
935 a9 = -0.82215223, a10 = 0.17087277;
936 
937 double v = 1.0; // The return value
938 double z = ::fabs(x);
939 
940 if (z <= 0) return v; // erfc(0)=1
941 
942 double t = 1.0/(1.0+0.5*z);
943 
944 v = t* ::exp((-z*z) +a1+t*(a2+t*(a3+t*(a4+t*(a5+t*(a6+t*(a7+t*(a8+t*(a9+t*a10)))))))));
945 
946 if (x < 0) v = 2.0-v; // erfc(-x)=2-erfc(x)
947 
948 return v;
949 #endif
950 }
951 
952 /*---------------------------------------------------------------
953  erf
954  ---------------------------------------------------------------*/
955 double math::erf(double x)
956 {
957 #if defined(HAVE_ERF)
958  return ::erf(x);
959 #else
960  return (1.0 - math::erfc(x));
961 #endif
962 }
963 
964 /*---------------------------------------------------------------
965  chi2inv
966  ---------------------------------------------------------------*/
967 double math::chi2inv(double P, unsigned int dim)
968 {
969  ASSERT_(P>=0 && P<1)
970  if (P==0)
971  return 0;
972  else return dim * pow( 1.0 - 2.0/(9*dim) + sqrt(2.0/(9*dim))*normalQuantile(P), 3 );
973 }
974 
975 /*---------------------------------------------------------------
976  factorial64
977  ---------------------------------------------------------------*/
978 uint64_t math::factorial64(unsigned int n)
979 {
980  uint64_t ret = 1;
981 
982  for (unsigned int i=2;i<=n;i++)
983  ret *= i;
984 
985  return ret;
986 }
987 
988 /*---------------------------------------------------------------
989  factorial
990  ---------------------------------------------------------------*/
991 double math::factorial(unsigned int n)
992 {
993  double retLog=0;
994 
995  for (unsigned int i=2;i<=n;i++)
996  retLog += ::log( (double)n );
997 
998  return ::exp(retLog);
999 }
1000 
1001 /*---------------------------------------------------------------
1002  fft_real
1003  ---------------------------------------------------------------*/
1004 void math::fft_real( CVectorFloat &in_realData,
1005  CVectorFloat &out_FFT_Re,
1006  CVectorFloat &out_FFT_Im,
1007  CVectorFloat &out_FFT_Mag )
1008 {
1009  MRPT_START
1010 
1011  unsigned long n = (unsigned long)in_realData.size();
1012 
1013  // TODO: Test data lenght is 2^N...
1014 
1015  CVectorFloat auxVect( n+1 );
1016 
1017  memcpy( &auxVect[1], &in_realData[0], n * sizeof(auxVect[0]));
1018 
1019  realft( &auxVect[0], n );
1020 
1021  unsigned int n_2 = 1 + (n / 2);
1022 
1023  out_FFT_Re.resize(n_2);
1024  out_FFT_Im.resize(n_2);
1025  out_FFT_Mag.resize(n_2);
1026 
1027  for (unsigned int i=0;i<n_2;i++)
1028  {
1029  if (i==(n_2-1))
1030  out_FFT_Re[i] = auxVect[2];
1031  else out_FFT_Re[i] = auxVect[1+i*2];
1032 
1033  if (i==0 || i==(n_2-1))
1034  out_FFT_Im[i] = 0;
1035  else out_FFT_Im[i] = auxVect[1+i*2+1];
1036 
1037  out_FFT_Mag[i] = sqrt( square(out_FFT_Re[i])+square(out_FFT_Im[i]) );
1038  }
1039 
1040  MRPT_END
1041 }
1042 
1043 /*---------------------------------------------------------------
1044  normalQuantile
1045  ---------------------------------------------------------------*/
1046 double math::normalQuantile(double p)
1047 {
1048  double q, t, u;
1049 
1050  static const double a[6] =
1051  {
1052  -3.969683028665376e+01, 2.209460984245205e+02,
1053  -2.759285104469687e+02, 1.383577518672690e+02,
1054  -3.066479806614716e+01, 2.506628277459239e+00
1055  };
1056  static const double b[5] =
1057  {
1058  -5.447609879822406e+01, 1.615858368580409e+02,
1059  -1.556989798598866e+02, 6.680131188771972e+01,
1060  -1.328068155288572e+01
1061  };
1062  static const double c[6] =
1063  {
1064  -7.784894002430293e-03, -3.223964580411365e-01,
1065  -2.400758277161838e+00, -2.549732539343734e+00,
1066  4.374664141464968e+00, 2.938163982698783e+00
1067  };
1068  static const double d[4] =
1069  {
1070  7.784695709041462e-03, 3.224671290700398e-01,
1071  2.445134137142996e+00, 3.754408661907416e+00
1072  };
1073 
1074  ASSERT_(!isNaN(p))
1075  ASSERT_(p < 1.0 && p > 0.0)
1076 
1077  q = min(p,1-p);
1078 
1079  if (q > 0.02425)
1080  {
1081  /* Rational approximation for central region. */
1082  u = q-0.5;
1083  t = u*u;
1084  u = u*(((((a[0]*t+a[1])*t+a[2])*t+a[3])*t+a[4])*t+a[5])
1085  /(((((b[0]*t+b[1])*t+b[2])*t+b[3])*t+b[4])*t+1);
1086  }
1087  else
1088  {
1089  /* Rational approximation for tail region. */
1090  t = sqrt(-2*::log(q));
1091  u = (((((c[0]*t+c[1])*t+c[2])*t+c[3])*t+c[4])*t+c[5])
1092  /((((d[0]*t+d[1])*t+d[2])*t+d[3])*t+1);
1093  }
1094 
1095  /* The relative error of the approximation has absolute value less
1096  than 1.15e-9. One iteration of Halley's rational method (third
1097  order) gives full machine precision... */
1098  t = normalCDF(u)-q; /* error */
1099  t = t* 2.506628274631000502415765284811 * ::exp(u*u/2); /* f(u)/df(u) */
1100  u = u-t/(1+u*t/2); /* Halley's method */
1101 
1102  return (p > 0.5 ? -u : u);
1103 }
1104 
1105 /*---------------------------------------------------------------
1106  normalCDF
1107  ---------------------------------------------------------------*/
1108 double math::normalCDF(double u)
1109 {
1110  static const double a[5] =
1111  {
1112  1.161110663653770e-002,3.951404679838207e-001,2.846603853776254e+001,
1113  1.887426188426510e+002,3.209377589138469e+003
1114  };
1115  static const double b[5] =
1116  {
1117  1.767766952966369e-001,8.344316438579620e+000,1.725514762600375e+002,
1118  1.813893686502485e+003,8.044716608901563e+003
1119  };
1120  static const double c[9] =
1121  {
1122  2.15311535474403846e-8,5.64188496988670089e-1,8.88314979438837594e00,
1123  6.61191906371416295e01,2.98635138197400131e02,8.81952221241769090e02,
1124  1.71204761263407058e03,2.05107837782607147e03,1.23033935479799725E03
1125  };
1126  static const double d[9] =
1127  {
1128  1.00000000000000000e00,1.57449261107098347e01,1.17693950891312499e02,
1129  5.37181101862009858e02,1.62138957456669019e03,3.29079923573345963e03,
1130  4.36261909014324716e03,3.43936767414372164e03,1.23033935480374942e03
1131  };
1132  static const double p[6] =
1133  {
1134  1.63153871373020978e-2,3.05326634961232344e-1,3.60344899949804439e-1,
1135  1.25781726111229246e-1,1.60837851487422766e-2,6.58749161529837803e-4
1136  };
1137  static const double q[6] =
1138  {
1139  1.00000000000000000e00,2.56852019228982242e00,1.87295284992346047e00,
1140  5.27905102951428412e-1,6.05183413124413191e-2,2.33520497626869185e-3
1141  };
1142  double y, z;
1143 
1144  ASSERT_( !isNaN(u) );
1145  ASSERT_( isFinite(u) );
1146 
1147  y = fabs(u);
1148  if (y <= 0.46875* 1.4142135623730950488016887242097 )
1149  {
1150  /* evaluate erf() for |u| <= sqrt(2)*0.46875 */
1151  z = y*y;
1152  y = u*((((a[0]*z+a[1])*z+a[2])*z+a[3])*z+a[4])
1153  /((((b[0]*z+b[1])*z+b[2])*z+b[3])*z+b[4]);
1154  return 0.5+y;
1155  }
1156 
1157  z = ::exp(-y*y/2)/2;
1158  if (y <= 4.0)
1159  {
1160  /* evaluate erfc() for sqrt(2)*0.46875 <= |u| <= sqrt(2)*4.0 */
1161  y = y/ 1.4142135623730950488016887242097 ;
1162  y =
1163  ((((((((c[0]*y+c[1])*y+c[2])*y+c[3])*y+c[4])*y+c[5])*y+c[6])*y+c[7])*y+c[8])
1164 
1165 
1166  /((((((((d[0]*y+d[1])*y+d[2])*y+d[3])*y+d[4])*y+d[5])*y+d[6])*y+d[7])*y+d[8]);
1167 
1168  y = z*y;
1169  }
1170  else
1171  {
1172  /* evaluate erfc() for |u| > sqrt(2)*4.0 */
1173  z = z* 1.4142135623730950488016887242097 /y;
1174  y = 2/(y*y);
1175  y = y*(((((p[0]*y+p[1])*y+p[2])*y+p[3])*y+p[4])*y+p[5])
1176  /(((((q[0]*y+q[1])*y+q[2])*y+q[3])*y+q[4])*y+q[5]);
1177  y = z*( 0.564189583547756286948 - y);
1178  }
1179  return (u < 0.0 ? y : 1-y);
1180 }
1181 
1182 
1183 /*---------------------------------------------------------------
1184  dft2_real
1185  ---------------------------------------------------------------*/
1186 void math::dft2_real(
1187  const CMatrixFloat &in_data,
1188  CMatrixFloat &out_real,
1189  CMatrixFloat &out_imag )
1190 {
1191  MRPT_START
1192 
1193  size_t i,j;
1194  typedef FFT_TYPE* float_ptr ;
1195 
1196  // The dimensions:
1197  size_t dim1 = in_data.getRowCount();
1198  size_t dim2 = in_data.getColCount();
1199 
1200  // Transform to format compatible with C routines:
1201  // ------------------------------------------------------------
1202  FFT_TYPE **a;
1203  FFT_TYPE *t;
1204  int *ip;
1205  FFT_TYPE *w;
1206 
1207  // Reserve memory and copy data:
1208  // --------------------------------------
1209  a = new float_ptr[dim1];
1210  for (i=0;i<dim1;i++)
1211  {
1212  a[i] = new FFT_TYPE[dim2];
1213  for (j=0;j<dim2;j++)
1214  a[i][j] = in_data.get_unsafe(i,j);
1215  }
1216 
1217  t = new FFT_TYPE[2*dim1+20];
1218  ip = new int[(int)ceil(20+2+sqrt((FFT_TYPE)max(dim1,dim2/2)))];
1219  ip[0] = 0;
1220  w = new FFT_TYPE[max(dim1/2,dim2/4)+dim2/4+20];
1221 
1222  // Do the job!
1223  // --------------------------------------
1224  rdft2d((int)dim1,(int)dim2,1,a,t,ip,w);
1225 
1226  // Transform back to MRPT matrix format:
1227  // --------------------------------------
1228  out_real.setSize(dim1,dim2);
1229  out_imag.setSize(dim1,dim2);
1230 
1231 
1232  // a[k1][2*k2] = R[k1][k2] = R[n1-k1][n2-k2],
1233  // a[k1][2*k2+1] = I[k1][k2] = -I[n1-k1][n2-k2],
1234  // 0<k1<n1, 0<k2<n2/2,
1235  for (i=1;i<dim1;i++)
1236  for (j=1;j<dim2/2;j++)
1237  {
1238  out_real.set_unsafe(i,j,(float)a[i][j*2]); out_real.set_unsafe(dim1-i,dim2-j,(float)a[i][j*2]);
1239  out_imag.set_unsafe(i,j,(float)-a[i][j*2+1]); out_imag.set_unsafe(dim1-i,dim2-j,(float)a[i][j*2+1]);
1240  }
1241  // a[0][2*k2] = R[0][k2] = R[0][n2-k2],
1242  // a[0][2*k2+1] = I[0][k2] = -I[0][n2-k2],
1243  // 0<k2<n2/2,
1244  for (j=1;j<dim2/2;j++)
1245  {
1246  out_real.set_unsafe(0,j,(float)a[0][j*2]); out_real.set_unsafe(0,dim2-j,(float)a[0][j*2]);
1247  out_imag.set_unsafe(0,j,(float)-a[0][j*2+1]); out_imag.set_unsafe(0,dim2-j,(float)a[0][j*2+1]);
1248  }
1249 
1250  // a[k1][0] = R[k1][0] = R[n1-k1][0],
1251  // a[k1][1] = I[k1][0] = -I[n1-k1][0],
1252  // a[n1-k1][1] = R[k1][n2/2] = R[n1-k1][n2/2],
1253  // a[n1-k1][0] = -I[k1][n2/2] = I[n1-k1][n2/2],
1254  // 0<k1<n1/2,
1255  for (i=1;i<dim1/2;i++)
1256  {
1257  out_real.set_unsafe(i,0,(float)a[i][0]); out_real.set_unsafe(dim1-i,0,(float)a[i][0]);
1258  out_imag.set_unsafe(i,0,(float)-a[i][1]); out_imag.set_unsafe(dim1-i,0,(float)a[i][1]);
1259  out_real.set_unsafe(i,dim2/2,(float)a[dim1-i][1]); out_real.set_unsafe(dim1-i,dim2/2,(float)a[dim1-i][1]);
1260  out_imag.set_unsafe(i,dim2/2,(float)a[dim1-i][0]); out_imag.set_unsafe(dim1-i,dim2/2,(float)-a[dim1-i][0]);
1261  }
1262 
1263  // a[0][0] = R[0][0],
1264  // a[0][1] = R[0][n2/2],
1265  // a[n1/2][0] = R[n1/2][0],
1266  // a[n1/2][1] = R[n1/2][n2/2]
1267  out_real.set_unsafe(0,0,(float)a[0][0]);
1268  out_real.set_unsafe(0,dim2/2,(float)a[0][1]);
1269  out_real.set_unsafe(dim1/2,0,(float)a[dim1/2][0]);
1270  out_real.set_unsafe(dim1/2,dim2/2,(float)a[dim1/2][1]);
1271 
1272 
1273  // Free temporary memory:
1274  for (i=0;i<dim1;i++) delete[] a[i];
1275  delete[] a;
1276  delete[] t;
1277  delete[] ip;
1278  delete[] w;
1279 
1280  MRPT_END
1281 }
1282 
1283 /*---------------------------------------------------------------
1284  idft2_real
1285  ---------------------------------------------------------------*/
1286 void math::idft2_real(
1287  const CMatrixFloat &in_real,
1288  const CMatrixFloat &in_imag,
1289  CMatrixFloat &out_data )
1290 {
1291  MRPT_START
1292 
1293  size_t i,j;
1294  typedef FFT_TYPE* float_ptr ;
1295 
1296  ASSERT_(in_real.getColCount()==in_imag.getColCount());
1297  ASSERT_(in_real.getRowCount()==in_imag.getRowCount());
1298 
1299  // The dimensions:
1300  size_t dim1 = in_real.getRowCount();
1301  size_t dim2 = in_real.getColCount();
1302 
1303  if (math::round2up(dim1)!=dim1 || math::round2up(dim2)!=dim2)
1304  THROW_EXCEPTION("Matrix sizes are not a power of two!")
1305 
1306  // Transform to format compatible with C routines:
1307  // ------------------------------------------------------------
1308  FFT_TYPE **a;
1309  FFT_TYPE *t;
1310  int *ip;
1311  FFT_TYPE *w;
1312 
1313  // Reserve memory and copy data:
1314  // --------------------------------------
1315  a = new float_ptr[dim1];
1316  for (i=0;i<dim1;i++) a[i] = new FFT_TYPE[dim2];
1317 
1318  //a[j1][2*j2] = R[j1][j2] = R[n1-j1][n2-j2],
1319  //a[j1][2*j2+1] = I[j1][j2] = -I[n1-j1][n2-j2],
1320  // 0<j1<n1, 0<j2<n2/2,
1321  for (i=1;i<dim1;i++)
1322  for (j=1;j<dim2/2;j++)
1323  {
1324  a[i][2*j ] = in_real.get_unsafe(i,j);
1325  a[i][2*j+1] = -in_imag.get_unsafe(i,j);
1326  }
1327 
1328  //a[0][2*j2] = R[0][j2] = R[0][n2-j2],
1329  //a[0][2*j2+1] = I[0][j2] = -I[0][n2-j2],
1330  // 0<j2<n2/2,
1331  for (j=1;j<dim2/2;j++)
1332  {
1333  a[0][2*j ] = in_real.get_unsafe(0,j);
1334  a[0][2*j+1] = -in_imag.get_unsafe(0,j);
1335  }
1336 
1337  //a[j1][0] = R[j1][0] = R[n1-j1][0],
1338  //a[j1][1] = I[j1][0] = -I[n1-j1][0],
1339  //a[n1-j1][1] = R[j1][n2/2] = R[n1-j1][n2/2],
1340  //a[n1-j1][0] = -I[j1][n2/2] = I[n1-j1][n2/2],
1341  // 0<j1<n1/2,
1342  for (i=1;i<dim1/2;i++)
1343  {
1344  a[i][0] = in_real.get_unsafe(i,0);
1345  a[i][1] = -in_imag.get_unsafe(i,0);
1346  a[dim1-i][1] = in_real.get_unsafe(i,dim2/2);
1347  a[dim1-i][0] = in_imag.get_unsafe(i,dim2/2);
1348  }
1349 
1350  //a[0][0] = R[0][0],
1351  //a[0][1] = R[0][n2/2],
1352  //a[n1/2][0] = R[n1/2][0],
1353  //a[n1/2][1] = R[n1/2][n2/2]
1354  a[0][0] = in_real.get_unsafe(0,0);
1355  a[0][1] = in_real.get_unsafe(0,dim2/2);
1356  a[dim1/2][0] = in_real.get_unsafe(dim1/2,0);
1357  a[dim1/2][1] = in_real.get_unsafe(dim1/2,dim2/2);
1358 
1359  t = new FFT_TYPE[2*dim1+20];
1360  ip = new int[(int)ceil(20+2+sqrt((FFT_TYPE)max(dim1,dim2/2)))];
1361  ip[0] = 0;
1362  w = new FFT_TYPE[max(dim1/2,dim2/4)+dim2/4+20];
1363 
1364  // Do the job!
1365  // --------------------------------------
1366  rdft2d((int)dim1,(int)dim2,-1,a,t,ip,w);
1367 
1368  // Transform back to MRPT matrix format:
1369  // --------------------------------------
1370  out_data.setSize(dim1,dim2);
1371 
1372  FFT_TYPE scale = 2.0f / (dim1*dim2);
1373 
1374  for (i=0;i<dim1;i++)
1375  for (j=0;j<dim2;j++)
1376  out_data.set_unsafe(i,j,(float)(a[i][j]*scale));
1377 
1378  // Free temporary memory:
1379  for (i=0;i<dim1;i++) delete[] a[i];
1380  delete[] a;
1381  delete[] t;
1382  delete[] ip;
1383  delete[] w;
1384 
1385  MRPT_END
1386 }
1387 
1388 
1389 /*---------------------------------------------------------------
1390  myGeneralDFT
1391 
1392  sign -> -1: DFT, 1:IDFT
1393  ---------------------------------------------------------------*/
1394 void myGeneralDFT(
1395  int sign,
1396  const CMatrixFloat &in_real,
1397  const CMatrixFloat &in_imag,
1398  CMatrixFloat &out_real,
1399  CMatrixFloat &out_imag )
1400 {
1401  ASSERT_(in_real.getRowCount() == in_imag.getRowCount() )
1402  ASSERT_(in_real.getColCount() == in_imag.getColCount() )
1403 
1404  // The dimensions:
1405  size_t dim1 = in_real.getRowCount();
1406  size_t dim2 = in_real.getColCount();
1407 
1408  size_t k1,k2,n1,n2;
1409  float w_r,w_i;
1410  float ang1 = (float)( sign*M_2PI/dim1 );
1411  float ang2 = (float)( sign*M_2PI/dim2 );
1412  float phase;
1413  float R,I;
1414  float scale = sign==1 ? (1.0f/(dim1*dim2)):1;
1415 
1416  out_real.setSize(dim1,dim2);
1417  out_imag.setSize(dim1,dim2);
1418 
1419  for (k1=0;k1<dim1;k1++)
1420  {
1421  for (k2=0;k2<dim2;k2++)
1422  {
1423  R=I=0; // Accum:
1424 
1425  for (n1=0;n1<dim1;n1++)
1426  {
1427  phase = ang1*n1*k1;
1428  for (n2=0;n2<dim2;n2++)
1429  {
1430  w_r = cos(phase);
1431  w_i = sin(phase);
1432 
1433  R+= w_r*in_real.get_unsafe(n1,n2) - w_i*in_imag.get_unsafe(n1,n2);
1434  I+= w_i*in_real.get_unsafe(n1,n2) + w_r*in_imag.get_unsafe(n1,n2);
1435 
1436  phase+= ang2*k2;
1437  } // end for k2
1438  } // end for k1
1439 
1440  // Save result:
1441  out_real.set_unsafe(k1,k2,R*scale);
1442  out_imag.set_unsafe(k1,k2,I*scale);
1443 
1444  } // end for k2
1445  } // end for k1
1446 }
1447 
1448 /*---------------------------------------------------------------
1449  dft2_complex
1450  ---------------------------------------------------------------*/
1451 void math::dft2_complex(
1452  const CMatrixFloat &in_real,
1453  const CMatrixFloat &in_imag,
1454  CMatrixFloat &out_real,
1455  CMatrixFloat &out_imag)
1456 {
1457  MRPT_START
1458 
1459  ASSERT_(in_real.getRowCount() == in_imag.getRowCount() )
1460  ASSERT_(in_real.getColCount() == in_imag.getColCount() )
1461 
1462  // The dimensions:
1463  size_t dim1 = in_real.getRowCount();
1464  size_t dim2 = in_real.getColCount();
1465  size_t i,j;
1466 
1467  bool dim1IsPower2 = (math::round2up(dim1) == dim1);
1468  bool dim2IsPower2 = (math::round2up(dim2) == dim2);
1469 
1470  // FFT or DFT??
1471  if (dim1IsPower2 && dim2IsPower2)
1472  {
1473  // ----------------------------------------
1474  // Optimized FFT:
1475  // ----------------------------------------
1476  typedef FFT_TYPE* float_ptr ;
1477 
1478  // Transform to format compatible with C routines:
1479  // ------------------------------------------------------------
1480  static FFT_TYPE **a=NULL;
1481  static FFT_TYPE *t= NULL;
1482  static int *ip=NULL;
1483  static FFT_TYPE *w=NULL;
1484 
1485  // Reserve memory
1486  // --------------------------------------
1487  static int alreadyInitSize1 = -1, alreadyInitSize2 = -1;
1488 
1489  if (alreadyInitSize1!=(int)dim1 || alreadyInitSize2!=(int)dim2)
1490  {
1491  // Create/realloc buffers:
1492  if (a)
1493  {
1494  for (i=0;i<dim1;i++) delete[] a[i];
1495  delete[] a;
1496  }
1497  if (ip) delete[] ip;
1498  if (t) delete[] t;
1499  if (w) delete[] w;
1500 
1501  alreadyInitSize1 = (int)dim1;
1502  alreadyInitSize2 = (int)dim2;
1503 
1504  a = new float_ptr[dim1];
1505  for (i=0;i<dim1;i++) a[i] = new FFT_TYPE[2*dim2];
1506 
1507  t = new FFT_TYPE[2*dim1+20];
1508  ip = new int[(int)ceil(20+2+sqrt((FFT_TYPE)max(dim1,dim2/2)))];
1509  ip[0] = 0;
1510  w = new FFT_TYPE[max(dim1/2,dim2/4)+dim2/4+20];
1511  }
1512 
1513  // and copy data:
1514  // --------------------------------------
1515  for (i=0;i<dim1;i++)
1516  for (j=0;j<dim2;j++)
1517  {
1518  a[i][2*j+0] = in_real.get_unsafe(i,j);
1519  a[i][2*j+1] = in_imag.get_unsafe(i,j);
1520  }
1521 
1522 
1523  // Do the job!
1524  // --------------------------------------
1525  cdft2d((int)dim1,(int)(2*dim2),1,a,t,ip,w);
1526 
1527  // Transform back to MRPT matrix format:
1528  // --------------------------------------
1529  out_real.setSize(dim1,dim2);
1530  out_imag.setSize(dim1,dim2);
1531 
1532  // a[k1][2*k2] = Re(X[k1][k2]),
1533  // a[k1][2*k2+1] = Im(X[k1][k2]),
1534  // 0<=k1<n1, 0<=k2<n2
1535  for (i=0;i<dim1;i++)
1536  for (j=0;j<dim2;j++)
1537  {
1538  out_real.set_unsafe(i,j,(float)a[i][j*2+0]);
1539  out_imag.set_unsafe(i,j,(float)a[i][j*2+1]);
1540  }
1541 
1542  } // end FFT
1543  else
1544  {
1545  // ----------------------------------------
1546  // General DFT:
1547  // ----------------------------------------
1548  printf("Using general DFT...\n");
1549  myGeneralDFT(-1,in_real, in_imag, out_real,out_imag );
1550 
1551  }
1552 
1553 
1554  MRPT_END
1555 }
1556 
1557 /*---------------------------------------------------------------
1558  idft2_complex
1559  ---------------------------------------------------------------*/
1560 void math::idft2_complex(
1561  const CMatrixFloat &in_real,
1562  const CMatrixFloat &in_imag,
1563  CMatrixFloat &out_real,
1564  CMatrixFloat &out_imag )
1565 {
1566  MRPT_START
1567 
1568  ASSERT_(in_real.getRowCount() == in_imag.getRowCount() )
1569  ASSERT_(in_real.getColCount() == in_imag.getColCount() )
1570 
1571  // The dimensions:
1572  size_t dim1 = in_real.getRowCount();
1573  size_t dim2 = in_real.getColCount();
1574  size_t i,j;
1575 
1576  bool dim1IsPower2 = (math::round2up(dim1) == dim1);
1577  bool dim2IsPower2 = (math::round2up(dim2) == dim2);
1578 
1579  // FFT or DFT??
1580  if (dim1IsPower2 && dim2IsPower2)
1581  {
1582  typedef FFT_TYPE* float_ptr ;
1583  // ----------------------------------------
1584  // Optimized FFT:
1585  // ----------------------------------------
1586 
1587  // Transform to format compatible with C routines:
1588  // ------------------------------------------------------------
1589  static FFT_TYPE **a=NULL;
1590  static FFT_TYPE *t=NULL;
1591  static int *ip=NULL;
1592  static FFT_TYPE *w=NULL;
1593 
1594  // Reserve memory
1595  // --------------------------------------
1596 
1597  // and copy data:
1598  // --------------------------------------
1599  static int alreadyInitSize1 = -1, alreadyInitSize2 = -1;
1600 
1601  if (alreadyInitSize1!=(int)dim1 || alreadyInitSize2!=(int)dim2)
1602  {
1603  // Create/realloc buffers:
1604  if (a)
1605  {
1606  for (i=0;i<dim1;i++) delete[] a[i];
1607  delete[] a;
1608  }
1609  if (ip) delete[] ip;
1610  if (t) delete[] t;
1611  if (w) delete[] w;
1612 
1613  alreadyInitSize1 = (int)dim1;
1614  alreadyInitSize2 = (int)dim2;
1615 
1616  a = new float_ptr[dim1];
1617  for (i=0;i<dim1;i++) a[i] = new FFT_TYPE[2*dim2];
1618  t = new FFT_TYPE[2*dim1+20];
1619  ip = new int[(int)ceil(20+2+sqrt((FFT_TYPE)max(dim1,dim2/2)))];
1620  ip[0] = 0;
1621  w = new FFT_TYPE[max(dim1/2,dim2/4)+dim2/4+20];
1622  }
1623 
1624  // and copy data:
1625  // --------------------------------------
1626  for (i=0;i<dim1;i++)
1627  for (j=0;j<dim2;j++)
1628  {
1629  a[i][2*j+0] = in_real.get_unsafe(i,j);
1630  a[i][2*j+1] = in_imag.get_unsafe(i,j);
1631  }
1632 
1633  // Do the job!
1634  // --------------------------------------
1635  cdft2d((int)dim1,(int)(2*dim2),-1,a,t,ip,w);
1636 
1637  // Transform back to MRPT matrix format:
1638  // --------------------------------------
1639  out_real.setSize(dim1,dim2);
1640  out_imag.setSize(dim1,dim2);
1641 
1642  FFT_TYPE scale = 1.0f/(dim1*dim2);
1643 
1644  // a[k1][2*k2] = Re(X[k1][k2]),
1645  // a[k1][2*k2+1] = Im(X[k1][k2]),
1646  // 0<=k1<n1, 0<=k2<n2
1647  for (i=0;i<dim1;i++)
1648  for (j=0;j<dim2;j++)
1649  {
1650 // out_real.set_unsafe(i,j,(float)(a[i][j*2+0]*scale));
1651 // out_imag.set_unsafe(i,j,(float)(a[i][j*2+1]*scale));
1652  out_real.set_unsafe(i,j,(float)(a[i][j*2+0]));
1653  out_imag.set_unsafe(i,j,(float)(a[i][j*2+1]));
1654  }
1655 
1656  out_real *= scale;
1657  out_imag *= scale;
1658 
1659  // The element (0,0) is purely real!
1660  out_imag.set_unsafe(0,0,0);
1661 
1662  } // end FFT
1663  else
1664  {
1665  // ----------------------------------------
1666  // General DFT:
1667  // ----------------------------------------
1668  printf("Using general DFT...\n");
1669  myGeneralDFT(1,in_real, in_imag, out_real,out_imag );
1670  }
1671 
1672  MRPT_END
1673 }
1674 
1675 
1676 /*---------------------------------------------------------------
1677  isNan
1678  ---------------------------------------------------------------*/
1679 bool mrpt::math::isNaN(float v) MRPT_NO_THROWS
1680 {
1681 #if defined(__BORLANDC__) || defined(_MSC_VER)
1682  return 0!=_isnan((double)v);
1683 #elif defined(__GNUC__)
1684  return isnan(v);
1685 #else
1686  return false;
1687 #endif
1688 }
1689 
1690 /** Returns true if value is Not-a-number (NAN)
1691  */
1692 bool mrpt::math::isNaN(double v) MRPT_NO_THROWS
1693 {
1694 #if defined(__BORLANDC__) || defined(_MSC_VER)
1695  return 0!=_isnan(v);
1696 #elif defined(__GNUC__)
1697  return std::isnan(v);
1698 #else
1699  return false;
1700 #endif
1701 }
1702 
1703 /** Returns true if value is finite
1704  */
1705 bool mrpt::math::isFinite(float v) MRPT_NO_THROWS
1706 {
1707 #if defined(__BORLANDC__) || defined(_MSC_VER)
1708  return 0!=_finite(v);
1709 #elif defined(__GNUC__)
1710  return std::isfinite(v);
1711 #else
1712  return false;
1713 #endif
1714 }
1715 
1716 
1717 /** Returns true if value is finite
1718  */
1719 bool mrpt::math::isFinite(double v) MRPT_NO_THROWS
1720 {
1721 #if defined(__BORLANDC__) || defined(_MSC_VER)
1722  return 0!=_finite(v);
1723 #elif defined(__GNUC__)
1724  return isfinite(v);
1725 #else
1726  return false;
1727 #endif
1728 }
1729 
1730 
1731 #ifdef HAVE_LONG_DOUBLE
1732  /*---------------------------------------------------------------
1733  isnan
1734  ---------------------------------------------------------------*/
1735  bool mrpt::math::isNaN(long double f) MRPT_NO_THROWS
1736  {
1737 #if MRPT_CHECK_VISUALC_VERSION(14) || defined(__GNUC__)
1738  return std::isnan(f);
1739 #else
1740  return 0!=_isnan(f);
1741 #endif
1742  }
1743 
1744  /*---------------------------------------------------------------
1745  isFinite
1746  ---------------------------------------------------------------*/
1747  bool mrpt::math::isFinite(long double f) MRPT_NO_THROWS
1748  {
1749 #if MRPT_CHECK_VISUALC_VERSION(14) || defined(__GNUC__)
1750  return std::isfinite(f);
1751 #else
1752  return 0!=_finite(f);
1753 #endif
1754  }
1755 #endif
1756 
1757 
1758 // Loads a vector from a text file:
1759 bool math::loadVector( CFileStream &f, ::std::vector<int> &d)
1760 {
1761  MRPT_START
1762 
1763  std::string str;
1764  if (!f.readLine(str)) return false;
1765 
1766  const char *s = str.c_str();
1767 
1768  char *nextTok, *context;
1769  const char *delim=" \t";
1770 
1771  d.clear();
1772  nextTok = mrpt::system::strtok( (char*)s,delim,&context);
1773  while (nextTok != NULL)
1774  {
1775  d.push_back( atoi(nextTok) );
1776  nextTok = mrpt::system::strtok (NULL,delim,&context);
1777  };
1778 
1779  return true;
1780  MRPT_END
1781 }
1782 
1783 bool math::loadVector( CFileStream &f, ::std::vector<double> &d)
1784 {
1785  MRPT_START
1786 
1787  std::string str;
1788  if (!f.readLine(str)) return false;
1789 
1790  const char *s = str.c_str();
1791 
1792  char *nextTok, *context;
1793  const char *delim=" \t";
1794 
1795  d.clear();
1796  nextTok = mrpt::system::strtok( (char*)s,delim,&context);
1797  while (nextTok != NULL)
1798  {
1799  d.push_back( atof(nextTok) );
1800  nextTok = mrpt::system::strtok (NULL,delim,&context);
1801  };
1802 
1803  return true;
1804  MRPT_END
1805 }
1806 
1807 // See declaration for the documentation
1808 double math::averageLogLikelihood(
1809  const CVectorDouble &logWeights,
1810  const CVectorDouble &logLikelihoods )
1811 {
1812  MRPT_START
1813 
1814  // Explained in:
1815  // http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability
1816  ASSERT_( logWeights.size()==logLikelihoods.size() );
1817 
1818  if ( !logWeights.size() )
1819  THROW_EXCEPTION("ERROR: logWeights vector is empty!")
1820 
1821  CVectorDouble::const_iterator itLW,itLL;
1822  double lw_max = math::maximum(logWeights);
1823  double ll_max = math::maximum(logLikelihoods);
1824  double SUM1=0, SUM2=0, tmpVal;
1825 
1826  for (itLW=logWeights.begin(),itLL=logLikelihoods.begin();itLW!=logWeights.end();itLW++,itLL++)
1827  {
1828  tmpVal = *itLW - lw_max;
1829  SUM1+=std::exp( tmpVal );
1830  SUM2+=std::exp( tmpVal + *itLL - ll_max );
1831  }
1832 
1833  double res = -std::log(SUM1) + std::log(SUM2) + ll_max;
1834  MRPT_CHECK_NORMAL_NUMBER(res);
1835  return res;
1836  MRPT_END
1837 }
1838 
1839 // Unweighted version:
1840 double math::averageLogLikelihood( const CVectorDouble &logLikelihoods )
1841 {
1842  MRPT_START
1843 
1844  // Explained in:
1845  // http://www.mrpt.org/Averaging_Log-Likelihood_Values:Numerical_Stability
1846  size_t N = logLikelihoods.size();
1847  if ( !N )
1848  THROW_EXCEPTION("ERROR: logLikelihoods vector is empty!")
1849 
1850  double ll_max = math::maximum(logLikelihoods);
1851  double SUM1=0;
1852 
1853  for (size_t i=0;i<N;i++)
1854  SUM1+=exp( logLikelihoods[i] - ll_max );
1855 
1856  double res = log(SUM1) - log (static_cast<double>(N)) + ll_max;
1857 
1858  MRPT_CHECK_NORMAL_NUMBER(res);
1859  return res;
1860  MRPT_END
1861 }
1862 
1863 // Wrapped angles average:
1864 double math::averageWrap2Pi(const CVectorDouble &angles )
1865 {
1866  if (angles.empty()) return 0;
1867 
1868  int W_phi_R=0,W_phi_L=0;
1869  double phi_R=0,phi_L=0;
1870 
1871  // First: XY
1872  // -----------------------------------
1873  for (CVectorDouble::Index i=0;i<angles.size();i++)
1874  {
1875  double phi = angles[i];
1876  if (abs(phi)>1.5707963267948966192313216916398)
1877  {
1878  // LEFT HALF: 0,2pi
1879  if (phi<0) phi = (M_2PI + phi);
1880 
1881  phi_L += phi;
1882  W_phi_L ++;
1883  }
1884  else
1885  {
1886  // RIGHT HALF: -pi,pi
1887  phi_R += phi;
1888  W_phi_R ++;
1889  }
1890  }
1891 
1892  // Next: PHI
1893  // -----------------------------------
1894  // The mean value from each side:
1895  if (W_phi_L) phi_L /= static_cast<double>(W_phi_L); // [0,2pi]
1896  if (W_phi_R) phi_R /= static_cast<double>(W_phi_R); // [-pi,pi]
1897 
1898  // Left side to [-pi,pi] again:
1899  if (phi_L>M_PI) phi_L -= M_2PI;
1900 
1901  // The total mean:
1902  return ((phi_L * W_phi_L + phi_R * W_phi_R )/(W_phi_L+W_phi_R));
1903 }
1904 
1905 string math::MATLAB_plotCovariance2D(
1906  const CMatrixFloat &cov,
1907  const CVectorFloat &mean,
1908  const float &stdCount,
1909  const string &style,
1910  const size_t &nEllipsePoints )
1911 {
1912  MRPT_START
1913  CMatrixD cov2(cov);
1914  CVectorDouble mean2(2);
1915  mean2[0]=mean[0];
1916  mean2[1]=mean[1];
1917 
1918  return MATLAB_plotCovariance2D( cov2,mean2,stdCount, style,nEllipsePoints );
1919 
1920  MRPT_END
1921 }
1922 
1923 string math::MATLAB_plotCovariance2D(
1924  const CMatrixDouble &cov,
1925  const CVectorDouble &mean,
1926  const float &stdCount,
1927  const string &style,
1928  const size_t &nEllipsePoints )
1929 {
1930  MRPT_START
1931 
1932  ASSERT_(cov.getColCount()==cov.getRowCount() && cov.getColCount()==2 );
1933  ASSERT_(cov(0,1)==cov(1,0) );
1934  ASSERT_(!((cov(0,0)==0) ^ (cov(1,1)==0)) ); // Both or none 0
1935  ASSERT_(mean.size()==2);
1936 
1937  std::vector<float> X,Y,COS,SIN;
1938  std::vector<float>::iterator x,y,Cos,Sin;
1939  double ang;
1940  CMatrixD eigVal,eigVec,M;
1941  string str;
1942 
1943  X.resize(nEllipsePoints);
1944  Y.resize(nEllipsePoints);
1945  COS.resize(nEllipsePoints);
1946  SIN.resize(nEllipsePoints);
1947 
1948  // Fill the angles:
1949  for (Cos=COS.begin(),Sin=SIN.begin(),ang=0;Cos!=COS.end();++Cos,++Sin, ang+= (M_2PI/(nEllipsePoints-1)) )
1950  {
1951  *Cos = (float)cos(ang);
1952  *Sin = (float)sin(ang);
1953  }
1954 
1955  cov.eigenVectors(eigVec,eigVal);
1956  eigVal = eigVal.array().sqrt().matrix();
1957  M = eigVal * eigVec.adjoint();
1958 
1959  // Compute the points of the ellipsoid:
1960  // ----------------------------------------------
1961  for (x=X.begin(), y=Y.begin(), Cos=COS.begin(),Sin=SIN.begin(); x!=X.end(); ++x,++y,++Cos,++Sin)
1962  {
1963  *x = (float)( mean[0] + stdCount * (*Cos * M(0,0) + *Sin * M(1,0)) );
1964  *y = (float)( mean[1] + stdCount * (*Cos * M(0,1) + *Sin * M(1,1)) );
1965  }
1966 
1967  // Save the code to plot the ellipsoid:
1968  // ----------------------------------------------
1969  str += string("plot([ ");
1970  for (x=X.begin();x!=X.end();++x)
1971  {
1972  str += format("%.4f",*x);
1973  if (x!=(X.end()-1)) str += format(",");
1974  }
1975  str += string("],[ ");
1976  for (y=Y.begin();y!=Y.end();++y)
1977  {
1978  str += format("%.4f",*y);
1979  if (y!=(Y.end()-1)) str += format(",");
1980  }
1981 
1982  str += format("],'%s');\n",style.c_str());
1983 
1984  return str;
1985  MRPT_END_WITH_CLEAN_UP(cerr << "The matrix that led to error was: " << endl << cov << endl; )
1986 }
1987 
1988 /*---------------------------------------------------------------
1989  cross_correlation_FFT
1990  ---------------------------------------------------------------*/
1991 void mrpt::math::cross_correlation_FFT(
1992  const CMatrixFloat &A,
1993  const CMatrixFloat &B,
1994  CMatrixFloat &out_corr )
1995 {
1996  MRPT_START
1997 
1998  ASSERT_( size(A,1)==size(B,1) && size(A,2)==size(B,2) );
1999  if ( math::round2up( size(A,1) ) != size(A,1) || math::round2up( size(A,2) ) != size(A,2) )
2000  THROW_EXCEPTION("Size of input matrices must be powers of two.");
2001 
2002  // Find smallest valid size:
2003  size_t x,y;
2004  const size_t lx = size(A,2);
2005  const size_t ly = size(A,1);
2006 
2007  const CMatrixFloat &i1 = A;
2008  const CMatrixFloat &i2 = B;
2009 
2010  // FFT:
2011  CMatrixFloat I1_R,I1_I,I2_R,I2_I,ZEROS(ly,lx);
2012  math::dft2_complex(i1,ZEROS,I1_R,I1_I);
2013  math::dft2_complex(i2,ZEROS,I2_R,I2_I);
2014 
2015  // Compute the COMPLEX division of I2 by I1:
2016  for (y = 0;y<ly;y++)
2017  for (x = 0;x<lx;x++)
2018  {
2019  float r1 = I1_R.get_unsafe(y,x);
2020  float r2 = I2_R.get_unsafe(y,x);
2021 
2022  float ii1 = I1_I.get_unsafe(y,x);
2023  float ii2 = I2_I.get_unsafe(y,x);
2024 
2025  float den = square(r1)+square(ii1);
2026  I2_R.set_unsafe(y,x, (r1*r2+ii1*ii2)/den);
2027  I2_I.set_unsafe(y,x, (ii2*r1-r2*ii1)/den);
2028  }
2029 
2030  // IFFT:
2031  CMatrixFloat res_R,res_I;
2032  math::idft2_complex(I2_R,I2_I,res_R,res_I);
2033 
2034  out_corr.setSize(ly,lx);
2035  for (y = 0;y<ly;y++)
2036  for (x = 0;x<lx;x++)
2037  out_corr(y,x) = sqrt( square(res_R(y,x)) + square(res_I(y,x)) );
2038 
2039  MRPT_END
2040 }
2041 
2042 
2043 double mrpt::math::interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi )
2044 {
2045  MRPT_START
2046  if (x0==x1) THROW_EXCEPTION_FMT("ERROR: Both x0 and x1 are equal (=%f)",x0);
2047 
2048  const double Ax = x1-x0;
2049  const double Ay = y1-y0;
2050 
2051  double r = y0+Ay*(x-x0)/Ax;
2052  if (!wrap2pi)
2053  return r;
2054  else return mrpt::math::wrapToPi(r);
2055 
2056  MRPT_END
2057 }
2058 
2059 /*---------------------------------------------------------------
2060  median filter of a vector
2061  ---------------------------------------------------------------*/
2062 //template<typename VECTOR>
2063 void mrpt::math::medianFilter( const std::vector<double> &inV, std::vector<double> &outV, const int &_winSize, const int &numberOfSigmas )
2064 {
2065  MRPT_UNUSED_PARAM(numberOfSigmas);
2066  ASSERT_( (int)inV.size() >= _winSize );
2067  ASSERT_( _winSize >= 2 ); // The minimum window size is 3 elements
2068  size_t winSize = _winSize;
2069 
2070  if( !(winSize%2) ) // We use an odd number of elements for the window size
2071  winSize++;
2072 
2073  size_t sz = inV.size();
2074  outV.resize( sz );
2075 
2076  std::vector<double> aux(winSize);
2077  size_t mpoint = winSize/2;
2078  for( size_t k = 0; k < sz; ++k )
2079  {
2080  aux.clear();
2081 
2082  size_t idx_to_start = std::max( size_t(0), k-mpoint ); // Dealing with the boundaries
2083  size_t n_elements = std::min(std::min( winSize, sz+mpoint-k), k+mpoint+1 );
2084 
2085  aux.resize(n_elements);
2086  for(size_t m = idx_to_start, n = 0; m < idx_to_start+n_elements; ++m, ++n )
2087  aux[n] = inV[m];
2088 
2089  std::sort( aux.begin(), aux.end() );
2090 
2091  size_t auxSz = aux.size();
2092  size_t auxMPoint = auxSz/2;
2093  outV[k] = (auxSz%2) ? (aux[auxMPoint]) : (0.5*(aux[auxMPoint-1]+aux[auxMPoint])); // If the window is even, take the mean value of the middle points
2094  } // end-for
2095 } // end medianFilter
2096 
2097 
2098 double mrpt::math::chi2CDF(unsigned int degreesOfFreedom, double arg)
2099 {
2100  return noncentralChi2CDF(degreesOfFreedom, 0.0, arg);
2101 }
2102 
2103 template <class T>
2104 void noncentralChi2OneIteration(T arg, T & lans, T & dans, T & pans, unsigned int & j)
2105 {
2106  double tol = -50.0;
2107  if(lans < tol)
2108  {
2109  lans = lans + std::log(arg / j);
2110  dans = std::exp(lans);
2111  }
2112  else
2113  {
2114  dans = dans * arg / j;
2115  }
2116  pans = pans - dans;
2117  j += 2;
2118 }
2119 
2120 std::pair<double, double> mrpt::math::noncentralChi2PDF_CDF(unsigned int degreesOfFreedom, double noncentrality, double arg, double eps)
2121 {
2122  ASSERTMSG_(noncentrality >= 0.0 && arg >= 0.0 && eps > 0.0,"noncentralChi2PDF_CDF(): parameters must be positive.")
2123 
2124  if (arg == 0.0 && degreesOfFreedom > 0)
2125  return std::make_pair(0.0, 0.0);
2126 
2127  // Determine initial values
2128  double b1 = 0.5 * noncentrality,
2129  ao = std::exp(-b1),
2130  eps2 = eps / ao,
2131  lnrtpi2 = 0.22579135264473,
2132  probability, density, lans, dans, pans, sum, am, hold;
2133  unsigned int maxit = 500,
2134  i, m;
2135  if(degreesOfFreedom % 2)
2136  {
2137  i = 1;
2138  lans = -0.5 * (arg + std::log(arg)) - lnrtpi2;
2139  dans = std::exp(lans);
2140  pans = mrpt::math::erf(std::sqrt(arg/2.0));
2141  }
2142  else
2143  {
2144  i = 2;
2145  lans = -0.5 * arg;
2146  dans = std::exp(lans);
2147  pans = 1.0 - dans;
2148  }
2149 
2150  // Evaluate first term
2151  if(degreesOfFreedom == 0)
2152  {
2153  m = 1;
2154  degreesOfFreedom = 2;
2155  am = b1;
2156  sum = 1.0 / ao - 1.0 - am;
2157  density = am * dans;
2158  probability = 1.0 + am * pans;
2159  }
2160  else
2161  {
2162  m = 0;
2163  degreesOfFreedom = degreesOfFreedom - 1;
2164  am = 1.0;
2165  sum = 1.0 / ao - 1.0;
2166  while(i < degreesOfFreedom)
2167  noncentralChi2OneIteration(arg, lans, dans, pans, i);
2168  degreesOfFreedom = degreesOfFreedom + 1;
2169  density = dans;
2170  probability = pans;
2171  }
2172  // Evaluate successive terms of the expansion
2173  for(++m; m<maxit; ++m)
2174  {
2175  am = b1 * am / m;
2176  noncentralChi2OneIteration(arg, lans, dans, pans, degreesOfFreedom);
2177  sum = sum - am;
2178  density = density + am * dans;
2179  hold = am * pans;
2180  probability = probability + hold;
2181  if((pans * sum < eps2) && (hold < eps2))
2182  break; // converged
2183  }
2184  if(m == maxit)
2185  THROW_EXCEPTION("noncentralChi2PDF_CDF(): no convergence.");
2186  return std::make_pair(0.5 * ao * density, std::min(1.0, std::max(0.0, ao * probability)));
2187 }
2188 
2189 double mrpt::math::chi2PDF(unsigned int degreesOfFreedom, double arg, double accuracy)
2190 {
2191  return mrpt::math::noncentralChi2PDF_CDF(degreesOfFreedom, 0.0, arg, accuracy).first;
2192 }
2193 
2194 double mrpt::math::noncentralChi2CDF(unsigned int degreesOfFreedom, double noncentrality, double arg)
2195 {
2196  const double a = degreesOfFreedom + noncentrality;
2197  const double b = (a + noncentrality) / square(a);
2198  const double t = (std::pow((double)arg / a, 1.0/3.0) - (1.0 - 2.0 / 9.0 * b)) / std::sqrt(2.0 / 9.0 * b);
2199  return 0.5*(1.0 + mrpt::math::erf(t/std::sqrt(2.0)));
2200 }
mrpt::system::os::memcpy
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
mrpt::utils::CFileStream::readLine
bool readLine(std::string &str)
Reads one string line from the file (until a new-line character)
Definition: CFileStream.cpp:194
mrpt::math::averageWrap2Pi
double BASE_IMPEXP averageWrap2Pi(const CVectorDouble &angles)
Computes the average of a sequence of angles in radians taking into account the correct wrapping in t...
Definition: math.cpp:1864
mrpt::math::chi2CDF
double BASE_IMPEXP chi2CDF(unsigned int degreesOfFreedom, double arg)
Definition: math.cpp:2098
mrpt::math::MATLAB_plotCovariance2D
std::string BASE_IMPEXP 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...
Definition: math.cpp:1905
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
t
GLdouble GLdouble t
Definition: glext.h:3610
z
GLdouble GLdouble z
Definition: glext.h:3734
mrpt::math::CMatrixD
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:30
min
#define min(a, b)
Definition: rplidar_driver.cpp:43
mrpt::mrpt::math::isNaN
bool BASE_IMPEXP isNaN(float v) MRPT_NO_THROWS
mrpt::math::cdft
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: math.cpp:495
types_math.h
MRPT_END_WITH_CLEAN_UP
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: maps/CColouredPointsMap.h:373
mrpt::math::bitrv2
void bitrv2(int n, int *ip, FFT_TYPE *a)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
Definition: math.cpp:437
mrpt::math::rdft
void rdft(int n, int isgn, FFT_TYPE *a, int *ip, FFT_TYPE *w)
Definition: math.cpp:574
CFileStream.h
scale
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:5717
mrpt::math::dft2_real
void BASE_IMPEXP 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: math.cpp:1186
MRPT_CHECK_NORMAL_NUMBER
#define MRPT_CHECK_NORMAL_NUMBER(val)
Definition: maps/CColouredPointsMap.h:279
mrpt::math::normalCDF
double BASE_IMPEXP normalCDF(double p)
Evaluates the Gaussian cumulative density function.
Definition: math.cpp:1108
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: maps/CColouredPointsMap.h:154
q
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: maps/CColouredPointsMap.h:163
n
GLenum GLsizei n
Definition: glext.h:4618
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::utils::CFileStream
This CStream derived class allow using a file as a read/write binary stream, creating it if the file ...
Definition: CFileStream.h:39
mrpt::math::fft_real
void BASE_IMPEXP 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: math.cpp:1004
MRPT_NO_THROWS
#define MRPT_NO_THROWS
C++11 noexcept: Used after member declarations.
Definition: maps/CColouredPointsMap.h:72
data_utils.h
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
ops_matrices.h
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
std
STL namespace.
M_PI
#define M_PI
Definition: bits.h:78
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
myGeneralDFT
void myGeneralDFT(int sign, const CMatrixFloat &in_real, const CMatrixFloat &in_imag, CMatrixFloat &out_real, CMatrixFloat &out_imag)
Definition: math.cpp:1394
s
GLdouble s
Definition: glext.h:3602
mrpt::math::cftbsub
void cftbsub(int n, FFT_TYPE *a, FFT_TYPE *w)
Definition: math.cpp:156
w
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
mrpt::math::makect
void makect(int nc, int *ip, FFT_TYPE *c)
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
Definition: math.cpp:414
M_2PI
#define M_2PI
Definition: mrpt_macros.h:380
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
string_utils.h
mrpt::math::factorial
double BASE_IMPEXP factorial(unsigned int n)
Computes the factorial of an integer number and returns it as a double value (internally it uses loga...
Definition: math.cpp:991
mrpt::math::isNaN
bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS
Returns true if the number is NaN.
Definition: math.cpp:1679
mrpt::math::normalQuantile
double BASE_IMPEXP normalQuantile(double p)
Evaluates the Gaussian distribution quantile for the probability value p=[0,1].
Definition: math.cpp:1046
mrpt::math::erfc
double BASE_IMPEXP erfc(const double x)
The complementary error function of a Normal distribution.
Definition: math.cpp:917
mrpt::math::makewt
void makewt(int nw, int *ip, FFT_TYPE *w)
Definition: math.cpp:382
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::math::round2up
T round2up(T val)
Round up to the nearest power of two of a given number.
Definition: base/include/mrpt/math/utils.h:167
mrpt::math::four1
void four1(float data[], unsigned long nn, int isign)
Definition: math.cpp:41
MRPT_END
#define MRPT_END
Definition: maps/CColouredPointsMap.h:370
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: maps/CColouredPointsMap.h:307
mrpt::math::erf
double BASE_IMPEXP erf(const double x)
The error function of a Normal distribution.
Definition: math.cpp:955
mrpt::math::cov
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...
Definition: ops_matrices.h:135
mrpt::math::averageLogLikelihood
double BASE_IMPEXP averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
Definition: math.cpp:1840
c
const GLubyte * c
Definition: glext.h:5590
mrpt::math::cross_correlation_FFT
void BASE_IMPEXP cross_correlation_FFT(const CMatrixFloat &A, const CMatrixFloat &B, CMatrixFloat &out_corr)
Correlation of two matrixes using 2D FFT.
Definition: math.cpp:1991
mrpt::math::cdft2d
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: math.cpp:869
mrpt::math::sum
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
Definition: ops_containers.h:167
mrpt::math::factorial64
uint64_t BASE_IMPEXP factorial64(unsigned int n)
Computes the factorial of an integer number and returns it as a 64-bit integer number.
Definition: math.cpp:978
mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
b
GLubyte GLubyte b
Definition: glext.h:5575
eps
const double eps
Definition: distributions_unittest.cpp:21
mrpt::math::dstsub
void dstsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
Definition: math.cpp:553
mrpt::math::maximum
CONTAINER::Scalar maximum(const CONTAINER &v)
Definition: ops_containers.h:111
mrpt::math::rftfsub
void rftfsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
Definition: math.cpp:510
string
GLsizei const GLchar ** string
Definition: glext.h:3919
mrpt::math::wrapToPi
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
mrpt::math::realft
void realft(float data[], unsigned long n)
Definition: math.cpp:102
mrpt::utils::sign
int sign(T x)
Returns the sign of X as "1" or "-1".
Definition: bits.h:104
mrpt::math::idft2_complex
void BASE_IMPEXP 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: math.cpp:1560
noncentralChi2OneIteration
void noncentralChi2OneIteration(T arg, T &lans, T &dans, T &pans, unsigned int &j)
Definition: math.cpp:2104
mrpt::math::isFinite
bool BASE_IMPEXP isFinite(float f) MRPT_NO_THROWS
Returns true if the number is non infinity.
Definition: math.cpp:1705
MRPT_START
#define MRPT_START
Definition: maps/CColouredPointsMap.h:366
uint64_t
unsigned __int64 uint64_t
Definition: rptypes.h:52
mrpt::math::cftfsub
void cftfsub(int n, FFT_TYPE *a, FFT_TYPE *w)
Definition: math.cpp:268
v
const GLdouble * v
Definition: glext.h:3603
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::math::idft2_real
void BASE_IMPEXP idft2_real(const CMatrixFloat &in_real, const CMatrixFloat &in_imag, CMatrixFloat &out_data)
Compute the 2D inverse Discrete Fourier Transform (DFT)
Definition: math.cpp:1286
mrpt::math::chi2inv
double BASE_IMPEXP chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
Definition: math.cpp:967
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
mrpt::math::loadVector
bool BASE_IMPEXP loadVector(utils::CFileStream &f, std::vector< int > &d)
Loads one row of a text file as a numerical std::vector.
R
const float R
Definition: CKinematicChain.cpp:141
mrpt::math::noncentralChi2PDF_CDF
std::pair< double, double > BASE_IMPEXP noncentralChi2PDF_CDF(unsigned int degreesOfFreedom, double noncentrality, double arg, double eps=1e-7)
Returns the &#39;exact&#39; PDF (first) and CDF (second) of a Non-central chi-squared probability distributio...
Definition: math.cpp:2120
ASSERT_
#define ASSERT_(f)
Definition: maps/CColouredPointsMap.h:278
y
GLenum GLint GLint y
Definition: glext.h:3516
CMatrixD.h
mrpt::math::rftbsub
void rftbsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
Definition: math.cpp:611
mrpt::math::medianFilter
void BASE_IMPEXP medianFilter(const std::vector< double > &inV, std::vector< double > &outV, const int &winSize, const int &numberOfSigmas=2)
Definition: math.cpp:2063
interp_fit.hpp
size
GLsizeiptr size
Definition: glext.h:3779
mrpt::mrpt::math::isFinite
bool BASE_IMPEXP isFinite(float v) MRPT_NO_THROWS
res
GLuint res
Definition: glext.h:6298
mrpt::math::interpolate2points
double BASE_IMPEXP 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).
Definition: math.cpp:2043
x
GLenum GLint x
Definition: glext.h:3516
mrpt::math::dft2_complex
void BASE_IMPEXP 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: math.cpp:1451
mrpt::math::normalPDF
double BASE_IMPEXP normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
Definition: math.cpp:908
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: maps/CColouredPointsMap.h:277
data
GLsizei GLsizei GLenum GLenum const GLvoid * data
Definition: glext.h:3520
a
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
p
GLfloat GLfloat p
Definition: glext.h:5587
mrpt::math::chi2PDF
double BASE_IMPEXP chi2PDF(unsigned int degreesOfFreedom, double arg, double accuracy=1e-7)
Definition: math.cpp:2189
mrpt::system::strtok
char BASE_IMPEXP * strtok(char *str, const char *strDelimit, char **context) MRPT_NO_THROWS
An OS-independent method for tokenizing a string.
Definition: string_utils.cpp:172
mrpt::math::dctsub
void dctsub(int n, FFT_TYPE *a, int nc, FFT_TYPE *c)
Definition: math.cpp:533
mean
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
Definition: eigen_plugins.h:355
mrpt::math::rdft2d
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: math.cpp:734
mrpt::math::noncentralChi2CDF
double BASE_IMPEXP noncentralChi2CDF(unsigned int degreesOfFreedom, double noncentrality, double arg)
Definition: math.cpp:2194
mrpt::math::FFT_TYPE
float FFT_TYPE
Copyright(C) 1997 Takuya OOURA (email: ooura@mmm.t.u-tokyo.ac.jp).
Definition: math.cpp:143


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019