Go to the documentation of this file.00001
00019 #ifndef C_PARTICLE_FILTER_H_
00020 #define C_PARTICLE_FILTER_H_
00021
00022 #include <stdio.h>
00023 #include <stdlib.h>
00024 #include <math.h>
00025 #include <time.h>
00026 #include <string.h>
00027 #include <vector>
00028 #include <Eigen/Core>
00029
00035 class ownRandom{
00036 public:
00037 float *normalRandomCache;
00038 bool isOk;
00039 int size;
00040
00041 ownRandom(){
00042 isOk=false;
00043 size = 0;
00044 allocate(10000);
00045 srand( (unsigned)time( NULL ) );
00046 }
00047 ~ownRandom(){
00048
00049 }
00051 void allocate(int m_size){
00052 if(size != 0){
00053 if(normalRandomCache)free(normalRandomCache);
00054 }
00055
00056 normalRandomCache = (float *) malloc(sizeof(float)*m_size);
00057 isOk=true;
00058 size = m_size;
00059 }
00060
00061 void fillCache(){
00062 if(size > 0 && normalRandomCache != NULL){
00063 for(int i=0;i<size;i++){
00064 normalRandomCache[i] = normalRandom();
00065 isOk = true;
00066 }
00067 }else{
00068 allocate(10000);
00069 fillCache();
00070 }
00071 }
00076 float getCachedUniformRandom(){
00077 int rv;
00078 float scale;
00079
00080 if(!isOk) fillCache();
00081
00082 scale = (float) size / (float)RAND_MAX;
00083 rv = rand();
00084 rv = (int) ((float) rv * scale);
00085 if(rv >=size){
00086 fprintf(stderr,"ERROR in getCachedUniformRandom(). rv=%d\n",rv);
00087 rv = size-1;
00088 }
00089 return normalRandomCache[rv];
00090 }
00091
00095 float uniformRandom(void){
00096 return ( ((float) rand() / (float) RAND_MAX));
00097 }
00098
00103 float normalRandom()
00104 {
00105 static float gset;
00106 static int randomStored = 0;
00107 float fac,rsq,v1,v2;
00108
00109 if(randomStored){
00110 return gset;
00111 }
00112
00113 do {
00114 v1=2.0*uniformRandom()-1.0;
00115
00116 v2=2.0*uniformRandom()-1.0;
00117 rsq=v1*v1+v2*v2;
00118 } while (rsq >= 1.0 || rsq == 0.0);
00119
00120 fac=sqrt(-2.0*log(rsq)/rsq);
00121 gset=v1*fac;
00122 return v2*fac;
00123 }
00124
00132 void covRandom(float& xRand, float& yRand, float cov[3]){
00133 float a,b,c;
00134 float eig1,eig2;
00135 float vx1,vy1,vx2,vy2;
00136 float d;
00137 float x,y;
00138
00139 xRand = normalRandom();
00140 yRand = normalRandom();
00141
00142 a = 1.0;
00143 b = -(cov[0]+cov[2]);
00144 c = cov[0]*cov[2] - cov[1]*cov[1];
00145
00146 eig1 = (-b + sqrt(b*b-4*a*c))/(2*a);
00147 eig2 = (-b - sqrt(b*b-4*a*c))/(2*a);
00148
00149
00150 vx1 = cov[1];
00151 vy1 = eig1 - cov[0];
00152 d = sqrt(vx1*vx1+vy1+vy1);
00153 vx1 /= d;
00154 vy1 /= d;
00155
00156 vx2 = cov[1];
00157 vy2 = eig2 - cov[0];
00158 d = sqrt(vx2*vx2+vy2+vy2);
00159 vx2 /= d;
00160 vy2 /= d;
00161
00162
00163 x = xRand * sqrt(eig1);
00164 y = yRand * sqrt(eig2);
00165
00166 xRand = x * vx1 + y * vx2;
00167 yRand = x * vy1 + y * vy2;
00168 }
00169 };
00170
00171
00172
00178 namespace mcl{
00184 struct scan{
00185 public:
00186 float *r;
00187 float *a;
00188 int N;
00189 scan(){
00190 N=0;
00191 r=NULL;
00192 a=NULL;
00193 }
00194 scan(const scan &s) {
00195 *this = s;
00196 }
00197 scan &operator=(const scan &s) {
00198 s.copy(*this);
00199 return *this;
00200 }
00201
00202
00203
00204
00205
00206 void allocate(int n){
00207 if(r) free(r);
00208 if(a) free(a);
00209 r = (float *) malloc(n*sizeof(float));
00210 a = (float *) malloc(n*sizeof(float));
00211 N=n;
00212 }
00213 void copy(struct scan &new_scan) const {
00214 new_scan.r = (float *) malloc(N*sizeof(float));
00215 new_scan.a = (float *) malloc(N*sizeof(float));
00216 new_scan.N = N;
00217 memcpy(new_scan.r,r,N*sizeof(float));
00218 memcpy(new_scan.a,a,N*sizeof(float));
00219 }
00220 void set(float *rr,float *aa){
00221 if(N<=0) return;
00222 for(int i=0;i<N;i++){
00223 r[i]=rr[i];
00224 a[i]=aa[i];
00225 }
00226 }
00227 };
00232 struct pose{
00233 public:
00234 float x;
00235 float y;
00236 float a;
00237 pose(){
00238 x=0;
00239 y=0;
00240 a=0;
00241 }
00242 pose(float _x,float _y,float _a){
00243 x=_x;y=_y;a=_a;
00244 }
00245 void set(pose &p){
00246 x=p.x;
00247 y=p.y;
00248 a=p.a;
00249 }
00250 void set(float xx,float yy, float aa){
00251 x=xx;
00252 y=yy;
00253 a=aa;
00254 }
00259 void setToDifferentialPose(pose odo_cur,pose odo_ref){
00260 float ddx,ddy,dist,alpha;
00261 pose tmp;
00263 ddx = odo_cur.x - odo_ref.x;
00264 ddy = odo_cur.y - odo_ref.y;
00265 alpha = atan2(ddy, ddx);
00266 dist = sqrt(ddx*ddx+ddy*ddy);
00267
00268 tmp.x = dist * cos( alpha - odo_ref.a );
00269 tmp.y = dist * sin( alpha - odo_ref.a );
00270 tmp.a = (float)fmod((float)(odo_cur.a - odo_ref.a), (float)(2.0*M_PI));
00271 if(tmp.a < 0) tmp.a += 2*(float)M_PI;
00272 x = tmp.x;
00273 y= tmp.y;
00274 a = tmp.a;
00275
00276 }
00282 const pose integrateDifferential(const pose diff){
00283 pose result;
00284 float l,phii;
00285
00286 l = sqrt(diff.x*diff.x + diff.y*diff.y);
00287 phii = atan2(diff.y,diff.x);
00288
00289 result.x = x + (float)cos(a+phii)*l;
00290 result.y = y + (float)sin(a+phii)*l;
00291
00292
00293 result.a = (float)fmod((float)(diff.a + a),(float)( 2*M_PI));
00294
00295 if(result.a < 0) result.a += 2*(float)M_PI;
00296 return result;
00297 }
00298 void to2PI(){
00299 a = (float)fmod((float)(a),(float)( 2*M_PI));
00300 if(a < 0) a += 2*(float)M_PI;
00301 }
00302 void toPI(){
00303 int cnt=0;
00304 if(a>M_PI) while(a>M_PI){
00305 a-=2.0*M_PI;
00306 cnt++;
00307
00308 }
00309 else if(a<-M_PI) while(a<-M_PI){
00310 a+=2.0*M_PI;
00311 cnt++;
00312
00313 }
00314 }
00315
00316 ~pose(){}
00317
00318 };
00319
00320
00326 struct TPoseParticle{
00327 public:
00328 float x;
00329 float y;
00330 float a;
00331 float p;
00332 float lik;
00333 TPoseParticle(){
00334 x=0;
00335 y=0;
00336 a=0;
00337 p=0;
00338 lik=0;
00339 }
00340 void to2PI(){
00341 a = (float)fmod((float)(a),(float)( 2*M_PI));
00342 if(a < 0) a += 2*(float)M_PI;
00343 }
00344 void toPI(){
00345 if(a>M_PI) while(a>M_PI) a-=2.0*M_PI;
00346 else if(a<-M_PI) while(a<-M_PI) a+=2.0*M_PI;
00347 }
00348 };
00349
00350
00351
00357
00358 class CParticleFilter{
00359 public:
00360 TPoseParticle *Particles;
00361 float Lik;
00362 float outLiers;
00363 int NumOfParticles;
00364 int size;
00365 mcl::pose average;
00366 mcl::pose variance;
00367 bool isAvgSet;
00368 ownRandom myrand;
00369
00370 CParticleFilter();
00371 ~CParticleFilter();
00372
00376 void allocate(int num_particles);
00380 void myfree();
00381
00386 mcl::pose getDistributionMean(bool doWeighting=false);
00395 mcl::pose averageOverNBestAndRandomize(int N, int M=0,float vx=0,float vy=0,float va=0);
00399 Eigen::Matrix3d getDistributionVariances();
00400
00405 void initializeNormalRandom(mcl::pose p0, mcl::pose variance,int size);
00406
00414 void initializeUniform(mcl::pose Pmin, mcl::pose Pmax, mcl::pose dP);
00415
00425 void SIRUpdate();
00426
00433 void normalize();
00434
00442 void predict(mcl::pose dP, mcl::pose std);
00443
00452 void predict(mcl::pose dP, float Q[4]);
00453
00463 void updateLikelihood(float *lik);
00464
00472 void resize(int n);
00473
00477 void print();
00478 void saveToFile(int Fileind);
00479
00480 private:
00481 TPoseParticle *tmp;
00482
00487 void hpsrt(int * indx);
00488
00489 };
00490 }
00491 #endif
00492