datasmoother.h
Go to the documentation of this file.
1 #ifndef DATASMOOTHER_H
2 #define DATASMOOTHER_H
3 
4 #include <list>
5 #include <stdio.h>
6 #include <math.h>
7 #include <values.h>
8 #include "stat.h"
9 #include <assert.h>
10 
11 namespace GMapping {
12 
13 class DataSmoother {
14  public:
15  struct DataPoint {
16  DataPoint(double _x=0.0, double _y=0.0) { x=_x;y=_y;}
17  double x;
18  double y;
19  };
20 
21  typedef std::vector<DataPoint> Data;
22 
23  DataSmoother(double parzenWindow) {
24  init(parzenWindow);
25  };
26 
27  virtual ~DataSmoother() {
28  m_data.clear();
29  m_cummulated.clear();
30  };
31 
32  void init(double parzenWindow) {
33  m_data.clear();
34  m_cummulated.clear();
35  m_int=-1;
36  m_parzenWindow = parzenWindow;
37  m_from = MAXDOUBLE;
38  m_to = -MAXDOUBLE;
39  m_lastStep = 0.001;
40  };
41 
42 
43  double sqr(double x) {
44  return x*x;
45  }
46 
47 
48  void setMinToZero() {
49  double minval=MAXDOUBLE;
50 
51  for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) {
52  const DataPoint& d = *it;
53  if (minval > d.y)
54  minval = d.y;
55  }
56 
57  for (Data::iterator it = m_data.begin(); it != m_data.end(); it++) {
58  DataPoint& d = *it;
59  d.y = d.y - minval;
60  }
61 
62  m_cummulated.clear();
63  }
64 
65  void add(double x, double p) {
66  m_data.push_back(DataPoint(x,p));
67  m_int=-1;
68 
69  if (x-3.0*m_parzenWindow < m_from)
70  m_from = x - 3.0*m_parzenWindow;
71 
72  if (x+3.0*m_parzenWindow > m_to)
73  m_to = x + 3.0*m_parzenWindow;
74 
75  m_cummulated.clear();
76  }
77 
78  void integrate(double step) {
79  m_lastStep = step;
80  double sum=0;
81  for (double x=m_from; x<=m_to; x+=step)
82  sum += smoothedData(x)*step;
83  m_int = sum;
84  }
85 
86  double integral(double step, double xTo) {
87  double sum=0;
88  for (double x=m_from; x<=xTo; x+=step)
89  sum += smoothedData(x)*step;
90  return sum;
91  }
92 
93 
94  double smoothedData(double x) {
95  assert( m_data.size() > 0 );
96 
97  double p=0;
98  double sum_y=0;
99  for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) {
100  const DataPoint& d = *it;
101  double dist = fabs(x - d.x);
102  p += d.y * exp( -0.5 * sqr ( dist/m_parzenWindow ) );
103  sum_y += d.y;
104  }
105  double denom = sqrt(2.0 * M_PI) * (sum_y) * m_parzenWindow;
106  p *= 1./denom;
107 
108  return p;
109  }
110 
111  double sampleNumeric(double step) {
112 
113  assert( m_data.size() > 0 );
114 
115  if (m_int <0 || step != m_lastStep)
116  integrate(step);
117 
118  double r = sampleUniformDouble(0.0, m_int);
119  double sum2=0;
120  for (double x=m_from; x<=m_to; x+=step) {
121  sum2 += smoothedData(x)*step;
122  if (sum2 > r)
123  return x-0.5*step;
124  }
125  return m_to;
126  }
127 
128  void computeCummuated() {
129  assert( m_data.size() > 0 );
130  m_cummulated.resize(m_data.size());
131  std::vector<double>::iterator cit = m_cummulated.begin();
132  double sum=0;
133  for (Data::const_iterator it = m_data.begin(); it != m_data.end(); ++it) {
134  sum += it->y;
135  (*cit) = sum;
136  ++cit;
137  }
138  }
139 
140  double sample() {
141 
142  assert( m_data.size() > 0 );
143 
144  if (m_cummulated.size() == 0) {
145  computeCummuated();
146  }
147  double maxval = m_cummulated.back();
148 
149  double random = sampleUniformDouble(0.0, maxval);
150  int nCum = (int) m_cummulated.size();
151  double sum=0;
152  int i=0;
153  while (i<nCum) {
154  sum += m_cummulated[i];
155 
156  if (sum >= random) {
157  return m_data[i].x + sampleGaussian(m_parzenWindow);
158  }
159  i++;
160  }
161  assert(0);
162  }
163 
164 
165  void sampleMultiple(std::vector<double>& samples, int num) {
166 
167  assert( m_data.size() > 0 );
168  samples.clear();
169 
170  if (m_cummulated.size() == 0) {
171  computeCummuated();
172  }
173  double maxval = m_cummulated.back();
174 
175  std::vector<double> randoms(num);
176  for (int i=0; i<num; i++)
177  randoms[i] = sampleUniformDouble(0.0, maxval);
178 
179  std::sort(randoms.begin(), randoms.end());
180 
181  int nCum = (int) m_cummulated.size();
182 
183  double sum=0;
184  int i=0;
185  int j=0;
186  while (i<nCum && j < num) {
187  sum += m_cummulated[i];
188 
189  while (sum >= randoms[j] && j < num) {
190  samples.push_back( m_data[i].x + sampleGaussian(m_parzenWindow) );
191  j++;
192  }
193  i++;
194  }
195  }
196 
197 
198 
199  void approxGauss(double step, double* mean, double* sigma) {
200 
201  assert( m_data.size() > 0 );
202 
203  double sum=0;
204  double d=0;
205 
206  *mean=0;
207  for (double x=m_from; x<=m_to; x+=step) {
208  d = smoothedData(x);
209  sum += d;
210  *mean += x*d;
211  }
212  *mean /= sum;
213 
214  double var=0;
215  for (double x=m_from; x<=m_to; x+=step) {
216  d = smoothedData(x);
217  var += sqr(x-*mean) * d;
218  }
219  var /= sum;
220 
221  *sigma = sqrt(var);
222  }
223 
224  double gauss(double x, double mean, double sigma) {
225  return 1.0/(sqrt(2.0*M_PI)*sigma) * exp(-0.5 * sqr( (x-mean)/sigma));
226  }
227 
228  double cramerVonMisesToGauss(double step, double mean, double sigma) {
229 
230  double p=0;
231  double s=0;
232  double g=0;
233  double sint=0;
234  double gint=0;
235 
236  for (double x=m_from; x<=m_to; x+=step) {
237  s = smoothedData(x);
238  sint += s * step;
239 
240  g = gauss(x, mean, sigma);
241  gint += g * step;
242 
243  p += sqr( (sint - gint) );
244  }
245 
246  return p;
247  }
248 
249  double kldToGauss(double step, double mean, double sigma) {
250 
251  double p=0;
252  double d=0;
253  double g=0;
254 
255  double sd=0;
256  double sg=0;
257 
258  for (double x=m_from; x<=m_to; x+=step) {
259 
260  d = 1e-10 + smoothedData(x);
261  g = 1e-10 + gauss(x, mean, sigma);
262 
263  sd += d;
264  sg += g;
265 
266  p += d * log(d/g);
267  }
268 
269  sd *= step;
270  sg *= step;
271 
272  if (fabs(sd-sg) > 0.1)
273  assert(0);
274 
275  p *= step;
276  return p;
277  }
278 
279 
280  void gnuplotDumpData(FILE* fp) {
281  for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) {
282  const DataPoint& d = *it;
283  fprintf(fp, "%f %f\n", d.x, d.y);
284  }
285  }
286 
287  void gnuplotDumpSmoothedData(FILE* fp, double step) {
288  for (double x=m_from; x<=m_to; x+=step)
289  fprintf(fp, "%f %f\n", x, smoothedData(x));
290  }
291 
292  protected:
293  Data m_data;
294  std::vector<double> m_cummulated;
295  double m_int;
296  double m_lastStep;
297 
298  double m_parzenWindow;
299  double m_from;
300  double m_to;
301 
302 };
303 
304 
305 
306 
307 /* class DataSmoother3D { */
308 /* public: */
309 /* struct InputPoint { */
310 /* InputPoint(double _x=0.0, double _y=0.0, double _t=0.0) { x=_x;y=_y;t=_t;} */
311 /* double x; */
312 /* double y; */
313 /* double t; */
314 /* }; */
315 
316 /* struct DataPoint { */
317 /* DataPoint(const InputPoint& _p, double _val=0.0;) { p=_p;val=_val;} */
318 /* InputPoint p; */
319 /* double val; */
320 /* }; */
321 
322 /* typedef std::list<DataPoint> Data; */
323 
324 /* DataSmoother(double parzenWindow) { */
325 /* m_int=-1; */
326 /* m_parzenWindow = parzenWindow; */
327 /* m_from = InputPoint(MAXDOUBLE,MAXDOUBLE,MAXDOUBLE); */
328 /* m_to = InputPoint(-MAXDOUBLE,-MAXDOUBLE,-MAXDOUBLE); */
329 /* }; */
330 
331 /* virtual ~DataSmoother() { */
332 /* m_data.clear(); */
333 /* }; */
334 
335 /* double sqr(double x) { */
336 /* return x*x; */
337 /* } */
338 
339 
340 /* void setMinToZero() { */
341 /* double minval=MAXDOUBLE; */
342 /* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */
343 /* const DataPoint& d = *it; */
344 /* if (minval > d.val) */
345 /* minval = d.val; */
346 /* } */
347 
348 /* for (Data::iterator it = m_data.begin(); it != m_data.end(); it++) { */
349 /* DataPoint& d = *it; */
350 /* d.val = d.val - minval; */
351 /* } */
352 
353 /* } */
354 
355 /* void add(double x, double y, double t, double v) { */
356 /* m_data.push_back(DataPoint(InputPoint(x,y,t),v)); */
357 /* m_int=-1; */
358 
359 /* if (x-3.0*m_parzenWindow < m_from.x) */
360 /* m_from.x = x - 3.0*m_parzenWindow.x; */
361 /* if (x+3.0*m_parzenWindow.x > m_to.x) */
362 /* m_to.x = x + 3.0*m_parzenWindow.x; */
363 
364 /* if (y-3.0*m_parzenWindow < m_from.y) */
365 /* m_from.y = y - 3.0*m_parzenWindow.y; */
366 /* if (y+3.0*m_parzenWindow.y > m_to.y) */
367 /* m_to.y = y + 3.0*m_parzenWindow.y; */
368 
369 /* if (t-3.0*m_parzenWindow < m_from.t) */
370 /* m_from.t = t - 3.0*m_parzenWindow.t; */
371 /* if (t+3.0*m_parzenWindow.t > m_to.t) */
372 /* m_to.t = t + 3.0*m_parzenWindow.t; */
373 /* } */
374 
375 /* void integrate(InputPoint step) { */
376 /* m_lastStep = step; */
377 /* double sum=0; */
378 /* for (double x=m_from.x; x<=m_to.x; x+=step.x) { */
379 /* for (double y=m_from.y; x<=m_to.y; y+=step.y) { */
380 /* for (double t=m_from.t; t<=m_to.t; t+=step.t) { */
381 /* sum += smoothedData(InputPoint(x,y,t)) * step.x * step.y * step.t; */
382 /* } */
383 /* } */
384 /* } */
385 /* m_int = sum; */
386 /* } */
387 
388 
389 /* double smoothedData(InputPoint pnt) { */
390 /* assert( m_data.size() > 0 ); */
391 /* double p=0; */
392 /* double sum_y=0; */
393 /* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */
394 /* const DataPoint& d = *it; */
395 /* double u = sqr( (pnt.x-d.x)/m_parzenWindow.x) + */
396 /* sqr((pnt.y-d.y)/m_parzenWindow.y) + */
397 /* sqr((pnt.t-d.t)/m_parzenWindow.t); */
398 /* p += d.val * exp( -0.5 * u); */
399 /* sum_y += d.y; */
400 /* } */
401 /* double denom = sqr(m_parzenWindow.x)*sqr(m_parzenWindow.x)*sqr(m_parzenWindow.x) * (sum_y) * */
402 /* sqrt(sqr(m_parzenWindow.x) + sqr(m_parzenWindow.y) + sqr(m_parzenWindow.t)); */
403 /* p *= 1./denom; */
404 
405 /* return p; */
406 /* } */
407 
408 /* double sample(const InputPoint& step) { */
409 
410 /* assert( m_data.size() > 0 ); */
411 
412 /* if (m_int <0 || step != m_lastStep) */
413 /* integrate(step); */
414 
415 /* double r = sampleUniformDouble(0.0, m_int); */
416 /* double sum2=0; */
417 /* for (double x=m_from; x<=m_to; x+=step) { */
418 /* sum2 += smoothedData(x)*step; */
419 /* if (sum2 > r) */
420 /* return x-0.5*step; */
421 /* } */
422 /* return m_to; */
423 /* } */
424 
425 /* void gnuplotDumpData(FILE* fp) { */
426 /* for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */
427 /* const DataPoint& d = *it; */
428 /* fprintf(fp, "%f %f %f %f\n", d.x, d.y, d.t, d.val); */
429 /* } */
430 /* } */
431 
432 /* void gnuplotDumpSmoothedData(FILE* fp, double step) { */
433 /* for (double x=m_from; x<=m_to; x+=step) */
434 /* fprintf(fp, "%f %f %f %f\n", x, ,y, t, smoothedData(x,y,t)); */
435 /* } */
436 
437 /* protected: */
438 /* Data m_data; */
439 /* vector<double> m_intdata; */
440 /* double m_int; */
441 /* double m_lastStep; */
442 
443 /* double m_parzenWindow; */
444 /* double m_from; */
445 /* double m_to; */
446 
447 /* }; */
448 
449 }
450 
451 #endif
GMapping::sampleUniformDouble
double sampleUniformDouble(double min, double max)
GMapping::DataSmoother::kldToGauss
double kldToGauss(double step, double mean, double sigma)
Definition: datasmoother.h:249
GMapping::DataSmoother::add
void add(double x, double p)
Definition: datasmoother.h:65
GMapping::DataSmoother::gnuplotDumpData
void gnuplotDumpData(FILE *fp)
Definition: datasmoother.h:280
GMapping::DataSmoother::sampleMultiple
void sampleMultiple(std::vector< double > &samples, int num)
Definition: datasmoother.h:165
GMapping::sampleGaussian
double sampleGaussian(double sigma, unsigned int S=0)
Definition: stat.cpp:49
GMapping::DataSmoother::approxGauss
void approxGauss(double step, double *mean, double *sigma)
Definition: datasmoother.h:199
GMapping::DataSmoother::gauss
double gauss(double x, double mean, double sigma)
Definition: datasmoother.h:224
GMapping::DataSmoother::integral
double integral(double step, double xTo)
Definition: datasmoother.h:86
GMapping::DataSmoother::sqr
double sqr(double x)
Definition: datasmoother.h:43
GMapping::DataSmoother::cramerVonMisesToGauss
double cramerVonMisesToGauss(double step, double mean, double sigma)
Definition: datasmoother.h:228
GMapping::DataSmoother::init
void init(double parzenWindow)
Definition: datasmoother.h:32
GMapping::DataSmoother::Data
std::vector< DataPoint > Data
Definition: datasmoother.h:21
MAXDOUBLE
#define MAXDOUBLE
Definition: gvalues.h:4
GMapping::DataSmoother::integrate
void integrate(double step)
Definition: datasmoother.h:78
GMapping::DataSmoother::smoothedData
double smoothedData(double x)
Definition: datasmoother.h:94
GMapping::DataSmoother::sampleNumeric
double sampleNumeric(double step)
Definition: datasmoother.h:111
GMapping::DataSmoother::DataSmoother
DataSmoother(double parzenWindow)
Definition: datasmoother.h:23
GMapping::DataSmoother::DataPoint::DataPoint
DataPoint(double _x=0.0, double _y=0.0)
Definition: datasmoother.h:16
GMapping::DataSmoother::gnuplotDumpSmoothedData
void gnuplotDumpSmoothedData(FILE *fp, double step)
Definition: datasmoother.h:287
GMapping::DataSmoother::m_cummulated
std::vector< double > m_cummulated
Definition: datasmoother.h:294

openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Mon Jun 10 2019 14:04:22