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


Page generated by Doxygen 1.9.1 for MRPT 1.9.9 Git: 63ea9d1f1 Thu Nov 23 00:06:53 2017 +0100 at mar 26 may 2026 12:19:29 CEST