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 long 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 };
00146 #endif
00147