stat.cpp
Go to the documentation of this file.
00001 #include <stdlib.h>
00002 
00003 //#include <gsl/gsl_rng.h>
00004 //#include <gsl/gsl_randist.h>
00005 //#include <gsl/gsl_eigen.h>
00006 //#include <gsl/gsl_blas.h>
00007 #include <math.h>
00008 #include <gmapping/utils/gvalues.h>
00009 #include <gmapping/utils/stat.h>
00010 
00011 namespace GMapping {
00012 
00013 #if 0
00014 
00015 int sampleUniformInt(int max)
00016 {
00017   return (int)(max*(rand()/(RAND_MAX+1.0)));
00018 }
00019 
00020 double sampleUniformDouble(double min, double max)
00021 {
00022   return min + (rand() / (double)RAND_MAX) * (max - min);
00023 }
00024 
00025 
00026 #endif
00027 
00028 // Draw randomly from a zero-mean Gaussian distribution, with standard
00029 // deviation sigma.
00030 // We use the polar form of the Box-Muller transformation, explained here:
00031 //   http://www.taygeta.com/random/gaussian.html
00032 double pf_ran_gaussian(double sigma)
00033 {
00034   double x1, x2, w;
00035   double r;
00036 
00037   do
00038   {
00039     do { r = drand48(); } while (r == 0.0);
00040     x1 = 2.0 * r - 1.0;
00041     do { r = drand48(); } while (r == 0.0);
00042     x2 = 2.0 * drand48() - 1.0;
00043     w = x1*x1 + x2*x2;
00044   } while(w > 1.0 || w==0.0);
00045 
00046   return(sigma * x2 * sqrt(-2.0*log(w)/w));
00047 }
00048 
00049 double sampleGaussian(double sigma, unsigned int S) {
00050   /*
00051         static gsl_rng * r = NULL;
00052         if(r==NULL) {
00053                 gsl_rng_env_setup();
00054                 r = gsl_rng_alloc (gsl_rng_default);
00055         }
00056         */
00057         if (S!=0)
00058         {
00059                 //gsl_rng_set(r, S);
00060                 srand(S);
00061         }
00062         if (sigma==0)
00063                 return 0;
00064         //return gsl_ran_gaussian (r,sigma);
00065         return pf_ran_gaussian (sigma);
00066 }
00067 #if 0
00068 
00069 double evalGaussian(double sigmaSquare, double delta){
00070         if (sigmaSquare<=0)
00071                 sigmaSquare=1e-4;
00072         return exp(-.5*delta*delta/sigmaSquare)/sqrt(2*M_PI*sigmaSquare);
00073 }
00074 
00075 #endif
00076 double evalLogGaussian(double sigmaSquare, double delta){
00077         if (sigmaSquare<=0)
00078                 sigmaSquare=1e-4;
00079         return -.5*delta*delta/sigmaSquare-.5*log(2*M_PI*sigmaSquare);
00080 }
00081 #if 0
00082 
00083 
00084 Covariance3 Covariance3::zero={0.,0.,0.,0.,0.,0.};
00085 
00086 Covariance3 Covariance3::operator + (const Covariance3 & cov) const{
00087         Covariance3 r(*this);
00088         r.xx+=cov.xx;
00089         r.yy+=cov.yy;
00090         r.tt+=cov.tt;
00091         r.xy+=cov.xy;
00092         r.yt+=cov.yt;
00093         r.xt+=cov.xt;
00094         return r;
00095 }
00096 
00097 EigenCovariance3::EigenCovariance3(){}
00098 
00099 EigenCovariance3::EigenCovariance3(const Covariance3& cov){
00100         static gsl_eigen_symmv_workspace * m_eigenspace=NULL;
00101         static gsl_matrix * m_cmat=NULL;
00102         static gsl_matrix * m_evec=NULL;
00103         static gsl_vector * m_eval=NULL;
00104         static gsl_vector * m_noise=NULL;
00105         static gsl_vector * m_pnoise=NULL;
00106         
00107         if (m_eigenspace==NULL){
00108                 m_eigenspace=gsl_eigen_symmv_alloc(3);
00109                 m_cmat=gsl_matrix_alloc(3,3);
00110                 m_evec=gsl_matrix_alloc(3,3);
00111                 m_eval=gsl_vector_alloc(3);
00112                 m_noise=gsl_vector_alloc(3);
00113                 m_pnoise=gsl_vector_alloc(3);
00114         }
00115 
00116         gsl_matrix_set(m_cmat,0,0,cov.xx); gsl_matrix_set(m_cmat,0,1,cov.xy); gsl_matrix_set(m_cmat,0,2,cov.xt);
00117         gsl_matrix_set(m_cmat,1,0,cov.xy); gsl_matrix_set(m_cmat,1,1,cov.yy); gsl_matrix_set(m_cmat,1,2,cov.yt);
00118         gsl_matrix_set(m_cmat,2,0,cov.xt); gsl_matrix_set(m_cmat,2,1,cov.yt); gsl_matrix_set(m_cmat,2,2,cov.tt);
00119         gsl_eigen_symmv (m_cmat, m_eval,  m_evec, m_eigenspace);
00120         for (int i=0; i<3; i++){
00121                 eval[i]=gsl_vector_get(m_eval,i);
00122                 for (int j=0; j<3; j++)
00123                         evec[i][j]=gsl_matrix_get(m_evec,i,j);
00124         }
00125 }
00126 
00127 EigenCovariance3 EigenCovariance3::rotate(double angle) const{
00128         static gsl_matrix * m_rmat=NULL;
00129         static gsl_matrix * m_vmat=NULL;
00130         static gsl_matrix * m_result=NULL;
00131         if (m_rmat==NULL){
00132                 m_rmat=gsl_matrix_alloc(3,3);
00133                 m_vmat=gsl_matrix_alloc(3,3);
00134                 m_result=gsl_matrix_alloc(3,3);
00135         }
00136         
00137         double c=cos(angle);
00138         double s=sin(angle);
00139         gsl_matrix_set(m_rmat,0,0, c ); gsl_matrix_set(m_rmat,0,1, -s); gsl_matrix_set(m_rmat,0,2, 0.);
00140         gsl_matrix_set(m_rmat,1,0, s ); gsl_matrix_set(m_rmat,1,1,  c); gsl_matrix_set(m_rmat,1,2, 0.);
00141         gsl_matrix_set(m_rmat,2,0, 0.); gsl_matrix_set(m_rmat,2,1, 0.); gsl_matrix_set(m_rmat,2,2, 1.);
00142         
00143         for (unsigned int i=0; i<3; i++)
00144                 for (unsigned int j=0; j<3; j++)
00145                         gsl_matrix_set(m_vmat,i,j,evec[i][j]);
00146         gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1., m_rmat, m_vmat, 0., m_result);
00147         EigenCovariance3 ecov(*this);
00148         for (int i=0; i<3; i++){
00149                 for (int j=0; j<3; j++)
00150                         ecov.evec[i][j]=gsl_matrix_get(m_result,i,j);
00151         }
00152         return ecov;
00153 }
00154 
00155 OrientedPoint EigenCovariance3::sample() const{
00156         static gsl_matrix * m_evec=NULL;
00157         static gsl_vector * m_noise=NULL;
00158         static gsl_vector * m_pnoise=NULL;
00159         if (m_evec==NULL){
00160                 m_evec=gsl_matrix_alloc(3,3);
00161                 m_noise=gsl_vector_alloc(3);
00162                 m_pnoise=gsl_vector_alloc(3);
00163         }
00164         for (int i=0; i<3; i++){
00165                 for (int j=0; j<3; j++)
00166                         gsl_matrix_set(m_evec,i,j, evec[i][j]);
00167         }
00168         for (int i=0; i<3; i++){
00169                 double v=sampleGaussian(sqrt(eval[i]));
00170                 if(isnan(v))
00171                         v=0;
00172                 gsl_vector_set(m_pnoise,i, v);
00173         }
00174         gsl_blas_dgemv (CblasNoTrans, 1., m_evec, m_pnoise, 0, m_noise);
00175         OrientedPoint ret(gsl_vector_get(m_noise,0),gsl_vector_get(m_noise,1),gsl_vector_get(m_noise,2));
00176         ret.theta=atan2(sin(ret.theta), cos(ret.theta));
00177         return ret;
00178 }
00179 
00180 #endif
00181 
00182 double Gaussian3::eval(const OrientedPoint& p) const{
00183         OrientedPoint q=p-mean;
00184         q.theta=atan2(sin(p.theta-mean.theta),cos(p.theta-mean.theta));
00185         double v1,v2,v3;
00186         v1 = covariance.evec[0][0]*q.x+covariance.evec[1][0]*q.y+covariance.evec[2][0]*q.theta;
00187         v2 = covariance.evec[0][1]*q.x+covariance.evec[1][1]*q.y+covariance.evec[2][1]*q.theta;
00188         v3 = covariance.evec[0][2]*q.x+covariance.evec[1][2]*q.y+covariance.evec[2][2]*q.theta;
00189         return evalLogGaussian(covariance.eval[0], v1)+evalLogGaussian(covariance.eval[1], v2)+evalLogGaussian(covariance.eval[2], v3);
00190 }
00191 
00192 #if 0
00193 void Gaussian3::computeFromSamples(const std::vector<OrientedPoint> & poses, const std::vector<double>& weights ){
00194         OrientedPoint mean=OrientedPoint(0,0,0);
00195         double wcum=0;
00196         double s=0, c=0;
00197         std::vector<double>::const_iterator w=weights.begin();
00198         for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
00199                 s+=*w*sin(p->theta);
00200                 c+=*w*cos(p->theta);
00201                 mean.x+=*w*p->x;
00202                 mean.y+=*w*p->y;
00203                 wcum+=*w;
00204                 w++;
00205         }
00206         mean.x/=wcum;
00207         mean.y/=wcum;
00208         s/=wcum;
00209         c/=wcum;
00210         mean.theta=atan2(s,c);
00211 
00212         Covariance3 cov=Covariance3::zero;
00213         w=weights.begin();
00214         for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
00215                 OrientedPoint delta=(*p)-mean;
00216                 delta.theta=atan2(sin(delta.theta),cos(delta.theta));
00217                 cov.xx+=*w*delta.x*delta.x;
00218                 cov.yy+=*w*delta.y*delta.y;
00219                 cov.tt+=*w*delta.theta*delta.theta;
00220                 cov.xy+=*w*delta.x*delta.y;
00221                 cov.yt+=*w*delta.y*delta.theta;
00222                 cov.xt+=*w*delta.x*delta.theta;
00223                 w++;
00224         }
00225         cov.xx/=wcum;
00226         cov.yy/=wcum;
00227         cov.tt/=wcum;
00228         cov.xy/=wcum;
00229         cov.yt/=wcum;
00230         cov.xt/=wcum;
00231         EigenCovariance3 ecov(cov);
00232         this->mean=mean;
00233         this->covariance=ecov;
00234         this->cov=cov;
00235 }
00236 
00237 void Gaussian3::computeFromSamples(const std::vector<OrientedPoint> & poses){
00238         OrientedPoint mean=OrientedPoint(0,0,0);
00239         double wcum=1;
00240         double s=0, c=0;
00241         for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
00242                 s+=sin(p->theta);
00243                 c+=cos(p->theta);
00244                 mean.x+=p->x;
00245                 mean.y+=p->y;
00246                 wcum+=1.;
00247         }
00248         mean.x/=wcum;
00249         mean.y/=wcum;
00250         s/=wcum;
00251         c/=wcum;
00252         mean.theta=atan2(s,c);
00253 
00254         Covariance3 cov=Covariance3::zero;
00255         for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
00256                 OrientedPoint delta=(*p)-mean;
00257                 delta.theta=atan2(sin(delta.theta),cos(delta.theta));
00258                 cov.xx+=delta.x*delta.x;
00259                 cov.yy+=delta.y*delta.y;
00260                 cov.tt+=delta.theta*delta.theta;
00261                 cov.xy+=delta.x*delta.y;
00262                 cov.yt+=delta.y*delta.theta;
00263                 cov.xt+=delta.x*delta.theta;
00264         }
00265         cov.xx/=wcum;
00266         cov.yy/=wcum;
00267         cov.tt/=wcum;
00268         cov.xy/=wcum;
00269         cov.yt/=wcum;
00270         cov.xt/=wcum;
00271         EigenCovariance3 ecov(cov);
00272         this->mean=mean;
00273         this->covariance=ecov;
00274         this->cov=cov;
00275 }
00276 #endif
00277 
00278 }// end namespace
00279 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Thu Jun 6 2019 19:25:13