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


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