00001
00005 #include "ndt_mcl/CParticleFilter.h"
00006
00007 using namespace mcl;
00008
00009 CParticleFilter::CParticleFilter(){
00010 Particles=NULL;
00011 Lik=0;
00012 outLiers=0;
00013 NumOfParticles=0;
00014 size=0;
00015 isAvgSet = false;
00016 }
00017 CParticleFilter::~CParticleFilter(){
00018
00019 }
00020
00025 void CParticleFilter::allocate(int num_particles){
00026 Particles = (TPoseParticle *) malloc(sizeof(TPoseParticle)*num_particles);
00027
00028 if(!Particles){
00029 fprintf(stderr,"Failed to reserve memory - exiting\n");
00030 exit(1);
00031 }
00032
00033 tmp = (TPoseParticle *) malloc(sizeof(TPoseParticle)*num_particles);
00034
00035 if(!tmp){
00036 fprintf(stderr,"Failed to reserve memory - exiting\n");
00037 exit(1);
00038 }
00039
00040 size = num_particles;
00041 NumOfParticles=0;
00042 outLiers = 0;
00043 Lik=0;
00044 }
00048 void CParticleFilter::myfree(){
00049 if(Particles) free(Particles);
00050 if(tmp) free(tmp);
00051 size = 0;
00052 NumOfParticles = 0;
00053 }
00054
00059 mcl::pose CParticleFilter::getDistributionMean(bool doWeighting){
00060 double sumX=0,sumY=0,sumA=0;
00061 int i;
00062 mcl::pose pos;
00063 double sumW=0;
00064 isAvgSet = true;
00065
00066 double dsum_a=0;
00067
00068 double ax=0,ay=0;
00069 if(doWeighting){
00070 for(i=0;i<NumOfParticles;i++){
00071 sumX += (double)Particles[i].p*(double)Particles[i].x;
00072 sumY += (double)Particles[i].p*(double)Particles[i].y;
00073 ax += (double)Particles[i].p * cos(Particles[i].a);
00074 ay += (double)Particles[i].p * sin(Particles[i].a);
00075 sumW += (double)Particles[i].p;
00076 }
00077
00078 if(fabs(sumA)>10000){
00079 fprintf(stderr,"GANSKA ISOA (%.2f %.2f %.2f)\n",sumX,sumY,sumA);
00080
00081 }
00082 if( fabs(sumW-1.0)> 0.01){
00083 fprintf(stderr,"getDistributionMean::SUMW=%.2f\n",sumW); exit(1);
00084 }
00085 pos.set(sumX,sumY,atan2(ay,ax));
00086
00087 }else{
00088 for(i=0;i<NumOfParticles;i++){
00089 sumX += Particles[i].x;
00090 sumY += Particles[i].y;
00091 ax += cos(Particles[i].a);
00092 ay += sin(Particles[i].a);
00093
00094
00095 }
00096 if(NumOfParticles==0) fprintf(stderr,"CParticleFilter::getDistributionMean():: WTF!!!!\n");
00097 pos.x = sumX / (float) NumOfParticles;
00098 pos.y = sumY / (float) NumOfParticles;
00099 pos.a = atan2(ay,ax);
00100 }
00101 pos.toPI();
00102 average = pos;
00103
00104 return pos;
00105 }
00106
00113 Eigen::Matrix3d CParticleFilter::getDistributionVariances(){
00114 mcl::pose avg;
00115 int i;
00116
00117 if(isAvgSet) avg = average;
00118 else avg = getDistributionMean();
00119
00120 double xx = 0, yy=0, xy=0, aax=0, aay=0;
00121 double ax = cos(avg.a);
00122 double ay = sin(avg.a);
00123 double w2 = 0;
00124
00125
00126 for(i=0;i<NumOfParticles;i++){
00127 xx += Particles[i].p*(Particles[i].x - avg.x)*(Particles[i].x - avg.x);
00128 yy += Particles[i].p*(Particles[i].y - avg.y)*(Particles[i].y - avg.y);
00129 xy += Particles[i].p*(Particles[i].x - avg.x)*(Particles[i].y - avg.y);
00130
00131 aax += Particles[i].p*(cos(Particles[i].a) - ax)*(cos(Particles[i].a) - ax);
00132 aay += Particles[i].p*(sin(Particles[i].a) - ay)*(sin(Particles[i].a) - ay);
00133 w2 += Particles[i].p * Particles[i].p;
00134 }
00135
00136 if(w2 == 1.0){
00137 fprintf(stderr,"CParticleFilter::getDistributionVariances -- w2=%lf Should not happen!\n",w2);
00138 w2 = 0.99;
00139 }
00140 double wc = 1.0 / (1.0 - w2);
00141
00142 Eigen::Matrix3d cov;
00143 cov << wc*xx , wc*xy , 0,
00144 wc*xy , wc*yy , 0,
00145 0 , 0 , atan2(wc*aay, wc*aax);
00146
00147 return cov;
00148 }
00149
00150
00151
00152
00157 void CParticleFilter::initializeNormalRandom(mcl::pose p0, mcl::pose variance,int size){
00158 int i;
00159
00160 if(this->size != size){
00161 this->allocate(size);
00162 }
00163 for(i=0;i<size;i++){
00164 Particles[i].x = p0.x + myrand.normalRandom() * variance.x;
00165 Particles[i].y = p0.y + myrand.normalRandom() * variance.y;
00166 Particles[i].a = p0.a + myrand.normalRandom() * variance.a;
00167 Particles[i].lik = 1.0;
00168 Particles[i].p = 1.0 / size;
00169 Particles[i].toPI();
00170 }
00171 NumOfParticles = size;
00172 isAvgSet = false;
00173 }
00174
00182 void CParticleFilter::initializeUniform(mcl::pose Pmin, mcl::pose Pmax, mcl::pose dP){
00183 int Nx,Ny,Na;
00184 int i,j,k;
00185
00186 if(dP.x==0 || dP.y==0 || dP.a==0){
00187 fprintf(stderr,"initializeUniform():: Invalid argument\n");
00188 return;
00189 }
00190
00191 Nx = (int)((Pmax.x - Pmin.x)/dP.x);
00192 Ny = (int)((Pmax.y - Pmin.y)/dP.y);
00193 Na = (int)((Pmax.a - Pmin.a)/dP.a);
00195 if(Nx == 0) Nx = 1;
00196 if(Ny == 0) Ny = 1;
00197 if(Na == 0) Na = 1;
00198
00199 fprintf(stderr,"initializeUniform()::Allocating (%d * %d * %d) = %d particles\n",Nx,Ny,Na,Nx*Ny*Na);
00200 this->allocate(Nx*Na*Ny);
00201
00202 for(i=0;i<Nx;i++){
00203 for(j=0;j<Ny;j++){
00204 for(k=0;k<Na;k++){
00205 Particles[NumOfParticles].x = Pmin.x + dP.x*i;
00206 Particles[NumOfParticles].y = Pmin.y + dP.y*j;
00207 Particles[NumOfParticles].a = Pmin.a + dP.a*k;
00208 Particles[NumOfParticles].lik = 1.0;
00209 Particles[NumOfParticles].p = 1.0 / (Nx*Ny*Na);
00210 Particles[NumOfParticles].toPI();
00211 NumOfParticles++;
00212 }
00213 }
00214 }
00215 isAvgSet = false;
00216 }
00226 void CParticleFilter::SIRUpdate(){
00227 TPoseParticle *tmp2;
00228 float U=0,Q=0;
00229 int i=0,j=0,k=0;
00230
00231 U = myrand.uniformRandom() / (float) NumOfParticles;
00232
00233 while(U < 1.0){
00234
00235 if(Q>U){
00236 U += 1.0/NumOfParticles;
00237
00238 if(k>=NumOfParticles || i>=NumOfParticles){
00239 while(i<NumOfParticles){
00240 tmp[i]=Particles[NumOfParticles-1];
00241 tmp[i].p = 1.0 / NumOfParticles;
00242 i++;
00243 }
00244
00245 tmp2 = Particles;
00246 Particles = tmp;
00247 tmp = tmp2;
00248 return;
00249 }
00250 tmp[i]=Particles[k];
00251 tmp[i].p = 1.0 / NumOfParticles;
00252 i++;
00253 }
00254 else{
00255 j++;
00256 k=j;
00257 if(j>=NumOfParticles){
00258 while(i<NumOfParticles){
00259 tmp[i]=Particles[NumOfParticles-1];
00260 tmp[i].p = 1.0 / NumOfParticles;
00261 i++;
00262 }
00263
00264 tmp2 = Particles;
00265 Particles = tmp;
00266 tmp = tmp2;
00267 return;
00268 }
00269 Q += Particles[j].p;
00271
00272 if(j==NumOfParticles){
00273 while(i<NumOfParticles){
00274 tmp[i]=Particles[k-1];
00275 tmp[i].p = 1.0 / NumOfParticles;
00276 i++;
00277 }
00278 tmp2 = Particles;
00279 Particles = tmp;
00280 tmp = tmp2;
00281 return;
00282 }
00283 }
00284 }
00285 while(i<NumOfParticles){
00286 if(k>=NumOfParticles) k=NumOfParticles-1;
00287 tmp[i]=Particles[k];
00288 tmp[i].p = 1.0 / NumOfParticles;
00289 i++;
00290 }
00291 isAvgSet = false;
00292 tmp2 = Particles;
00293 Particles = tmp;
00294 tmp = tmp2;
00295 }
00296
00303 void CParticleFilter::normalize(){
00304 int i;
00305 double summ=0;
00306 isAvgSet = false;
00307 for(i=0;i<NumOfParticles;i++){
00308 Particles[i].p *= Particles[i].lik;
00309 summ+=Particles[i].p;
00310 }
00311 if(summ != 0){
00312 for(i=0;i<NumOfParticles;i++){
00313 Particles[i].p = Particles[i].p/summ;
00314 }
00315 }else{
00316 for(i=0;i<NumOfParticles;i++){
00317 Particles[i].p = 1.0/NumOfParticles;
00318 }
00319 }
00320 }
00321
00322
00330 void CParticleFilter::predict(mcl::pose dP, mcl::pose std){
00331 int i;
00332 float dx=0.0,dy=0.0,dl=0.0;
00333 float a=0.0;
00334
00335
00336
00337
00342 float dxe,dye;
00343 for(i=0;i<NumOfParticles;i++){
00345 dxe = dP.x + myrand.normalRandom()* std.x;
00346 dye = dP.y + myrand.normalRandom()* std.y;
00347
00348 dl = sqrt(dxe*dxe+dye*dye);
00349 a = atan2(dye,dxe);
00350
00351 dx = dl * cos(Particles[i].a +a );
00352 dy = dl * sin(Particles[i].a +a );
00353
00354 Particles[i].x = Particles[i].x + dx;
00355 Particles[i].y = Particles[i].y + dy;
00356 Particles[i].a = Particles[i].a + dP.a + myrand.normalRandom()* std.a;
00357 Particles[i].toPI();
00358 }
00359 isAvgSet = false;
00360 }
00361
00371 void CParticleFilter::predict(mcl::pose dP, float Q[4]){
00372 int i;
00373 float dx,dy,dl;
00374 float cov[3];
00375 float xRand,yRand;
00376 float a,b,c;
00377 float eig1=1,eig2=1;
00378 float vx1,vy1,vx2,vy2;
00379 float d;
00380 float x,y;
00381
00382
00383 cov[0] = Q[0];
00384 cov[1] = Q[1];
00385 cov[2] = Q[2];
00386
00387 a = 1.0;
00388 b = -(cov[0]+cov[2]);
00389 c = cov[0]*cov[2] - cov[1]*cov[1];
00390
00391 if( (b*b-4*c)>=0){
00392 eig1 = (-b + sqrt(b*b-4*a*c))/(2*a);
00393 eig2 = (-b - sqrt(b*b-4*a*c))/(2*a);
00394
00395
00396 if(eig1 != eig2){
00397 vx1 = cov[1];
00398 vy1 = eig1 - cov[0];
00399 d = sqrt(vx1*vx1+vy1*vy1);
00400 vx1 /= d;
00401 vy1 /= d;
00402
00403 vx2 = cov[1];
00404 vy2 = eig2 - cov[0];
00405 d = sqrt(vx2*vx2+vy2*vy2);
00406 vx2 /= d;
00407 vy2 /= d;
00408 }
00409 else{
00410 vx1 = 1.0;
00411 vx2 = 0.0;
00412 vy1 = 0.0;
00413 vy2 = 1.0;
00414 }
00415
00416 }
00417 else{
00418 fprintf(stderr,"IMAGINARY EIGEN VALUES\n");
00419 eig1=1;
00420 eig2=1;
00421
00422 vx1 = 1.0;
00423 vx2 = 0.0;
00424 vy1 = 0.0;
00425 vy2 = 1.0;
00426 }
00427
00428
00429 dl = sqrt(dP.x*dP.x+dP.y*dP.y);
00430
00431 for(i=0;i<NumOfParticles;i++){
00432 xRand = myrand.normalRandom();
00433 yRand = myrand.normalRandom();
00434
00435 x = xRand * sqrt(eig1);
00436 y = yRand * sqrt(eig2);
00437
00438 xRand = x * vx1 + y * vx2;
00439 yRand = x * vy1 + y * vy2;
00440
00441 dx = dl * cos(Particles[i].a + dP.a);
00442 dy = dl * sin(Particles[i].a + dP.a);
00443
00446 float covX,covY,covA,covL;
00447 covL = sqrt(xRand*xRand + yRand*yRand);
00448 covA = atan2(yRand,xRand);
00449 covX = covL * cos(Particles[i].a+covA);
00450 covY = covL * sin(Particles[i].a+covA);
00451
00452 Particles[i].x = Particles[i].x + dx + covX;
00453 Particles[i].y = Particles[i].y + dy + covY;
00454 Particles[i].a = Particles[i].a + dP.a + myrand.normalRandom()* Q[3];
00455 Particles[i].toPI();
00456 }
00457 isAvgSet = false;
00458 }
00459
00469 void CParticleFilter::updateLikelihood(float *lik){
00470 int i;
00471 for(i=0;i<NumOfParticles;i++){
00472 Particles[i].lik = lik[i];
00473 }
00474 isAvgSet = false;
00475 }
00476
00480 void CParticleFilter::print(){
00481 getDistributionMean();
00482 getDistributionVariances();
00483 fprintf(stderr,"Filter:: size=%d, AVG:(%.1f,%.1f,%.1f), VAR:(%.1f,%.1f,%.1f)\n",size, average.x,
00484 average.y,average.a, variance.x,variance.y,variance.a);
00485 }
00486
00487
00488
00495 void CParticleFilter::resize(int n){
00496 int i;
00497 TPoseParticle *tmp2;
00498 int *ind = (int *) malloc(NumOfParticles * sizeof(int));
00499 for(i=0;i<NumOfParticles;i++) ind[i] = i;
00500 hpsrt(ind);
00501 for(i=0;i<10;i++) fprintf(stderr,"%d ",ind[i]);
00502
00503 fprintf(stderr,"The best p=%f and worst %f \n",Particles[ind[0]].p,Particles[ind[NumOfParticles-1]].p);
00504
00505 for(i=0;i<n;i++){
00506 tmp[i] = Particles[ind[NumOfParticles-1-i]];
00507 }
00508
00509
00510 tmp2 = Particles;
00511 Particles = tmp;
00512 tmp = tmp2;
00513 NumOfParticles = n;
00514 free(ind);
00515 fprintf(stderr,"Filter resized. New size=%d. The Pb=%f Pw=%f\n",n,Particles[0].p,Particles[NumOfParticles-1].p);
00516
00517 }
00518
00519 mcl::pose CParticleFilter::averageOverNBestAndRandomize(int N, int M,float vx,float vy,float va){
00520
00521 if(N>=NumOfParticles){
00522 return getDistributionMean(true);
00523 }
00524 mcl::pose avg;
00525 if(N<=0) return avg;
00526 int *ind = (int *) malloc(NumOfParticles * sizeof(int));
00527
00528 for(int i=0;i<NumOfParticles;i++) ind[i] = i;
00529 hpsrt(ind);
00530 avg.x = 0;
00531 avg.y = 0;
00532 avg.a = 0;
00533 float ax=0,ay=0;
00534
00535 for(int i=0;i<N;i++){
00536 avg.x+=Particles[ind[i]].x;
00537 avg.y+=Particles[ind[i]].y;
00538 ax += cos(Particles[ind[i]].a);
00539 ay += sin(Particles[ind[i]].a);
00540 }
00541
00542 avg.x/=N;
00543 avg.y/=N;
00544 avg.a = atan2(ay,ax);
00545
00546 if(M>0 && M<NumOfParticles){
00547 for(int i=NumOfParticles-1;i>NumOfParticles-M;i--){
00548 Particles[ind[i]].x = avg.x + myrand.normalRandom() * vx;
00549 Particles[ind[i]].y = avg.y + myrand.normalRandom() * vy;
00550 Particles[ind[i]].a = avg.a + myrand.normalRandom() * va;
00551 }
00552
00553 }
00554
00555
00556 free(ind);
00557 return avg;
00558 }
00559
00565 void CParticleFilter::hpsrt(int * indx){
00566 unsigned long i,ir,j,k,ira;
00567 float rra;
00568 float *ra = (float *) malloc(NumOfParticles*sizeof(float));
00569 int n = NumOfParticles;
00570
00571 for(i=0;i<NumOfParticles;i++) ra[i] = Particles[i].p;
00572
00573 if (n < 1) return;
00574 k=(n >> 1);
00575 ir=n-1;
00576 for (;;)
00577 {
00578 if(k > 0)
00579 {
00580 rra=ra[--k];
00581 ira=indx[k];
00582 }
00583 else
00584 {
00585 rra=ra[ir];
00586 ira=indx[ir];
00587
00588 ra[ir]=ra[0];
00589 indx[ir]=indx[0];
00590 if (--ir == 0)
00591 {
00592 ra[0]=rra;
00593 indx[0]=ira;
00594 break;
00595 }
00596 }
00597 i=k;
00598 j=k+1;
00599 while (j <= ir)
00600 {
00601 if (j < ir && ra[j] < ra[j+1]) j++;
00602 if (rra < ra[j])
00603 {
00604 ra[i]=ra[j];
00605 indx[i]=indx[j];
00606 i=j;
00607 j <<= 1;
00608 } else break;
00609 }
00610 ra[i]=rra;
00611 indx[i]=ira;
00612 }
00613 free(ra);
00614 }
00615
00616 void CParticleFilter::saveToFile(int Fileind){
00617 char name[20];
00618 FILE *f;
00619
00620 sprintf(name,"particle%d.tx",Fileind);
00621
00622 f= fopen(name,"wt");
00623 for(int i=0;i<NumOfParticles;i++){
00624 fprintf(f,"%.3f %.3f %.3f\n",Particles[i].x,Particles[i].y,Particles[i].a);
00625 }
00626 fclose(f);
00627 }
00628
00629
00630
00631