Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ParticleFilter.h

Go to the documentation of this file.
00001 #ifndef INCLUDED_ParticleFilter_h
00002 #define INCLUDED_ParticleFilter_h
00003 
00004 #include <vector>
00005 #include <algorithm>
00006 #include <iostream>
00007 #include <cfloat>
00008 #include <cmath>
00009 
00010 //! Provides a common base class for particles used by the ParticleFilter
00011 /*! Each particle represents a hypothesis regarding a position in state space.
00012  *  The state space being modeled is defined by the fields tracked by
00013  *  the particles, a sensor model which evaluates the 'health' of each particle
00014  *  based on incoming information from the world, and a motion model
00015  *  which updates particles based on the robot's own changes to the world.
00016  *
00017  *  For a common example, see the LocalizationParticle for 
00018  *  tracking a robot's position and orientation in a 2D world.
00019  *
00020  *  The default LowVarianceResamplingPolicy has two requirements for
00021  *  particles.  One requirement is that all particle implementations must
00022  *  define a 'DistributionPolicy' (usually via typedef within your class) so
00023  *  that the resampler can create randomly generated particles and
00024  *  modify existing ones. (see ParticleFilter::DistributionPolicy)
00025  *
00026  *  The second requirement is that all particles provide a public 'weight'
00027  *  field so that particles can be compared.  The recommended way to do
00028  *  this is to inherit from this ParticleBase base class.  However, since
00029  *  templates are used to specify the particle type to the particle filter,
00030  *  you can use an unaffiliated class as long as it provides a weight member.
00031  *  (However, inheritance is still recommended so you'll automatically pick up
00032  *  any changes or new requirements made to this base class.)
00033  *
00034  *  The final requirement of the ParticleFilter itself is to provide a
00035  *  sumSqErr() function so that a confidence interval can be computed.
00036  *  However, the meaning of the value returned by this function is entirely
00037  *  up to you.  The base class provides a prototype for the function, but
00038  *  its implementation is abstract.*/
00039 class ParticleBase {
00040 public:
00041   //! constructor
00042   ParticleBase() : weight(0) {}
00043   //! destructor
00044   virtual ~ParticleBase() {};
00045   
00046   //! returns the sum squared error between this particle and @a p
00047   /*! This is only used to compute the confidence of the particle filter,
00048    *  you may want to weight some dimensions differently if they tend
00049    *  to have smaller values or are more important.  How you interpret
00050    *  ParticleFilter::confidence() depends on how this function is implemented.
00051    *
00052    *  A template is used so you can assume the argument type is that
00053    *  of your own particle type. (it doesn't really make sense to compare
00054    *  the distance between particles of different types.) */
00055   template<typename ParticleT> float sumSqErr(const ParticleT& p) const=0;
00056   
00057   //! indicates the 'health' of the particle -- the bigger the value, the better this particle
00058   /*! Generally weights are indicative of probability, but are often unnormalized
00059    *  since the normalization factor is constant across particles and thus doesn't
00060    *  affect matters of relative comparison.
00061    *
00062    *  Further, weights tend to be very small as the accumulation of a number of
00063    *  sensor values tend to be each somewhat unlikely, and taken together
00064    *  the particle's weight shrinks exponentially.  Thus it useful to work in
00065    *  log space to avoid numeric underflow on the range of a floating point value.
00066    *  This also has the advantage of transforming multiplication operations to
00067    *  slightly quicker addition operations.  The default LowVarianceResamplingPolicy
00068    *  has a logWeights member so you can indicate whether weight values
00069    *  should be interpreted logarithmically (i.e. negative values)
00070    *  or linearly (e.g. positive (and generally very small) values). (default is logarithmic) */
00071   float weight;
00072 };
00073 
00074 //! Implements a particle filter with support for a variety of applications through the usage of arbitrary combination of a variety of models and policies
00075 /*! The particle type is passed as a template parameter, which provides the implementation
00076  *  advantage of being able to directly store arrays of particles as contiguous blocks in memory.  This allows
00077  *  better cache coherency and enables platform-specific acceleration tricks, such as SIMD calls.
00078  *
00079  *  There are a number of embedded classes which together form the implementation of the
00080  *  particle filter.  The advantage of this architecture is that you can mix and match any
00081  *  combination of modules to get the features needed for your application.
00082  *  - SensorModel: pass one of these to updateSensors in order to evaluate the particles
00083  *    as new information is discovered.  You may have several different sensors at the same
00084  *    time, simply create a model for each type of sensor, and pass it to the filter when updated.
00085  *  - MotionModel: modifies particle states based on the expected outcome of
00086  *    any controls you might have over the system.  See DeadReckoningBehavior for an example.
00087  *    Generally, you will install one motion model, and this model will be given a opportunity
00088  *    to update expected particle state before each sensor update.  (unless you pass 'false'
00089  *    to updateSensors()).  MotionModel can be NULL if you have no control over the system.
00090  *  - DistributionPolicy: defines how to generate random particles and "jiggle" existing ones.
00091  *    The default DistributionPolicy is usually specified via a typedef in the particle itself, and
00092  *    is stored as a property of the ResamplingPolicy (next item) since that is what will use it.
00093  *  - ResamplingPolicy: Following a sensor update, you may wish to re-evaluate the particles
00094  *    in use, making copies of the "good" particles, and dropping those which are not matching
00095  *    sensor values.  If you receive a group of different sensor readings, you may want to
00096  *    hold off on resampling until they have all been applied for better evaluation of the particles
00097  *    before selecting which to replicate.  Similarly, if your sensors are noisy, you may want to
00098  *    take several readings before allowing resampling so you don't kill off all the "good" particles
00099  *    based on a bad reading.  Pass 'false' to updateSensors() or use the delayCount parameter of
00100  *    LowVarianceResamplingPolicy.  The resampling policy can be 'NULL' if you never want to
00101  *    resample, but it defaults to an instance of LowVarianceResamlingPolicy.
00102  *
00103  *  Generally, preparing to use a particle filter requires these prerequisites:
00104  *  - write your particle class and its associated distribution policy
00105  *    (see LocalizationParticle and LocalizationParticleDistributionPolicy)
00106  *  - write a sensor model to evaluate particles using sensors you'll have available
00107  *    (see DualCoding::ShapeSensorModel)
00108  *  - write a motion model if you have any knowledge of modifications to the state
00109  *    (see HolonomicMotionModel and DeadReckoningBehavior)
00110  *
00111  *  Once these are available, usage goes something like this:
00112  *  -# create particle filter, optionally passing motion model and/or resampling policy
00113  *  -# customize parameters for resampling and distribution policies
00114  *  -# while active (note these are all "as needed", in no particular order):
00115  *     - update motion model whenever there's a change in controls (e.g. call setVelocity() on
00116  *        a HolonomicMotionModel)
00117  *     - create/pass SensorModel(s) for any measurements obtained (e.g. call updateSensors()
00118  *        on the particle filter)
00119  *     - query getBestParticle() on the particle filter to obtain current state estimate
00120  *
00121  *  Remember that the particle filter takes responsibility for deallocating all policies and the
00122  *  motion model when they are removed.  Do not attempt to reuse them between particle
00123  *  filters. SensorModels are the only exception -- they are @e not retained between calls
00124  *  to updateSensors, so you can reuse them.
00125  */
00126 template<typename ParticleT>
00127 class ParticleFilter {
00128 public:
00129   typedef ParticleT particle_type; //!< redefinition here allows reference to the particle type even if the template parameter may be abstracted away due to a typedef
00130   typedef typename std::vector<particle_type> particle_collection; //!< the collection type we'll be using to store the particles
00131   typedef typename particle_collection::size_type index_t; //!< index type for refering to particles within the collection
00132   
00133   //! A sensor model is used to update particle weights to account based on each particle's ability to explain observations taken from the system
00134   class SensorModel {
00135   public:
00136     typedef ParticleT particle_type; //!< redefinition here allows reference to the particle type even if the template parameter may be abstracted away due to a typedef
00137     typedef typename std::vector<particle_type> particle_collection; //!< the collection type we'll be using to store the particles
00138     typedef typename particle_collection::size_type index_t; //!< index type for refering to particles within the collection
00139     virtual ~SensorModel() {} //!< destructor (no-op for base class)
00140     
00141     //! once passed to the particle filter's updateSensors(), this will be called to allow the sensor model to update the 'weight' member of each particle
00142     /*! @param particles the current set of particles to be evaluated
00143      *  @param[in] bestIndex on input, this will hold the index within @a particles of the particle with the highest weight.
00144      *  @param[out] bestIndex on return, your function should update the parameter so it still indicates the best particle
00145      *
00146      *  Remember to @e update each particle's weight, don't overwrite it.  In other words,
00147      *  you want to combine (e.g. add or multiply) the weight from the current sensor evaluation
00148      *  with the weight currently stored in each particle, don't just replace it.  This is because
00149      *  there may be several sensor updates between resamplings so that particles can be
00150      *  more accurately evaluated. */
00151     virtual void evaluate(particle_collection& particles, index_t& bestIndex)=0;
00152   };
00153 
00154   //! A motion model is retained by the particle filter to query before evaluating sensor measurements so all known influences are accounted for before testing the particles
00155   /*! It's a good idea to apply noise to the motion model depending on the precision of the model.
00156    *  This allows the particle cluster to spread over time until new information is obtained to
00157    *  to evaluate how accurate the motion really was, at which point resampling will collapse
00158    *  the cluster back down again. */
00159   class MotionModel {
00160   public:
00161     typedef ParticleT particle_type; //!< redefinition here allows reference to the particle type even if the template parameter may be abstracted away due to a typedef
00162     typedef typename std::vector<particle_type> particle_collection; //!< the collection type we'll be using to store the particles
00163     typedef typename particle_collection::size_type index_t; //!< index type for refering to particles within the collection
00164     virtual ~MotionModel() {} //!< destructor
00165     
00166     //! The particle filter will call these when it wants to update particle state values to account for known influences
00167     /*! See the class notes regarding the usefulness of adding noise to the control parameters (or their effects) */
00168     virtual void updateMotion(particle_collection& particles)=0;
00169   };
00170 
00171   //! A distribution policy provides the ability to randomize ("redistribute") or tweak the values of a group of particles
00172   /*! Unlike the other particle filter helper classes, the functions for the distribution policy
00173    *  operate on a subset of the particles at a time.
00174    *  You may wonder why the randomize() and jiggle() functions aren't simply made methods
00175    *  of the ParticleBase class.  The main reason is that these functions may need additional 
00176    *  parameters, such as specification of how large an area to distribute over, and these
00177    *  parameters are static across particles.  However, if they were actually static members
00178    *  of the particle class, then the values would be shared by all particle filters.  By making
00179    *  a separate class to hold the parameters and apply the one-to-many relationship, you
00180    *  can have multiple particle filters with the same type of particle, and each filter can have
00181    *  different parameter values controlling distribution of its particles.
00182    *
00183    *  Note that the DistributionPolicy is actually a property of the resampling policy, not
00184    *  directly of the particle filter itself. */
00185   class DistributionPolicy {
00186   public:
00187     typedef ParticleT particle_type; //!< redefinition here allows reference to the particle type even if the template parameter may be abstracted away due to a typedef
00188     typedef typename std::vector<particle_type> particle_collection; //!< the collection type we'll be using to store the particles
00189     typedef typename particle_collection::size_type index_t; //!< index type for refering to particles within the collection
00190     virtual ~DistributionPolicy() {} //!< destructor
00191     
00192     //! This should redistribute the particles over a large area, independently of the particle's current value
00193     /*! Randomization occurs whenever the particle filter doesn't have any usable particles for
00194      *  replication, either because the particle filter has just been created and doesn't have any
00195      *  information yet, or because new sensor readings have invalidated all of the current particles. */
00196     virtual void randomize(particle_type* begin, index_t num)=0;// { particle_type* end=begin+num; while(begin!=end) (begin++)->randomize(); }
00197     
00198     //! This should slightly modify the particles' state values
00199     /*! @param var indicates the scale of the variance desired -- multiply whatever variance you use for modifying each state parameter by this value
00200      *  @param begin the first particle in the array
00201      *  @param num the number of particles to apply the operation to
00202      *
00203      *  This function is called on particles which have been replicated from an existing
00204      *  particle to explore the space around that particle.  The more accurate your
00205      *  sensors and particle evaluation, the smaller the jiggle variance can be. */
00206     virtual void jiggle(float var, particle_type* begin, index_t num)=0;// { particle_type* end=begin+num; while(begin!=end) (begin++)->jiggle(var); }
00207   };
00208   
00209   //! The resampling policy focuses the particle filter on those particles which are performing well, and dropping those which are poorly rated
00210   /*! Resampling should replicate particles proportionally to how well their weights compare
00211    *  to other particles in the filter.  The process is similar to a genetic algorithm.
00212    *  This process typically does not vary between applications,
00213    *  so you will probably want to use the supplied LowVarianceResamplingPolicy, and
00214    *  simply tweak parameters as needed.
00215    *
00216    *  The ResamplingPolicy interface includes a DistributionPolicy so particles can be
00217    *  randomly generated or modified in an abstract manner. */
00218   class ResamplingPolicy {
00219   public:
00220     typedef ParticleT particle_type; //!< redefinition here allows reference to the particle type even if the template parameter may be abstracted away due to a typedef
00221     typedef typename std::vector<particle_type> particle_collection; //!< the collection type we'll be using to store the particles
00222     typedef typename particle_collection::size_type index_t; //!< index type for refering to particles within the collection
00223     
00224     //! constructor, creates a DistributionPolicy based on the particle_type's own DistributionPolicy typedef
00225     ResamplingPolicy() : dist(new typename particle_type::DistributionPolicy) {}
00226     //! constructor, pass your own custom distribution policy (responsibility for deallocation is assumed by the ResamplingPolicy)
00227     explicit ResamplingPolicy(DistributionPolicy * distPolicy) : dist(distPolicy) {}
00228     //! destructor
00229     virtual ~ResamplingPolicy() { delete dist; };
00230     //! the particle filter will call resample() when the particles have been evaluated and are ready to be selected
00231     virtual void resample(particle_collection& particles, index_t& bestIndex)=0;
00232     //! replaces #dist with a new distribution policy.  If you pass NULL, #dist will be reset to the particle_type's default distribution policy, as specified by a 'DistributionPolicy' typedef within the particle class
00233     virtual void setDistributionPolicy(DistributionPolicy * distPolicy) {
00234       delete dist;
00235       dist = (distPolicy!=NULL) ? distPolicy : new typename particle_type::DistributionPolicy;
00236     }
00237     //! returns the currently active distribution policy (#dist)
00238     virtual DistributionPolicy& getDistributionPolicy() const { return *dist; }
00239   protected:
00240     //! a pointer to the current distribution policy, which cannot be NULL
00241     DistributionPolicy * dist;
00242   private:
00243     ResamplingPolicy(const ResamplingPolicy& rp); //!< copy unsupported
00244     ResamplingPolicy& operator=(const ResamplingPolicy& rp); //!< assignment unsupported
00245   };
00246   
00247   //! This class provides a generic, default ResamplingPolicy.  It is based on the low variance resampling policy algorithm found in "Probabilistic Robotics" by Sebastian Thrun, Wolfram Burgard, Dieter Fox
00248   /*! This class is called "low variance" because it will maintain particle modalities in the face of
00249    *  uniform weighting.  This means that if resamples are triggered when no new information
00250    *  is available, every particle is resampled for the next generation.  This prevents the eventual
00251    *  convergence of particle clusters over time.
00252    *
00253    *  However, this implementation provides a #varianceScale parameter for adding variance
00254    *  to the particle's state on each generation, which can be useful for more rapidly exploring
00255    *  the state space around a "winning" particle.  Ideally, it is better to use a very low resampling
00256    *  variance, and rely on noise in the motion model and a broad probability distribution in
00257    *  the sensor model to allow particles to spread.  #varianceScale is really a crutch to manage
00258    *  an overconfident sensor model (one which weights "correct" particles with sharply higher values).
00259    *  
00260    *  The #varianceScale parameter defaults to a negative value, which indicates the
00261    *  resampling variance will be scaled with particle weight to provide broader sampling when
00262    *  particle weights are poor, and tighten sampling when particles are tracking accurately.  This
00263    *  requires setting a value for #minAcceptableWeight, described next.
00264    *
00265    *  The other trick this implementation provides is specification of a minimum acceptable
00266    *  particle weight (#minAcceptableWeight).  If the best particle's weight is below this value,
00267    *  new, randomly generated particles will be created, up to #maxRedistribute percent of
00268    *  the particles on a round of resampling.  This handles situations where the actual state
00269    *  has somehow jumped out of the region being sampled by the particles, and the filter is "lost".
00270    *  Without some further information (i.e. fixing the MotionModel to predict the "kidnapping"),
00271    *  this can provide automatic re-acquisition of the position in state space.  (at the cost of
00272    *  spawning new modalities)
00273    *
00274    *  Finally, #resampleDelay is provided to limit actual resampling to one in every #resampleDelay
00275    *  attempts.  This allows you to only resample at a lower frequency than the sensor model,
00276    *  without having to manually track the number of sensor samples and pass a parameter to
00277    *  the ParticleFilter::updateSensors() to limit resampling yourself.  The reason you would
00278    *  want to limit the resampling frequency is to better evaluate the particles before selecting
00279    *  them for replication or pruning -- if your sensors are noisy and you resample too often,
00280    *  bad values will kill off good particles on a regular basis, causing the filter to continually be "lost".
00281    *
00282    *  This policy can interpret weights in either "log space" or "linear space".  It defaults to "log space",
00283    *  but if your sensor model is providing linear weights, set #logWeights to false.
00284    */
00285   class LowVarianceResamplingPolicy : public ResamplingPolicy {
00286   public:
00287     //! constructor
00288     LowVarianceResamplingPolicy()
00289     : varianceScale(-2), maxRedistribute(1/2.f), minAcceptableWeight(-FLT_MAX),
00290     logWeights(true), resampleDelay(0), newParticles(), resampleCount(0)
00291     {}
00292     virtual void resample(particle_collection& particles, index_t& bestIndex);
00293     
00294     //! returns true if the next call to resample will trigger a "real" resampling (is #resampleCount greater than #resampleDelay?)
00295     bool nextResampleIsFull() { return resampleCount>=resampleDelay; }
00296     
00297     //! If non-negative, passed to the DistributionPolicy's jiggle() for replicated particles; otherwise an "automatic" value is used which inversely scales with the best particle weight
00298     /*! A negative value is still used to control the maximum magnitude of the resampling variance.
00299      *  It's better to keep this small (or zero) and rely on the sensor and motion model's noise parameters */
00300     float varianceScale;
00301     //! A percentage (0-1) of the particles which can be randomly re-distributed on a single resampling if the best particle's weight is below #minAcceptableWeight
00302     float maxRedistribute;
00303     //! The lowest weight per resample attempt to consider "acceptable"
00304     /*! This is scaled by resampleDelay when being compared to particle weights, so that
00305      *  you don't have to adjust this parameter when you increase resampleDelay.
00306      *  As the best particle weight drops below this value, more particles will be randomly
00307      *  redistributed, up to #maxRedistribute. */
00308     float minAcceptableWeight; 
00309     //! This controls the interpretation of particle weights.  If true, they are interpreted as "log space", otherwise "linear space"
00310     bool logWeights;
00311     //! This indicates how many resampling attempts should be skipped before actually doing it.  See class notes for rationale.
00312     unsigned int resampleDelay;
00313   protected:
00314     particle_collection newParticles; //!< temporary scratch space as particles are created, and then the collections are swapped
00315     unsigned int resampleCount; //!< the number of resampling attempts which have occurred.
00316   };
00317   
00318   
00319   //! Constructor for the particle filter, specify number of particles and optionally pass a motion model and resampling policy
00320   /*! The particle filter assumes responsibility for eventual deallocation of the motion model and resampling policy */
00321   explicit ParticleFilter(unsigned int numParticles, MotionModel* mm=NULL, ResamplingPolicy* rs=new LowVarianceResamplingPolicy)
00322     : particles(numParticles), bestIndex(0), motion(mm), resampler(rs), hasEvaluation(false)
00323   {
00324     if(numParticles>0)
00325       resetFilter(particles.front().weight);
00326   }
00327   
00328   //! Destructor
00329   virtual ~ParticleFilter() { delete motion; delete resampler; }
00330   
00331   //! Returns the current motion model (#motion)
00332   virtual MotionModel * getMotionModel() const { return motion; }
00333   //! Reassigns the motion model, deleting the old one; motion model can be NULL
00334   virtual void installMotionModel(MotionModel* mm) { delete motion; motion=mm; }
00335   
00336   //! Returns the current resampling policy (#resampler)
00337   virtual ResamplingPolicy * getResamplingPolicy() const { return resampler; }
00338   //! Reassigns the resampling policy, deleting the old one; resampling policy can be NULL (although not recommended...)
00339   virtual void installResamplingPolicy(ResamplingPolicy* rs) { delete resampler; resampler=rs; }
00340   
00341   //! Sets the resampling policy's resampleDelay, which controls how many sensor updates to process before resampling the particles; policy must be a LowVarianceResamplingPolicy
00342   virtual void setResampleDelay(unsigned int d) {
00343     LowVarianceResamplingPolicy* p = dynamic_cast<LowVarianceResamplingPolicy*>(getResamplingPolicy());
00344     if ( p )
00345       p->resampleDelay = d;
00346     else
00347       std::cout << "Warning: setResampleDelay found getResamplingPolicy() returns wrong type policy; resampleDelay not set." << std::endl;
00348   }
00349   
00350   //! Sets the resampling policy's minimum acceptable weight for a particle; policy must be a LowVarianceResamplingPolicy
00351   virtual void setMinAcceptableWeight(float w) {
00352     LowVarianceResamplingPolicy* p = dynamic_cast<LowVarianceResamplingPolicy*>(getResamplingPolicy());
00353     if ( p )
00354       p->minAcceptableWeight = w;
00355     else
00356       std::cout << "Warning: setMinAcceptableWeight found getResamplingPolicy() returns wrong type policy; minAcceptableWeight not set." << std::endl;
00357   }
00358   
00359   //! If getResamplingPolicy() returns a LowVarianceResamplingPolicy instance, this will set LowVarianceResamplingPolicy::maxRedistribute; otherwise will display a warning
00360   virtual void setMaxRedistribute(float r) {
00361     LowVarianceResamplingPolicy* p = dynamic_cast<LowVarianceResamplingPolicy*>(getResamplingPolicy());
00362     if ( p )
00363       p->maxRedistribute = r;
00364     else
00365       std::cout << "Warning: setMaxRedistribute found getResamplingPolicy() returns wrong type policy; maxRedistribute not set." << std::endl;
00366   }
00367   
00368   //! If getResamplingPolicy() returns a LowVarianceResamplingPolicy instance, this will set LowVarianceResamplingPolicy::varianceScale; otherwise will display a warning
00369   virtual void setVarianceScale(float s) {
00370     LowVarianceResamplingPolicy* p = dynamic_cast<LowVarianceResamplingPolicy*>(getResamplingPolicy());
00371     if ( p )
00372       p->varianceScale = s;
00373     else
00374       std::cout << "Warning: setVarianceScale found getResamplingPolicy() returns wrong type policy; varianceScale not set." << std::endl;
00375   }
00376   
00377 
00378   //! Allows you to manually request a position update -- you might want to call this before using getBestParticle's state information
00379   virtual void updateMotion() {
00380     if(motion!=NULL)
00381       motion->updateMotion(particles);
00382   }
00383   
00384   //! Applies the sensor model's evaluation to the particles, optionally updating the motion model and resampling first
00385   /*! If you are applying a group of sensor readings, you probably only want to update motion for the first one
00386    *  (since no motion is occuring between the readings if they were taken at the same time), and may
00387    *  want to hold off on resampling until the end (so the particles are better evaluated).
00388    *  If using the default LowVarianceResamplingPolicy, see also LowVarianceResamplingPolicy::resampleDelay. */
00389   virtual void updateSensors(SensorModel& sm, bool updateMot=true, bool doResample=true) {
00390     if(updateMot)
00391       updateMotion();
00392     if(doResample && hasEvaluation)
00393       resample();
00394     sm.evaluate(particles, bestIndex);
00395     hasEvaluation=true;
00396   }
00397   
00398   //! A manual call to trigger resampling
00399   virtual void resample() {
00400     if(resampler!=NULL)
00401       resampler->resample(particles,bestIndex);
00402     hasEvaluation=false;
00403   }
00404   
00405   //! Assigns the specified weight value to all of the particles
00406   virtual void resetWeights(float w) {
00407     for(typename particle_collection::iterator it=particles.begin(); it!=particles.end(); ++it)
00408       it->weight=w;
00409     hasEvaluation=false;
00410   }
00411   //! Requests that the resampler's distribution policy randomly distribute all of the particles, and reset weights to @a w.
00412   /*! You might want to do this if you believe you have been "kidnapped" by some unmodeled motion
00413    *  to a new area of state space, and need to restart the filter to determine the new location. */
00414   virtual void resetFilter(float w) {
00415     if(resampler!=NULL)
00416       resampler->getDistributionPolicy().randomize(&particles[0],particles.size());
00417     resetWeights(w);
00418   }
00419   
00420   virtual index_t getBestIndex() const { return bestIndex; } //!< Returns the index of the best particle in #particles
00421   virtual const particle_type& getBestParticle() const { return particles[bestIndex]; } //!< Returns a reference to the best particle in #particles
00422   virtual particle_collection& getParticles() { return particles; } //!< Returns a reference to #particles itself (if you want to modify the particles, generally better to formulate it in terms of a sensor model or motion model for consistency)
00423   virtual const particle_collection& getParticles() const { return particles; } //!< Returns a reference to #particles itself (if you want to modify the particles, generally better to formulate it in terms of a sensor model or motion model for consistency)
00424 
00425   //! if you know the position in state space, pass it here, along with a positive varianceScale if you want some jiggle from the distribution policy
00426   virtual void setPosition(const particle_type& pos, float variance=0) {
00427     particles.assign(particles.size(),pos);
00428     if(variance>0)
00429       resampler->getDistributionPolicy().jiggle(variance,&particles.front(),particles.size());
00430   }
00431   
00432   //! Returns a confidence interval based on the particle_type's sumSqErr implementation (see ParticleBase::sumSqErr())
00433   virtual float getConfidenceInterval() const {
00434     float d=0;
00435     const particle_type& p=particles[bestIndex];
00436     for(typename particle_collection::const_iterator it=particles.begin(); it!=particles.end(); ++it)
00437       d += it->sumSqErr(p);
00438     return std::sqrt(d/(particles.size()-1));
00439   }
00440   //! Adjusts the size of the particle collection -- more particles gives better coverage, but more computation
00441   /*! You may wish to shrink the number of particles when the confidence interval is small or
00442    *  particle weights are high, and increase particles when the filter is getting "lost". */
00443   virtual void resizeParticles(unsigned int numParticles) {
00444     std::cerr << "Resizing particles from " << particles.size() << " to " << numParticles << std::endl;
00445     if(numParticles > particles.size()) {
00446       index_t oldsize=particles.size();
00447       particles.resize(numParticles);
00448       if(resampler!=NULL)
00449         resampler->getDistributionPolicy().randomize(&particles[oldsize],numParticles-oldsize);
00450       
00451     } else if(numParticles < particles.size()) {
00452       std::vector<particle_type*> sorted(particles.size());
00453       for(unsigned int i=0; i<particles.size(); ++i)
00454         sorted[i]=&particles[i];
00455       std::sort(sorted.begin(),sorted.end(),weightLess);
00456       particle_collection newParticles;
00457       newParticles.reserve(numParticles);
00458       for(unsigned int i=sorted.size()-numParticles-1; i<sorted.size(); ++i)
00459         newParticles.push_back(*sorted[i]);
00460       particles.swap(newParticles);
00461     }
00462   }
00463   
00464 protected:
00465   //!< used for sorting particles in resizeParticles() to drop the least weighted particles first
00466   static bool weightLess(const particle_type* a, const particle_type* b) { return a->weight < b->weight; }
00467   
00468   particle_collection particles; //!< storage of the particles (no particular order)
00469   index_t bestIndex; //!< index of the currently highest-rated particle
00470   MotionModel * motion; //!< motion model, can be NULL if you have no control or knowledge of changes in the system
00471   ResamplingPolicy * resampler; //!< resampling policy refocuses filter on "good" particles, can be NULL but filter won't work well without a resampler
00472   bool hasEvaluation; //!< set to true following each call to updateSensors, and false following resample() or resetWeights(); avoids repeated resamplings
00473   
00474 private:
00475   ParticleFilter(const ParticleFilter&); //!< don't call (copy constructor)
00476   ParticleFilter& operator=(const ParticleFilter&); //!< don't call (assignment operator)
00477 };
00478 
00479 template<typename ParticleT>
00480 void ParticleFilter<ParticleT>::LowVarianceResamplingPolicy::resample(particle_collection& particles, index_t& bestIndex) {
00481   if(resampleCount++<resampleDelay)
00482     return;
00483   resampleCount=0;
00484   //std::cerr << "RESAMPLE UNDERWAY" << std::endl;
00485   //std::cerr << "Best particle is " << bestIndex << " @ " << particles[bestIndex].weight << std::endl;
00486   
00487   // we reuse newParticles each time, doing an STL O(1) swap to quickly exchange contents
00488   // have to make sure it's still the right size though...
00489   if(newParticles.size()<particles.size() || newParticles.size()>particles.size()*2)
00490     newParticles.resize(particles.size());
00491   if(particles.size()==0)
00492     return;
00493   
00494   // This part figures out how many particles we're going to globally redistribute,
00495   // leaving the rest for resampling
00496   float bestWeight = particles[bestIndex].weight;
00497   float redistributeRatio = 0;
00498   if(logWeights) {
00499     if(bestWeight<=-FLT_MAX)
00500       redistributeRatio=1;
00501     else {
00502       float min=minAcceptableWeight*(resampleDelay+1);
00503       if(bestWeight<min) {
00504         redistributeRatio = (bestWeight-min)/min;
00505         if(redistributeRatio>1)
00506           redistributeRatio=1;
00507       }
00508     }
00509   } else {
00510     if(bestWeight<=0)
00511       redistributeRatio=1;
00512     else {
00513       float min=std::pow(minAcceptableWeight,(float)(resampleDelay+1));
00514       if(bestWeight<min)
00515         redistributeRatio = (1-bestWeight/min);
00516     }
00517   }
00518   unsigned int numRedistribute = (unsigned int)(particles.size() * redistributeRatio * maxRedistribute);
00519   
00520   
00521   // now do resampling, writing into newParticles
00522   const unsigned int numResample=newParticles.size()-numRedistribute;
00523   //std::cerr << "best " << bestIndex << " @ " << bestWeight << " numRedist. " << numRedistribute << " of " << particles.size() << std::endl;
00524   bestIndex=0;
00525   if(numResample>0) {
00526     // add up the cumulative weights for each particle...
00527     std::vector<float> weights(particles.size());
00528     if(logWeights) {
00529       weights[0]=std::exp(particles.front().weight-bestWeight);
00530       for (unsigned int i=1; i < particles.size(); i++)
00531         weights[i] = weights[i-1] + std::exp(particles[i].weight-bestWeight);
00532     } else {
00533       weights[0]=particles.front().weight/bestWeight;
00534       for (unsigned int i=1; i < particles.size(); i++)
00535         weights[i] = weights[i-1] + particles[i].weight/bestWeight;
00536     }
00537     if(weights.back()<=0) {
00538       std::cerr << "Warning particle filter attempted resampling with weight total " << weights.back() << std::endl;
00539       return;
00540     }
00541     
00542     float r = weights.back() / numResample; // last element of weights/number of particles
00543     float offset = r*float(rand())/RAND_MAX;
00544     unsigned int pos = 0;
00545     
00546     //unsigned int lpos=0, fi=0;
00547     for (unsigned int i=0; i < numResample; i++){
00548       float target = offset+r*i; // multiply instead of repeatedly adding to avoid rounding issues
00549       while (target >= weights[pos]) {
00550         pos++;
00551 /*#ifdef DEBUG
00552         if(pos>=particles.size()) {
00553           std::cerr << std::endl << std::endl << " WTF: ParticleFilter pos past end of particles " << std::endl << std::endl << std::endl;
00554           pos=particles.size()-1;
00555           break;
00556         }
00557 #endif*/
00558       }
00559       // debugging output (display which particles got sampled)
00560       /*if(lpos!=pos) {
00561         if(fi!=i)
00562           std::cerr << "selected " << lpos << " x" << i-fi << " @ " << particles[lpos].weight << std::endl;
00563         fi=i; lpos=pos;
00564       }*/
00565       //std::cerr << "copying particle " << pos << " of " << particles.size() << " to position " << i << " of " << newParticles.size() << std::endl;
00566       
00567       // copy over particle (we'll "jiggle" it later if desired)
00568       newParticles[i]=particles[pos];
00569       // keep track of index of best particle in the new list
00570       if(newParticles[i].weight>newParticles[bestIndex].weight)
00571         bestIndex=i;
00572     }
00573     // debugging output (display which particles got sampled)
00574     //std::cerr << "selected " << lpos << " x" << numResample-fi << " @ " << particles[lpos].weight << std::endl;
00575     
00576     // now jiggle all of the particles we've resampled
00577     if(varianceScale!=0) {
00578       float vs;
00579       if(varianceScale>=0)
00580         vs=varianceScale; // use the user's setting
00581       // otherwise varianceScale is negative, we'll try to pick a variance scale based on how well we're doing
00582       else if(redistributeRatio>0)
00583         vs=1+redistributeRatio*(-varianceScale-1);
00584       else {
00585         if(logWeights) {
00586           float min=minAcceptableWeight*(resampleDelay+1);
00587           vs=bestWeight/min;
00588         } else {
00589           float min=std::pow(minAcceptableWeight,(float)(resampleDelay+1));
00590           vs=(1-bestWeight)/(1-min);
00591         }
00592         //vs=std::pow(vs,4.f);
00593       }
00594       //std::cerr << "variance scale is " << vs << std::endl;
00595       ResamplingPolicy::dist->jiggle(vs,&newParticles[0],numResample);
00596     }
00597   }
00598   
00599   // finished resampling, redistribute the remaining particles (if needed due to falling below minAcceptableWeight)
00600   ResamplingPolicy::dist->randomize(&newParticles[numResample],numRedistribute);
00601   
00602   // reset weights
00603   if(logWeights) {
00604     for(typename particle_collection::iterator it=newParticles.begin(); it!=newParticles.end(); ++it)
00605       it->weight=0;
00606   } else {
00607     for(typename particle_collection::iterator it=newParticles.begin(); it!=newParticles.end(); ++it)
00608       it->weight=1;
00609   }
00610   
00611   particles.swap(newParticles); // all done!  swap the particle lists
00612 }
00613 
00614 /*! @file
00615 * @brief 
00616 * @author ejt (Creator)
00617 *
00618 * $Author: ejt $
00619 * $Name: tekkotsu-4_0 $
00620 * $Revision: 1.14 $
00621 * $State: Exp $
00622 * $Date: 2007/11/10 22:58:10 $
00623 */
00624 
00625 #endif

Tekkotsu v4.0
Generated Thu Nov 22 00:54:54 2007 by Doxygen 1.5.4