stat.cpp
Go to the documentation of this file.
1 #include <stdlib.h>
2 
3 //#include <gsl/gsl_rng.h>
4 //#include <gsl/gsl_randist.h>
5 //#include <gsl/gsl_eigen.h>
6 //#include <gsl/gsl_blas.h>
7 #include <math.h>
8 #include "gmapping/utils/stat.h"
9 
10 namespace GMapping {
11 
12 #if 0
13 
14 int sampleUniformInt(int max)
15 {
16  return (int)(max*(rand()/(RAND_MAX+1.0)));
17 }
18 
19 double sampleUniformDouble(double min, double max)
20 {
21  return min + (rand() / (double)RAND_MAX) * (max - min);
22 }
23 
24 
25 #endif
26 
27 // Draw randomly from a zero-mean Gaussian distribution, with standard
28 // deviation sigma.
29 // We use the polar form of the Box-Muller transformation, explained here:
30 // http://www.taygeta.com/random/gaussian.html
31 double pf_ran_gaussian(double sigma)
32 {
33  double x1, x2, w;
34  double r;
35 
36  do
37  {
38  do { r = drand48(); } while (r == 0.0);
39  x1 = 2.0 * r - 1.0;
40  do { r = drand48(); } while (r == 0.0);
41  x2 = 2.0 * drand48() - 1.0;
42  w = x1*x1 + x2*x2;
43  } while(w > 1.0 || w==0.0);
44 
45  return(sigma * x2 * sqrt(-2.0*log(w)/w));
46 }
47 
48 double sampleGaussian(double sigma, unsigned long int S) {
49  /*
50  static gsl_rng * r = NULL;
51  if(r==NULL) {
52  gsl_rng_env_setup();
53  r = gsl_rng_alloc (gsl_rng_default);
54  }
55  */
56  if (S!=0)
57  {
58  //gsl_rng_set(r, S);
59  srand48(S);
60  }
61  if (sigma==0)
62  return 0;
63  //return gsl_ran_gaussian (r,sigma);
64  return pf_ran_gaussian (sigma);
65 }
66 #if 0
67 
68 double evalGaussian(double sigmaSquare, double delta){
69  if (sigmaSquare<=0)
70  sigmaSquare=1e-4;
71  return exp(-.5*delta*delta/sigmaSquare)/sqrt(2*M_PI*sigmaSquare);
72 }
73 
74 #endif
75 double evalLogGaussian(double sigmaSquare, double delta){
76  if (sigmaSquare<=0)
77  sigmaSquare=1e-4;
78  return -.5*delta*delta/sigmaSquare-.5*log(2*M_PI*sigmaSquare);
79 }
80 #if 0
81 
82 
83 Covariance3 Covariance3::zero={0.,0.,0.,0.,0.,0.};
84 
86  Covariance3 r(*this);
87  r.xx+=cov.xx;
88  r.yy+=cov.yy;
89  r.tt+=cov.tt;
90  r.xy+=cov.xy;
91  r.yt+=cov.yt;
92  r.xt+=cov.xt;
93  return r;
94 }
95 
97 
99  static gsl_eigen_symmv_workspace * m_eigenspace=NULL;
100  static gsl_matrix * m_cmat=NULL;
101  static gsl_matrix * m_evec=NULL;
102  static gsl_vector * m_eval=NULL;
103  static gsl_vector * m_noise=NULL;
104  static gsl_vector * m_pnoise=NULL;
105 
106  if (m_eigenspace==NULL){
107  m_eigenspace=gsl_eigen_symmv_alloc(3);
108  m_cmat=gsl_matrix_alloc(3,3);
109  m_evec=gsl_matrix_alloc(3,3);
110  m_eval=gsl_vector_alloc(3);
111  m_noise=gsl_vector_alloc(3);
112  m_pnoise=gsl_vector_alloc(3);
113  }
114 
115  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);
116  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);
117  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);
118  gsl_eigen_symmv (m_cmat, m_eval, m_evec, m_eigenspace);
119  for (int i=0; i<3; i++){
120  eval[i]=gsl_vector_get(m_eval,i);
121  for (int j=0; j<3; j++)
122  evec[i][j]=gsl_matrix_get(m_evec,i,j);
123  }
124 }
125 
126 EigenCovariance3 EigenCovariance3::rotate(double angle) const{
127  static gsl_matrix * m_rmat=NULL;
128  static gsl_matrix * m_vmat=NULL;
129  static gsl_matrix * m_result=NULL;
130  if (m_rmat==NULL){
131  m_rmat=gsl_matrix_alloc(3,3);
132  m_vmat=gsl_matrix_alloc(3,3);
133  m_result=gsl_matrix_alloc(3,3);
134  }
135 
136  double c=cos(angle);
137  double s=sin(angle);
138  gsl_matrix_set(m_rmat,0,0, c ); gsl_matrix_set(m_rmat,0,1, -s); gsl_matrix_set(m_rmat,0,2, 0.);
139  gsl_matrix_set(m_rmat,1,0, s ); gsl_matrix_set(m_rmat,1,1, c); gsl_matrix_set(m_rmat,1,2, 0.);
140  gsl_matrix_set(m_rmat,2,0, 0.); gsl_matrix_set(m_rmat,2,1, 0.); gsl_matrix_set(m_rmat,2,2, 1.);
141 
142  for (unsigned int i=0; i<3; i++)
143  for (unsigned int j=0; j<3; j++)
144  gsl_matrix_set(m_vmat,i,j,evec[i][j]);
145  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1., m_rmat, m_vmat, 0., m_result);
146  EigenCovariance3 ecov(*this);
147  for (int i=0; i<3; i++){
148  for (int j=0; j<3; j++)
149  ecov.evec[i][j]=gsl_matrix_get(m_result,i,j);
150  }
151  return ecov;
152 }
153 
155  static gsl_matrix * m_evec=NULL;
156  static gsl_vector * m_noise=NULL;
157  static gsl_vector * m_pnoise=NULL;
158  if (m_evec==NULL){
159  m_evec=gsl_matrix_alloc(3,3);
160  m_noise=gsl_vector_alloc(3);
161  m_pnoise=gsl_vector_alloc(3);
162  }
163  for (int i=0; i<3; i++){
164  for (int j=0; j<3; j++)
165  gsl_matrix_set(m_evec,i,j, evec[i][j]);
166  }
167  for (int i=0; i<3; i++){
168  double v=sampleGaussian(sqrt(eval[i]));
169  if(isnan(v))
170  v=0;
171  gsl_vector_set(m_pnoise,i, v);
172  }
173  gsl_blas_dgemv (CblasNoTrans, 1., m_evec, m_pnoise, 0, m_noise);
174  OrientedPoint ret(gsl_vector_get(m_noise,0),gsl_vector_get(m_noise,1),gsl_vector_get(m_noise,2));
175  ret.theta=atan2(sin(ret.theta), cos(ret.theta));
176  return ret;
177 }
178 
179 #endif
180 
181 double Gaussian3::eval(const OrientedPoint& p) const{
182  OrientedPoint q=p-mean;
183  q.theta=atan2(sin(p.theta-mean.theta),cos(p.theta-mean.theta));
184  double v1,v2,v3;
185  v1 = covariance.evec[0][0]*q.x+covariance.evec[1][0]*q.y+covariance.evec[2][0]*q.theta;
186  v2 = covariance.evec[0][1]*q.x+covariance.evec[1][1]*q.y+covariance.evec[2][1]*q.theta;
187  v3 = covariance.evec[0][2]*q.x+covariance.evec[1][2]*q.y+covariance.evec[2][2]*q.theta;
188  return evalLogGaussian(covariance.eval[0], v1)+evalLogGaussian(covariance.eval[1], v2)+evalLogGaussian(covariance.eval[2], v3);
189 }
190 
191 #if 0
192 void Gaussian3::computeFromSamples(const std::vector<OrientedPoint> & poses, const std::vector<double>& weights ){
193  OrientedPoint mean=OrientedPoint(0,0,0);
194  double wcum=0;
195  double s=0, c=0;
196  std::vector<double>::const_iterator w=weights.begin();
197  for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
198  s+=*w*sin(p->theta);
199  c+=*w*cos(p->theta);
200  mean.x+=*w*p->x;
201  mean.y+=*w*p->y;
202  wcum+=*w;
203  w++;
204  }
205  mean.x/=wcum;
206  mean.y/=wcum;
207  s/=wcum;
208  c/=wcum;
209  mean.theta=atan2(s,c);
210 
212  w=weights.begin();
213  for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
214  OrientedPoint delta=(*p)-mean;
215  delta.theta=atan2(sin(delta.theta),cos(delta.theta));
216  cov.xx+=*w*delta.x*delta.x;
217  cov.yy+=*w*delta.y*delta.y;
218  cov.tt+=*w*delta.theta*delta.theta;
219  cov.xy+=*w*delta.x*delta.y;
220  cov.yt+=*w*delta.y*delta.theta;
221  cov.xt+=*w*delta.x*delta.theta;
222  w++;
223  }
224  cov.xx/=wcum;
225  cov.yy/=wcum;
226  cov.tt/=wcum;
227  cov.xy/=wcum;
228  cov.yt/=wcum;
229  cov.xt/=wcum;
230  EigenCovariance3 ecov(cov);
231  this->mean=mean;
232  this->covariance=ecov;
233  this->cov=cov;
234 }
235 
236 void Gaussian3::computeFromSamples(const std::vector<OrientedPoint> & poses){
237  OrientedPoint mean=OrientedPoint(0,0,0);
238  double wcum=1;
239  double s=0, c=0;
240  for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
241  s+=sin(p->theta);
242  c+=cos(p->theta);
243  mean.x+=p->x;
244  mean.y+=p->y;
245  wcum+=1.;
246  }
247  mean.x/=wcum;
248  mean.y/=wcum;
249  s/=wcum;
250  c/=wcum;
251  mean.theta=atan2(s,c);
252 
254  for (std::vector<OrientedPoint>::const_iterator p=poses.begin(); p!=poses.end(); p++){
255  OrientedPoint delta=(*p)-mean;
256  delta.theta=atan2(sin(delta.theta),cos(delta.theta));
257  cov.xx+=delta.x*delta.x;
258  cov.yy+=delta.y*delta.y;
259  cov.tt+=delta.theta*delta.theta;
260  cov.xy+=delta.x*delta.y;
261  cov.yt+=delta.y*delta.theta;
262  cov.xt+=delta.x*delta.theta;
263  }
264  cov.xx/=wcum;
265  cov.yy/=wcum;
266  cov.tt/=wcum;
267  cov.xy/=wcum;
268  cov.yt/=wcum;
269  cov.xt/=wcum;
270  EigenCovariance3 ecov(cov);
271  this->mean=mean;
272  this->covariance=ecov;
273  this->cov=cov;
274 }
275 #endif
276 
277 }// end namespace
278 
const char *const *argv double delta
Definition: gfs2stream.cpp:19
double UTILS_EXPORT sampleUniformDouble(double min, double max)
point< T > min(const point< T > &p1, const point< T > &p2)
Definition: point.h:154
Covariance3 operator+(const Covariance3 &cov) const
OrientedPoint sample() const
static Covariance3 zero
Definition: stat.h:21
double UTILS_EXPORT sampleGaussian(double sigma, unsigned long int S=0)
Definition: stat.cpp:48
point< T > max(const point< T > &p1, const point< T > &p2)
Definition: point.h:146
double UTILS_EXPORT evalGaussian(double sigmaSquare, double delta)
void computeFromSamples(const std::vector< OrientedPoint > &poses)
double UTILS_EXPORT eval(const OrientedPoint &p) const
Definition: stat.cpp:181
unsigned int c
Definition: gfs2stream.cpp:41
double UTILS_EXPORT evalLogGaussian(double sigmaSquare, double delta)
Definition: stat.cpp:75
double evec[3][3]
Definition: stat.h:31
int UTILS_EXPORT sampleUniformInt(int max)
double pf_ran_gaussian(double sigma)
Definition: stat.cpp:31
orientedpoint< double, double > OrientedPoint
Definition: point.h:203
EigenCovariance3 rotate(double angle) const


openslam_gmapping
Author(s): Cyrill Stachniss, Udo Frese, Giorgio Grisetti, Wolfram Burgard
autogenerated on Mon Feb 28 2022 22:59:20