datasmoother.h
Go to the documentation of this file.
00001 #ifndef DATASMOOTHER_H
00002 #define DATASMOOTHER_H
00003 
00004 #include <list>
00005 #include <stdio.h>
00006 #include <math.h>
00007 #include <values.h>
00008 #include "stat.h"
00009 #include <assert.h>
00010 
00011 namespace GMapping {
00012 
00013 class DataSmoother {
00014  public: 
00015   struct DataPoint {
00016     DataPoint(double _x=0.0, double _y=0.0) { x=_x;y=_y;}
00017     double x;
00018     double y;
00019   };
00020   
00021   typedef std::vector<DataPoint> Data;
00022 
00023   DataSmoother(double parzenWindow) {
00024     init(parzenWindow);
00025   };
00026 
00027   virtual ~DataSmoother() {
00028     m_data.clear();
00029     m_cummulated.clear();
00030   };
00031 
00032   void init(double parzenWindow) {
00033     m_data.clear();
00034     m_cummulated.clear();
00035     m_int=-1; 
00036     m_parzenWindow = parzenWindow;
00037     m_from = MAXDOUBLE;
00038     m_to = -MAXDOUBLE;
00039     m_lastStep = 0.001;
00040   };
00041 
00042 
00043   double sqr(double x) {
00044     return x*x;
00045   }
00046 
00047 
00048   void setMinToZero() {
00049     double minval=MAXDOUBLE;
00050 
00051     for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) {
00052       const DataPoint& d = *it;
00053       if (minval > d.y)
00054         minval = d.y;
00055     }
00056 
00057     for (Data::iterator it = m_data.begin(); it != m_data.end(); it++) {
00058       DataPoint& d = *it;
00059       d.y = d.y - minval;
00060     }
00061 
00062     m_cummulated.clear();
00063   }
00064 
00065   void add(double x, double p) {
00066     m_data.push_back(DataPoint(x,p));
00067     m_int=-1;
00068     
00069     if (x-3.0*m_parzenWindow < m_from)
00070       m_from = x - 3.0*m_parzenWindow;
00071 
00072     if (x+3.0*m_parzenWindow > m_to)
00073       m_to = x + 3.0*m_parzenWindow;
00074 
00075     m_cummulated.clear();
00076   }
00077    
00078   void integrate(double step) {
00079     m_lastStep = step;
00080     double sum=0;
00081     for (double x=m_from; x<=m_to; x+=step)
00082       sum += smoothedData(x)*step;
00083     m_int = sum;
00084   }
00085 
00086   double integral(double step, double xTo) {
00087     double sum=0;
00088     for (double x=m_from; x<=xTo; x+=step)
00089       sum += smoothedData(x)*step;
00090     return sum;
00091   }
00092 
00093 
00094   double smoothedData(double x) {
00095     assert( m_data.size() > 0 );
00096 
00097     double p=0;
00098     double sum_y=0;
00099     for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) {
00100       const DataPoint& d = *it;
00101       double dist = fabs(x - d.x);
00102       p +=  d.y * exp( -0.5 * sqr ( dist/m_parzenWindow ) );
00103       sum_y += d.y;
00104     }
00105     double denom = sqrt(2.0 * M_PI) * (sum_y) * m_parzenWindow;
00106     p *= 1./denom; 
00107 
00108     return p;
00109   }
00110 
00111   double sampleNumeric(double step) {
00112 
00113     assert( m_data.size() > 0 );
00114 
00115     if (m_int <0 || step != m_lastStep)
00116       integrate(step);
00117     
00118     double r = sampleUniformDouble(0.0, m_int);
00119     double sum2=0;
00120     for (double x=m_from; x<=m_to; x+=step) {
00121       sum2 += smoothedData(x)*step;
00122       if (sum2 > r)
00123         return x-0.5*step;
00124     }
00125     return m_to;
00126   }
00127 
00128   void computeCummuated() {
00129     assert( m_data.size() > 0 );
00130     m_cummulated.resize(m_data.size());
00131     std::vector<double>::iterator cit = m_cummulated.begin();
00132     double sum=0;
00133     for (Data::const_iterator it = m_data.begin(); it != m_data.end(); ++it) {
00134       sum += it->y;
00135       (*cit) = sum;
00136       ++cit;
00137     }
00138   }
00139   
00140   double sample() {
00141     
00142     assert( m_data.size() > 0 );
00143     
00144     if (m_cummulated.size() == 0) {
00145       computeCummuated();
00146     }
00147     double maxval = m_cummulated.back();
00148     
00149     double random = sampleUniformDouble(0.0, maxval);
00150     int nCum = (int) m_cummulated.size();
00151     double sum=0;
00152     int i=0;
00153     while (i<nCum) {
00154       sum += m_cummulated[i];
00155       
00156       if  (sum >= random) {
00157         return m_data[i].x + sampleGaussian(m_parzenWindow);
00158       }
00159       i++;
00160     }
00161     assert(0);
00162   }
00163 
00164 
00165   void sampleMultiple(std::vector<double>& samples, int num) {
00166     
00167     assert( m_data.size() > 0 );
00168     samples.clear();
00169     
00170     if (m_cummulated.size() == 0) {
00171       computeCummuated();
00172     }
00173     double maxval = m_cummulated.back();
00174     
00175     std::vector<double> randoms(num);
00176     for (int i=0; i<num; i++)
00177       randoms[i] = sampleUniformDouble(0.0, maxval);
00178     
00179     std::sort(randoms.begin(), randoms.end());
00180     
00181     int nCum = (int) m_cummulated.size();
00182     
00183     double sum=0;
00184     int i=0;
00185     int j=0;
00186     while (i<nCum && j < num) {
00187       sum += m_cummulated[i];
00188       
00189       while (sum >= randoms[j] && j < num) {
00190         samples.push_back( m_data[i].x + sampleGaussian(m_parzenWindow) );
00191         j++;
00192       }
00193       i++;
00194     }
00195   }
00196 
00197 
00198 
00199   void approxGauss(double step, double* mean, double* sigma) {
00200 
00201     assert( m_data.size() > 0 );
00202 
00203     double sum=0;
00204     double d=0;
00205 
00206     *mean=0;
00207     for (double x=m_from; x<=m_to; x+=step) {
00208       d = smoothedData(x);
00209       sum += d;
00210       *mean += x*d;
00211     }
00212     *mean /= sum;
00213 
00214     double var=0;
00215     for (double x=m_from; x<=m_to; x+=step) {
00216       d = smoothedData(x);
00217       var += sqr(x-*mean) * d;
00218     }
00219     var /= sum;
00220 
00221     *sigma = sqrt(var);
00222   }
00223 
00224   double gauss(double x, double mean, double sigma) {
00225     return 1.0/(sqrt(2.0*M_PI)*sigma) * exp(-0.5 * sqr( (x-mean)/sigma));
00226   }
00227 
00228   double cramerVonMisesToGauss(double step, double mean, double sigma) {
00229     
00230     double p=0;
00231     double s=0;
00232     double g=0;
00233     double sint=0;
00234     double gint=0;
00235     
00236     for (double x=m_from; x<=m_to; x+=step) {
00237       s = smoothedData(x);
00238       sint += s * step;
00239 
00240       g = gauss(x, mean, sigma);
00241       gint += g * step;
00242 
00243       p += sqr( (sint - gint) );
00244     }
00245     
00246     return p;
00247   }
00248 
00249   double kldToGauss(double step, double mean, double sigma) {
00250     
00251     double p=0;
00252     double d=0;
00253     double g=0;
00254 
00255     double sd=0;
00256     double sg=0;
00257 
00258     for (double x=m_from; x<=m_to; x+=step) {
00259 
00260       d = 1e-10 + smoothedData(x);
00261       g = 1e-10 + gauss(x, mean, sigma);
00262 
00263       sd += d;
00264       sg += g;
00265       
00266       p += d * log(d/g);
00267     }
00268     
00269     sd *= step;
00270     sg *= step;
00271 
00272     if (fabs(sd-sg) > 0.1)
00273       assert(0);
00274 
00275     p *= step;
00276     return p;
00277   }
00278   
00279   
00280   void gnuplotDumpData(FILE* fp) {
00281     for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) {
00282       const DataPoint& d = *it;
00283       fprintf(fp, "%f %f\n", d.x, d.y);
00284     }
00285   }
00286 
00287   void gnuplotDumpSmoothedData(FILE* fp, double step) {
00288     for (double x=m_from; x<=m_to; x+=step)
00289       fprintf(fp, "%f %f\n", x, smoothedData(x));
00290   }
00291   
00292  protected:
00293   Data m_data;
00294   std::vector<double> m_cummulated;
00295   double m_int;
00296   double m_lastStep;
00297 
00298   double m_parzenWindow;
00299   double m_from;
00300   double m_to;
00301 
00302 };
00303 
00304 
00305 
00306 
00307 /* class DataSmoother3D { */
00308 /*  public:  */
00309 /*   struct InputPoint { */
00310 /*     InputPoint(double _x=0.0, double _y=0.0, double _t=0.0) { x=_x;y=_y;t=_t;} */
00311 /*     double x; */
00312 /*     double y; */
00313 /*     double t; */
00314 /*   }; */
00315 
00316 /*   struct DataPoint { */
00317 /*     DataPoint(const InputPoint& _p, double _val=0.0;)  { p=_p;val=_val;} */
00318 /*     InputPoint p; */
00319 /*     double val; */
00320 /*   }; */
00321   
00322 /*   typedef std::list<DataPoint> Data; */
00323 
00324 /*   DataSmoother(double parzenWindow) { */
00325 /*     m_int=-1;  */
00326 /*     m_parzenWindow = parzenWindow; */
00327 /*     m_from = InputPoint(MAXDOUBLE,MAXDOUBLE,MAXDOUBLE); */
00328 /*     m_to = InputPoint(-MAXDOUBLE,-MAXDOUBLE,-MAXDOUBLE); */
00329 /*   }; */
00330 
00331 /*   virtual ~DataSmoother() { */
00332 /*     m_data.clear(); */
00333 /*   }; */
00334 
00335 /*   double sqr(double x) { */
00336 /*     return x*x; */
00337 /*   } */
00338 
00339 
00340 /*   void setMinToZero() { */
00341 /*     double minval=MAXDOUBLE; */
00342 /*     for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */
00343 /*       const DataPoint& d = *it; */
00344 /*       if (minval > d.val) */
00345 /*      minval = d.val; */
00346 /*     } */
00347 
00348 /*     for (Data::iterator it = m_data.begin(); it != m_data.end(); it++) { */
00349 /*       DataPoint& d = *it; */
00350 /*       d.val = d.val - minval; */
00351 /*     } */
00352 
00353 /*   } */
00354 
00355 /*   void add(double x, double y, double t, double v) { */
00356 /*     m_data.push_back(DataPoint(InputPoint(x,y,t),v)); */
00357 /*     m_int=-1; */
00358     
00359 /*     if (x-3.0*m_parzenWindow < m_from.x) */
00360 /*       m_from.x = x - 3.0*m_parzenWindow.x; */
00361 /*     if (x+3.0*m_parzenWindow.x > m_to.x) */
00362 /*       m_to.x = x + 3.0*m_parzenWindow.x; */
00363 
00364 /*     if (y-3.0*m_parzenWindow < m_from.y) */
00365 /*       m_from.y = y - 3.0*m_parzenWindow.y; */
00366 /*     if (y+3.0*m_parzenWindow.y > m_to.y) */
00367 /*       m_to.y = y + 3.0*m_parzenWindow.y; */
00368 
00369 /*     if (t-3.0*m_parzenWindow < m_from.t) */
00370 /*       m_from.t = t - 3.0*m_parzenWindow.t; */
00371 /*     if (t+3.0*m_parzenWindow.t > m_to.t) */
00372 /*       m_to.t = t + 3.0*m_parzenWindow.t; */
00373 /*   } */
00374    
00375 /*   void integrate(InputPoint step) { */
00376 /*     m_lastStep = step; */
00377 /*     double sum=0; */
00378 /*     for (double x=m_from.x; x<=m_to.x; x+=step.x) { */
00379 /*       for (double y=m_from.y; x<=m_to.y; y+=step.y) { */
00380 /*      for (double t=m_from.t; t<=m_to.t; t+=step.t) { */
00381 /*        sum += smoothedData(InputPoint(x,y,t)) * step.x * step.y * step.t; */
00382 /*      } */
00383 /*       } */
00384 /*     } */
00385 /*     m_int = sum; */
00386 /*   } */
00387 
00388 
00389 /*   double smoothedData(InputPoint pnt) { */
00390 /*     assert( m_data.size() > 0 ); */
00391 /*     double p=0; */
00392 /*     double sum_y=0; */
00393 /*     for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */
00394 /*       const DataPoint& d = *it; */
00395 /*       double u = sqr( (pnt.x-d.x)/m_parzenWindow.x) +  */
00396 /*      sqr((pnt.y-d.y)/m_parzenWindow.y) +  */
00397 /*      sqr((pnt.t-d.t)/m_parzenWindow.t); */
00398 /*       p +=  d.val * exp( -0.5 * u); */
00399 /*       sum_y += d.y; */
00400 /*     } */
00401 /*     double denom = sqr(m_parzenWindow.x)*sqr(m_parzenWindow.x)*sqr(m_parzenWindow.x) * (sum_y) *  */
00402 /*       sqrt(sqr(m_parzenWindow.x) + sqr(m_parzenWindow.y) + sqr(m_parzenWindow.t)); */
00403 /*     p *= 1./denom;  */
00404     
00405 /*     return p; */
00406 /*   } */
00407 
00408 /*   double sample(const InputPoint& step) { */
00409 
00410 /*     assert( m_data.size() > 0 ); */
00411 
00412 /*     if (m_int <0 || step != m_lastStep) */
00413 /*       integrate(step); */
00414     
00415 /*     double r = sampleUniformDouble(0.0, m_int); */
00416 /*     double sum2=0; */
00417 /*     for (double x=m_from; x<=m_to; x+=step) { */
00418 /*       sum2 += smoothedData(x)*step; */
00419 /*       if (sum2 > r) */
00420 /*      return x-0.5*step; */
00421 /*     } */
00422 /*     return m_to; */
00423 /*   } */
00424   
00425 /*   void gnuplotDumpData(FILE* fp) { */
00426 /*     for (Data::const_iterator it = m_data.begin(); it != m_data.end(); it++) { */
00427 /*       const DataPoint& d = *it; */
00428 /*       fprintf(fp, "%f %f %f %f\n", d.x, d.y, d.t, d.val); */
00429 /*     } */
00430 /*   } */
00431 
00432 /*   void gnuplotDumpSmoothedData(FILE* fp, double step) { */
00433 /*     for (double x=m_from; x<=m_to; x+=step) */
00434 /*       fprintf(fp, "%f %f %f %f\n", x, ,y, t, smoothedData(x,y,t)); */
00435 /*   } */
00436 
00437 /*  protected: */
00438 /*   Data m_data; */
00439 /*   vector<double> m_intdata; */
00440 /*   double m_int; */
00441 /*   double m_lastStep; */
00442 
00443 /*   double m_parzenWindow; */
00444 /*   double m_from; */
00445 /*   double m_to; */
00446 
00447 /* }; */
00448 
00449 }
00450 
00451 #endif


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21