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

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