Tekkotsu Homepage | Demos | Overview | Downloads | Dev. Resources | Reference | Credits |
ParticleFilter.hGo 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 |