Homepage Demos Overview Downloads Tutorials Reference
Credits
Main Page | Namespace List | Class Hierarchy | Alphabetical List | Compound List | File List | Namespace Members | Compound Members | File Members | Related Pages | Search

afsMain.cc

Go to the documentation of this file.
00001 /*
00002  * Main interface to the AIBO FastSLAM.
00003  */
00004 
00005 #ifdef __cplusplus
00006 extern "C" {
00007 #endif
00008 
00009 #include "afsParticle.h"
00010 #include "afsMotionResample.h"
00011 #include "afsMeasurementUpdate.h"
00012 /* #include "afsFindBestPose.h" */
00013 
00014 #include "Configuration.h"
00015 
00016 #include <stdio.h>
00017 #include <stdlib.h>
00018 #include <math.h>
00019 
00020 /* Globals mean never having to say malloc. ;-) */
00021 afsParticle ParticleSets[2][AFS_NUM_PARTICLES];
00022 afsParticle *Particles, *newParticles;
00023 double Weights[AFS_NUM_PARTICLES];
00024 
00025 /* Initialize everything */
00026 void afsInit()
00027 {
00028   int i;
00029 
00030   Particles = ParticleSets[0];
00031   newParticles = ParticleSets[1];
00032   for(i=0; i<AFS_NUM_PARTICLES; ++i) afsParticleInit(&Particles[i]);
00033 }
00034 
00035 /* Report a movement */
00036 void afsMotion(double dx, double dy, double da, unsigned int time)
00037 {
00038   /* If there was no motion at all, just don't do anything. */
00039   if((dx == 0) && (dy == 0) && (da == 0)) return;
00040 
00041 /* Rescale motion parameters to reflect actual AIBO motion */
00042 #include "motionReshapeKludge.h"
00043 
00044   /* Likewise don't do anything if there was no time spent moving */
00045   if(time == 0) return;
00046 
00047   int i;
00048   for(i=0; i<AFS_NUM_PARTICLES; ++i)
00049     afsMotionResample(&Particles[i], dx, dy, da, time);
00050 }
00051 
00052 /* Manually set a landmark location estimate. landmark is the landmark
00053  * to set, x and y are the location, and covariance is the spherical
00054  * covariance to set about the landmark. You should probably set it to
00055  * something small, like 500. */
00056 void afsSetLandmark(int landmark, double x, double y, double covariance)
00057 {
00058   int i;
00059   for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00060     afsLandmarkLoc *l = &Particles[i].landmarks[landmark];
00061 
00062     l->mean.x = x;
00063     l->mean.y = y;
00064     l->variance.x = l->variance.y = covariance;
00065     l->variance.xy = 0.0;
00066     l->state = PRIMED;
00067   }
00068 }
00069 
00070 /* Distribute the particle robot location estimates uniformly throughout
00071  * the specified bounding box. Recommended for use immediately after
00072  * afsSetLandmark is used to set all landmark locations and before any
00073  * measurement updates has taken place. The tighter the bounding box, 
00074  * the better the performance. This function with the prior function 
00075  * essentially reduces FastSLAM to Monte Carlo Localization. */
00076 /* lx = left x, ty = top y, rx = right x, by = bottom y */
00077 void afsDistribute(double lx, double ty, double rx, double by)
00078 {
00079   double dx = rx - lx;
00080   double dy = ty - by;
00081   int i;
00082 
00083   for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00084     Particles[i].pose.x = lx + (double) rand() * dx / (double) RAND_MAX;
00085     Particles[i].pose.y = by + (double) rand() * dy / (double) RAND_MAX;
00086     Particles[i].pose.theta = (double) rand() * 2.0*M_PI / (double) RAND_MAX;
00087   }
00088 }
00089 
00090 /* Report a sensor measurement for each particle */
00091 void afsMeasurement(int landmark, double theta)
00092 {
00093   int i;
00094 
00095   for(i=0; i<AFS_NUM_PARTICLES; ++i) 
00096     afsMeasurementUpdate(&Particles[i], landmark, theta);
00097 }
00098 
00099 /* Resample all the particles based on the last sensor measurement. */
00100 void afsResample()
00101 {
00102   int i, num_weighted=0;
00103   double weight_sum=0.0, prior_weight=0.0, surrogate_weight;
00104 
00105   double current_wt_point, wt_increment;
00106   int current_wt_index = 0;
00107 
00108   /* Get the weights for all the particles from the last measurement update */
00109   for(i=0; i<AFS_NUM_PARTICLES; ++i)
00110     if(Particles[i].gotweight) {
00111       ++num_weighted;
00112       weight_sum += Particles[i].weight;
00113       Weights[i] = Particles[i].weight;
00114     }
00115     /* In this case, the landmark location has not been triangulated yet,
00116      * so we don't get a weight back. */
00117     else Weights[i] = -1; /* negative means no weight yet. */
00118 
00119   /* Now we come up with a weight for particles that weren't weighted. It's
00120    * just the mean of the other particle weights, or, if no particle is
00121    * weighted, it's 1/AFS_NUM_PARTICLES */
00122   surrogate_weight = num_weighted ? weight_sum / (double) num_weighted
00123           : 1 / (double) AFS_NUM_PARTICLES;
00124 
00125   /* Now how much total weight do we have? */
00126   weight_sum += surrogate_weight * (double) (AFS_NUM_PARTICLES - num_weighted);
00127 
00128   /* Now, in one fell blow, we enter in all the substitute weights
00129    * and make them a cumulative tally (so we can sample proportionally). */
00130   for(i=0; i<AFS_NUM_PARTICLES; ++i)
00131     if(Weights[i] >= 0.0)
00132       prior_weight = Weights[i] = Weights[i] + prior_weight;
00133     else
00134       prior_weight = Weights[i] = surrogate_weight + prior_weight;
00135 
00136   /* We resample all the particles according to the low variance resampling
00137    * algorithm that Mike told me about. You randomly select a starting point
00138    * somewhere in [0 weight_sum) and do this:
00139    * 1.  Choose particle corresponding to current point
00140    * 2.  Increment current point by weight_sum/AFS_NUM_PARTICLES. If it gets
00141    *     bigger than weight_sum, subtract weight_sum. GOTO 1 unless we've
00142    *     sampled enough already!
00143    * The exciting part is that this resamples in O(N) and is somehow, according
00144    * to Mike, statistically purer. Don't ask me! */
00145   /* Choose initial current point */
00146   current_wt_point = (double) rand() * weight_sum / (double) RAND_MAX;
00147   /* Choose weight increment */
00148   wt_increment = weight_sum / (double) AFS_NUM_PARTICLES;
00149 
00150   /* Now we resample all the particles. */
00151   for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00152     /* Find out which particle corresponds to the current wt point */
00153     /* current_wt_index initialized at declaration */
00154     while(current_wt_point > Weights[current_wt_index])
00155       /* this code handles wraparound */
00156       if(++current_wt_index >= AFS_NUM_PARTICLES) {
00157   current_wt_index = 0;
00158         current_wt_point = fmod(current_wt_point, weight_sum);
00159       }
00160     /* copy the particle into the new particle set */
00161     afsParticleCopy(&Particles[current_wt_index], &newParticles[i]);
00162     /* increment current weight point */
00163     current_wt_point += wt_increment;
00164     /* Set weight of new particles to 1.0, to allow for multiplicative
00165      * weighting later. See afsMeasurementUpdate for more details. */
00166     newParticles[i].weight = 1.0;
00167   }
00168 
00169   /* Make the new particles the current particles and recycle the space for
00170    * the old particles. */
00171   {
00172     afsParticle* temp = Particles;
00173     Particles = newParticles;
00174     newParticles = temp;
00175   }
00176 
00177   /* that's it! */
00178 }
00179 
00180 /* Scratch space for afsWhatsUp */
00181 //static afsXY rectified_locations[AFS_NUM_PARTICLES];
00182 
00183 /* Get the particle that is most representative of where we are and what the
00184  * world looks like. This particle may be synthesized or it may be taken
00185  * directly out of the particle list. Don't modify it, please. The error for
00186  * this particle will be put into the variable pointed to by error, so long
00187  * as the address isn't NULL. */
00188 afsParticle *afsWhatsUp(double *error)
00189 {
00190   /* Simply averages the landmark locations and the robot headings of all
00191    * the particles in the filter. Apparently this is the right thing to
00192    * do according to a conversation with Mike Montemerlo--although the old
00193    * method commented out below, along with the map. It's certainly much
00194    * simpler and faster. */
00195   int i, j;
00196   static afsParticle avg;
00197   double theta_x, theta_y;
00198 
00199   /* Initialize the particle to be all zeroes. We have to set the landmark
00200    * locations explicitly to zero, since afsParticleInit uses warning
00201    * values instead. */
00202   afsParticleInit(&avg);
00203   for(j=0; j<AFS_NUM_LANDMARKS; ++j)
00204     avg.landmarks[j].mean.x = avg.landmarks[j].mean.x = 0.0;
00205   /* These variables are used for averaging the robot's bearing. We have
00206    * to average the x and y components of the bearing, since averaging
00207    * the bearing itself, a circular quantity between 0 and 2pi, would
00208    * produce nonsense */
00209   theta_x = theta_y = 0.0;
00210 
00211   /* Sum up all the robot pose information */
00212   for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00213     avg.pose.x += Particles[i].pose.x;
00214     avg.pose.y += Particles[i].pose.y;
00215 
00216     theta_x += cos(Particles[i].pose.theta);
00217     theta_y += sin(Particles[i].pose.theta);
00218   }
00219 
00220   /* Division step of the pose averaging */
00221   avg.pose.x /= (double) AFS_NUM_PARTICLES;
00222   avg.pose.y /= (double) AFS_NUM_PARTICLES;
00223   theta_x /= AFS_NUM_PARTICLES;
00224   theta_y /= AFS_NUM_PARTICLES;
00225   /* Here we turn the angle components into a real angle */
00226   avg.pose.theta = atan2(theta_y, theta_x);
00227   if(avg.pose.theta < 0) avg.pose.theta += 2*M_PI;
00228 
00229   /* Now we work on averaging landmark positions. This is a bit trickier
00230    * since some particles will not have certain landmarks initialized, while
00231    * others will. */
00232 
00233   for(j=0; j<AFS_NUM_LANDMARKS; ++j) {
00234     int primed_count = 0;
00235     double mx=0.0, my=0.0;    /* mean of landmark gaussian means */
00236     double covx=0.0, covxy=0.0, covy=0.0; /* similar stuff for cov. MX */
00237     double dcvx=0.0, dcvxy=0.0, dcvy=0.0; /* similar stuff for cov. MX */
00238 
00239     /* Finding the mean of the gaussian means is simple--just averaging.
00240      * Finding the covariance matrix of the combined gaussian is harder. Here,
00241      * we add the summed covariances of all estimates (cov* variables) to
00242      * the summed outer products of differences between the mean of means
00243      * and the individual means themselves. */
00244 
00245     /* summation stage for mean averaging and covariance computation */
00246     for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00247       afsLandmarkLoc *l = &(Particles[i].landmarks[j]);
00248 
00249       if(l->state == PRIMED) {
00250   mx += l->mean.x;
00251   my += l->mean.y;
00252 
00253   covx  += l->variance.x;
00254   covxy += l->variance.xy;
00255   covy  += l->variance.y;
00256 
00257   ++primed_count;
00258       }
00259     }
00260 
00261     /* break if there are no particles that have this landmark PRIMED. No
00262      * need to do anything special, since landmark in avg is initialized
00263      * PRIMING */
00264     if(primed_count == 0) break;
00265 
00266     /* division stage for mean averaging */
00267     mx /= (double) primed_count;
00268     my /= (double) primed_count;
00269 
00270     /* summation stage for outer products of distances from mean of means */
00271     for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00272       afsLandmarkLoc *l = &(Particles[i].landmarks[j]);
00273 
00274       if(l->state == PRIMED) {
00275   double dx = l->mean.x - mx;
00276   double dy = l->mean.y - my;
00277 
00278   dcvx  += dx * dx;
00279   dcvxy += dx * dy;
00280   dcvy  += dy * dy;
00281       }
00282     }
00283 
00284     /* Final consolidation of covariance bits */
00285     covx  = (covx  + dcvx ) / (double) primed_count;
00286     covxy = (covxy + dcvxy) / (double) primed_count;
00287     covy  = (covy  + dcvy ) / (double) primed_count;
00288 
00289     /* finally, set landmark location estimate in avg */
00290     avg.landmarks[j].state = PRIMED;
00291     avg.landmarks[j].mean.x = mx;
00292     avg.landmarks[j].mean.y = my;
00293     avg.landmarks[j].variance.x  = covx;
00294     avg.landmarks[j].variance.xy = covxy;
00295     avg.landmarks[j].variance.y  = covy;
00296   }
00297 
00298   /* This calculation is pretty bogus, but it might be useful. Just the
00299    * average distance from the X,Y pose of all particles to the X,Y pose
00300    * of the average particle */
00301   if(error) {
00302     double dist = 0.0;
00303     for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00304       double dx = avg.pose.x - Particles[i].pose.x;
00305       double dy = avg.pose.y - Particles[i].pose.y;
00306       dist += sqrt(dx*dx + dy*dy);
00307     }
00308     *error = dist / (double) AFS_NUM_PARTICLES;
00309   }
00310 
00311   return &avg;
00312 
00313 /* This old code finds out the state of the particle filter the old-fashioned
00314  * and complicated way, which I have since learned is incorrect. It finds
00315  * a sort of representative "average" particle and then determines which
00316  * particle in the filter is closest to this. */
00317 #if 0
00318   int i;
00319   afsParticle *closest = NULL;
00320   double best_dist = HUGE_VAL;
00321   afsXY avg_location;
00322 
00323   /* REMOVE */
00324   double avg_dist = 0;
00325 
00326   afsXY guessmap[AFS_NUM_LANDMARKS];
00327   afsPose guesspose, best_pose;
00328 
00329   avg_location.x = avg_location.y = 0;
00330 
00331   /* We start by generating the most likely map of the environment and the
00332    * most likely pose from all of the particles */
00333   afsGuessState(Particles, &guesspose, guessmap);
00334 
00335   /* Now we find out which particle has the map most like this one */
00336   for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00337     /* Transform particle i to best match the guessed map */
00338     best_pose = afsFindBestPose(&Particles[i], guesspose, guessmap);
00339 
00340     /* Cache this location in rectified_locations */
00341     /* Summing step of average location */
00342     avg_location.x += (rectified_locations[i].x = best_pose.x);
00343     avg_location.y += (rectified_locations[i].y = best_pose.y);
00344   }
00345 
00346   /* Division step of average location */
00347   avg_location.x /= (double) AFS_NUM_PARTICLES;
00348   avg_location.y /= (double) AFS_NUM_PARTICLES;
00349 
00350   /* Figure out which rectified particle has the robot closest to the mean
00351    * location */
00352   for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00353     double xdist, ydist, dist;
00354 
00355     xdist = rectified_locations[i].x - avg_location.x;
00356     ydist = rectified_locations[i].y - avg_location.y;
00357     dist = sqrt(xdist*xdist + ydist*ydist);
00358 
00359     /* REMOVE */
00360     avg_dist += dist;
00361 
00362     if(dist < best_dist) {
00363       best_dist = dist;
00364       closest = &Particles[i];
00365     }
00366   }
00367 
00368   /* REMOVE */
00369   fprintf(stderr, "AVERAGE DISTANCE: %f\n", avg_dist/(double)AFS_NUM_PARTICLES);
00370 
00371   /* Compute error for this particle if the user wants it. */
00372   if(error) {
00373     afsParticle work_p;
00374 
00375     best_pose = afsFindBestPose(closest, guesspose, guessmap);
00376     afsApplyBestPose(closest, &work_p, best_pose);
00377     *error = afsParticleError(&work_p, guesspose, guessmap);
00378   }
00379 
00380   return closest;
00381 #endif
00382 }
00383 
00384 /* Presently just returns the error estimate from afsWhatsUp. Old
00385  * functionality described below. */
00386 /* Get an indication of how certain the FastSLAM algorithm is about its
00387  * location. The lower the number, the better. Particles with unprimed
00388  * landmarks will tend to lower the error, but with more priming, things
00389  * become more reasonable */
00390 double afsCertainty()
00391 {
00392   double error;
00393   afsWhatsUp(&error);
00394   return error;
00395 #if 0
00396   int i;
00397   double total_error = 0;
00398 
00399   afsXY guessmap[AFS_NUM_LANDMARKS];
00400   afsPose guesspose;
00401 
00402   /* We start by generating the most likely map of the environment and the
00403    * most likely pose from all of the particles */
00404   afsGuessState(Particles, &guesspose, guessmap);
00405 
00406   /* Now we find out how close all the particles are to this map */
00407   for(i=0; i<AFS_NUM_PARTICLES; ++i) {
00408     afsPose best_pose;
00409     afsParticle work_p;
00410 
00411     /* Transform particle i to best match the guessed map */
00412     best_pose = afsFindBestPose(&Particles[i], guesspose, guessmap);
00413     afsApplyBestPose(&Particles[i], &work_p, best_pose);
00414 
00415     /* Compute error for this particle */
00416     total_error += afsParticleError(&work_p, guesspose, guessmap);
00417   }
00418 
00419   return log(total_error);
00420 #endif
00421 }
00422 
00423 /* Get direct access to the current particle set. Not approved for general
00424  * use--this is only used by the WorldModel2 serializer to send particle
00425  * filter information data for display. Software that makes (illicit) use
00426  * of this routine should not count on the pointer it returns remaining
00427  * accurate after a resampling. */
00428 afsParticle *afsInvadeFSData()
00429 {
00430   return Particles;
00431 }
00432 
00433 #ifdef __cplusplus
00434 }
00435 #endif

Tekkotsu v1.4
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2