stat.h
Go to the documentation of this file.
00001 #ifndef STAT_H
00002 #define STAT_H
00003 #include "point.h"
00004 #include <vector>
00005 #include "gvalues.h"
00006 
00007 namespace GMapping {
00008 
00011 double sampleGaussian(double sigma,unsigned int S=0);
00012 
00013 double evalGaussian(double sigmaSquare, double delta);
00014 double evalLogGaussian(double sigmaSquare, double delta);
00015 int sampleUniformInt(int max);
00016 double sampleUniformDouble(double min, double max);
00017 
00018 struct Covariance3{
00019         Covariance3 operator + (const Covariance3 & cov) const;
00020         static Covariance3 zero;
00021         double xx, yy, tt, xy, xt, yt;
00022 };
00023 
00024 struct EigenCovariance3{
00025         EigenCovariance3();
00026         EigenCovariance3(const Covariance3& c);
00027         EigenCovariance3 rotate(double angle) const;
00028         OrientedPoint sample() const;
00029         double eval[3];
00030         double evec[3][3];
00031 };
00032 
00033 struct Gaussian3{
00034         OrientedPoint mean;
00035         EigenCovariance3 covariance;
00036         Covariance3 cov;
00037         double eval(const OrientedPoint& p) const;
00038         void computeFromSamples(const std::vector<OrientedPoint> & poses);
00039         void computeFromSamples(const std::vector<OrientedPoint> & poses, const std::vector<double>& weights );
00040 };
00041 
00042 template<typename PointIterator, typename WeightIterator>
00043 Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd, WeightIterator& weightBegin, WeightIterator& weightEnd){
00044         Gaussian3 gaussian;
00045         OrientedPoint mean=OrientedPoint(0,0,0);
00046         double wcum=0;
00047         double s=0, c=0;
00048         WeightIterator wt=weightBegin;
00049         double *w=new double();
00050         OrientedPoint *p=new OrientedPoint();
00051         for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
00052                 *w=*wt;
00053                 *p=*pt;
00054                 s+=*w*sin(p->theta);
00055                 c+=*w*cos(p->theta);
00056                 mean.x+=*w*p->x;
00057                 mean.y+=*w*p->y;
00058                 wcum+=*w;
00059                 wt++;
00060         }
00061         mean.x/=wcum;
00062         mean.y/=wcum;
00063         s/=wcum;
00064         c/=wcum;
00065         mean.theta=atan2(s,c);
00066 
00067         Covariance3 cov=Covariance3::zero;
00068         wt=weightBegin;
00069         for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
00070                 *w=*wt;
00071                 *p=*pt;
00072                 OrientedPoint delta=(*p)-mean;
00073                 delta.theta=atan2(sin(delta.theta),cos(delta.theta));
00074                 cov.xx+=*w*delta.x*delta.x;
00075                 cov.yy+=*w*delta.y*delta.y;
00076                 cov.tt+=*w*delta.theta*delta.theta;
00077                 cov.xy+=*w*delta.x*delta.y;
00078                 cov.yt+=*w*delta.y*delta.theta;
00079                 cov.xt+=*w*delta.x*delta.theta;
00080                 wt++;
00081         }
00082         cov.xx/=wcum;
00083         cov.yy/=wcum;
00084         cov.tt/=wcum;
00085         cov.xy/=wcum;
00086         cov.yt/=wcum;
00087         cov.xt/=wcum;
00088         EigenCovariance3 ecov(cov);
00089         gaussian.mean=mean;
00090         gaussian.covariance=ecov;
00091         gaussian.cov=cov;
00092         delete w;
00093         delete p;
00094         return gaussian;
00095 }
00096 
00097 template<typename PointIterator>
00098 Gaussian3 computeGaussianFromSamples(PointIterator& pointBegin, PointIterator& pointEnd){
00099         Gaussian3 gaussian;
00100         OrientedPoint mean=OrientedPoint(0,0,0);
00101         double wcum=1;
00102         double s=0, c=0;
00103         OrientedPoint *p=new OrientedPoint();
00104         for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
00105                 *p=*pt;
00106                 s+=sin(p->theta);
00107                 c+=cos(p->theta);
00108                 mean.x+=p->x;
00109                 mean.y+=p->y;
00110                 wcum+=1.;
00111         }
00112         mean.x/=wcum;
00113         mean.y/=wcum;
00114         s/=wcum;
00115         c/=wcum;
00116         mean.theta=atan2(s,c);
00117 
00118         Covariance3 cov=Covariance3::zero;
00119         for (PointIterator pt=pointBegin; pt!=pointEnd; pt++){
00120                 *p=*pt;
00121                 OrientedPoint delta=(*p)-mean;
00122                 delta.theta=atan2(sin(delta.theta),cos(delta.theta));
00123                 cov.xx+=delta.x*delta.x;
00124                 cov.yy+=delta.y*delta.y;
00125                 cov.tt+=delta.theta*delta.theta;
00126                 cov.xy+=delta.x*delta.y;
00127                 cov.yt+=delta.y*delta.theta;
00128                 cov.xt+=delta.x*delta.theta;
00129         }
00130         cov.xx/=wcum;
00131         cov.yy/=wcum;
00132         cov.tt/=wcum;
00133         cov.xy/=wcum;
00134         cov.yt/=wcum;
00135         cov.xt/=wcum;
00136         EigenCovariance3 ecov(cov);
00137         gaussian.mean=mean;
00138         gaussian.covariance=ecov;
00139         gaussian.cov=cov;
00140         delete p;
00141         return gaussian;
00142 }
00143 
00144 
00145 }; //end namespace
00146 #endif
00147 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21