stat.h
Go to the documentation of this file.
1 #ifndef STAT_H
2 #define STAT_H
3 #include "point.h"
4 #include <vector>
5 #include "gvalues.h"
6
7 namespace GMapping {
8
11 double sampleGaussian(double sigma,unsigned int S=0);
12
13 double evalGaussian(double sigmaSquare, double delta);
14 double evalLogGaussian(double sigmaSquare, double delta);
15 int sampleUniformInt(int max);
16 double sampleUniformDouble(double min, double max);
17
18 struct Covariance3{
19  Covariance3 operator + (const Covariance3 & cov) const;
20  static Covariance3 zero;
21  double xx, yy, tt, xy, xt, yt;
22 };
23
27  EigenCovariance3 rotate(double angle) const;
28  OrientedPoint sample() const;
29  double eval[3];
30  double evec[3][3];
31 };
32
33 struct Gaussian3{
37  double eval(const OrientedPoint& p) const;
38  void computeFromSamples(const std::vector<OrientedPoint> & poses);
39  void computeFromSamples(const std::vector<OrientedPoint> & poses, const std::vector<double>& weights );
40 };
41
42 template<typename PointIterator, typename WeightIterator>
43 Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd, WeightIterator& weightBegin, WeightIterator& weightEnd){
44  Gaussian3 gaussian;
45  OrientedPoint mean=OrientedPoint(0,0,0);
46  double wcum=0;
47  double s=0, c=0;
48  WeightIterator wt=weightBegin;
49  double *w=new double();
51  for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
52  *w=*wt;
53  *p=*pt;
54  s+=*w*sin(p->theta);
55  c+=*w*cos(p->theta);
56  mean.x+=*w*p->x;
57  mean.y+=*w*p->y;
58  wcum+=*w;
59  wt++;
60  }
61  mean.x/=wcum;
62  mean.y/=wcum;
63  s/=wcum;
64  c/=wcum;
65  mean.theta=atan2(s,c);
66
68  wt=weightBegin;
69  for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
70  *w=*wt;
71  *p=*pt;
72  OrientedPoint delta=(*p)-mean;
73  delta.theta=atan2(sin(delta.theta),cos(delta.theta));
74  cov.xx+=*w*delta.x*delta.x;
75  cov.yy+=*w*delta.y*delta.y;
76  cov.tt+=*w*delta.theta*delta.theta;
77  cov.xy+=*w*delta.x*delta.y;
78  cov.yt+=*w*delta.y*delta.theta;
79  cov.xt+=*w*delta.x*delta.theta;
80  wt++;
81  }
82  cov.xx/=wcum;
83  cov.yy/=wcum;
84  cov.tt/=wcum;
85  cov.xy/=wcum;
86  cov.yt/=wcum;
87  cov.xt/=wcum;
88  EigenCovariance3 ecov(cov);
89  gaussian.mean=mean;
90  gaussian.covariance=ecov;
91  gaussian.cov=cov;
92  delete w;
93  delete p;
94  return gaussian;
95 }
96
97 template<typename PointIterator>
98 Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd){
99  Gaussian3 gaussian;
100  OrientedPoint mean=OrientedPoint(0,0,0);
101  double wcum=1;
102  double s=0, c=0;
103  OrientedPoint *p=new OrientedPoint();
104  for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
105  *p=*pt;
106  s+=sin(p->theta);
107  c+=cos(p->theta);
108  mean.x+=p->x;
109  mean.y+=p->y;
110  wcum+=1.;
111  }
112  mean.x/=wcum;
113  mean.y/=wcum;
114  s/=wcum;
115  c/=wcum;
116  mean.theta=atan2(s,c);
117
119  for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
120  *p=*pt;
121  OrientedPoint delta=(*p)-mean;
122  delta.theta=atan2(sin(delta.theta),cos(delta.theta));
123  cov.xx+=delta.x*delta.x;
124  cov.yy+=delta.y*delta.y;
125  cov.tt+=delta.theta*delta.theta;
126  cov.xy+=delta.x*delta.y;
127  cov.yt+=delta.y*delta.theta;
128  cov.xt+=delta.x*delta.theta;
129  }
130  cov.xx/=wcum;
131  cov.yy/=wcum;
132  cov.tt/=wcum;
133  cov.xy/=wcum;
134  cov.yt/=wcum;
135  cov.xt/=wcum;
136  EigenCovariance3 ecov(cov);
137  gaussian.mean=mean;
138  gaussian.covariance=ecov;
139  gaussian.cov=cov;
140  delete p;
141  return gaussian;
142 }
143
144
145 }; //end namespace
146 #endif
147
const char *const *argv double delta
Definition: gfs2stream.cpp:19
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)
OrientedPoint mean
Definition: stat.h:34
Gaussian3 computeGaussianFromSamples(PointIterator &pointBegin, PointIterator &pointEnd, WeightIterator &weightBegin, WeightIterator &weightEnd)
Definition: stat.h:43
double sampleGaussian(double sigma, unsigned int S=0)
Definition: stat.cpp:49
static Covariance3 zero
Definition: stat.h:20
double rotate
Definition: gfs2stream.cpp:21
Covariance3 cov
Definition: stat.h:36
point< T > max(const point< T > &p1, const point< T > &p2)
Definition: point.h:146
Covariance3 operator+(const Covariance3 &cov) const
unsigned int c
Definition: gfs2stream.cpp:41
int sampleUniformInt(int max)
double evalLogGaussian(double sigmaSquare, double delta)
Definition: stat.cpp:76
EigenCovariance3 covariance
Definition: stat.h:35
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