Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PFShapeSLAM.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 #ifndef _LOADED_PFShapeSLAM_h_
00004 #define _LOADED_PFShapeSLAM_h_
00005 
00006 #include <vector>
00007 #include <iostream>
00008 #include <cmath>
00009 #include "Behaviors/Services/DeadReckoningBehavior.h"
00010 #include "Shared/LocalizationParticle.h"
00011 
00012 #include "PFShapeLocalization.h"
00013 
00014 #ifndef USE_LOGWEIGHTS
00015 #  define USE_LOGWEIGHTS 1
00016 #endif
00017 
00018 namespace DualCoding {
00019   
00020   class ShapeSpace;
00021   class SLAMShapesParticleDistributionPolicy;
00022 
00023   //! Each Particle represents a hypothesis about the match of the local map to the world map, considering changes to the map
00024   /*! Only handles adding or removing landmarks, not moving existing landmarks.  (A move
00025    *  can be modeled as an addition plus a deletion, but there will be two penalties to pay.)
00026    *
00027    *  Note that the particle evaluation doesn't modify the world map, only
00028    *  tracks suggestions about what @e should be changed.
00029    *
00030    *  The size of addLocal and deleteWorld may not exactly match the number
00031    *  of current local or world landmarks -- the SensorModel is lazy about resizing
00032    *  for efficiency reasons. */
00033   class SLAMShapesParticle : public LocalizationParticle {
00034   public:
00035     //! Defines a default DistributionPolicy for the particle type (just reuse the same one as LocalizationParticle)
00036     typedef LocalizationParticleDistributionPolicy<SLAMShapesParticle> DistributionPolicy;
00037 
00038     //! Constructor
00039     SLAMShapesParticle() : LocalizationParticle(), addLocal(), deleteWorld() {}
00040   
00041     std::vector<bool> addLocal; //!< true for local landmarks missing from the world map
00042     std::vector<bool> deleteWorld;  //!< true for world landmarks missing from the local map
00043   };
00044 
00045   //! extends ParticleShapeEvaluator to handle the addition and removal of landmarks as necessary
00046   class SLAMParticleShapeEvaluator : public ParticleShapeEvaluator {
00047   public:
00048     //! constructor, addPenalty specifies how much to add/multiply (logspace/linear space) weight when considering a landmark as an addition
00049     SLAMParticleShapeEvaluator(ShapeSpace &localShS, ShapeSpace &worldShS, float addPenalty) :
00050       ParticleShapeEvaluator(localShS,worldShS), localMobile(false), worldMobile(false), ADDITION_PENALTY(addPenalty)
00051     {
00052       for ( unsigned int j=0; j<localLms.size(); j++ )
00053   localMobile |= localLms[j]->mobile;
00054       for ( unsigned int k=0; k<worldLms.size(); k++ )
00055   worldMobile |= worldLms[k]->mobile;
00056     }
00057     using ParticleShapeEvaluator::evaluate;
00058     void evaluate(SLAMShapesParticle& part); //!< provides evaluation of SLAM-particles
00059   protected:
00060     void determineAdditions(SLAMShapesParticle& part); //!< may mark landmarks for addition which don't appear in the world map
00061     void determineDeletions(SLAMShapesParticle& part); //!< may mark landmarks for removal which don't appear in the world map
00062     bool localMobile; //!< set to true if *any* landmarks are marked as "mobile"
00063     bool worldMobile; //!< set to true if *any* landmarks are marked as "mobile"
00064     const float ADDITION_PENALTY; //!< the value passed to the constructor, limits how readily landmarks are added to the map
00065   };
00066 
00067 
00068 //================ SLAMShapesSensorModel ================
00069 
00070   //! this wraps the SLAMParticleShapeEvaluator in a ParticleFilter::SensorModel so it can be used by the particle filter
00071   /*! see ShapeSensorModel for discussion of architectural issues vs separation of SLAMParticleShapeEvaluator */
00072   template<typename ParticleT>
00073   class SLAMShapesSensorModel : public ParticleFilter<ParticleT>::SensorModel {
00074   public:
00075     typedef typename ParticleFilter<ParticleT>::SensorModel::index_t index_t; //!< convenience typedef
00076     typedef typename ParticleFilter<ParticleT>::SensorModel::particle_collection particle_collection; //!< convenience typedef
00077   
00078     //! constructor, the standard deviation on matches defaults to 60, but you can always reassign #stdevSq directly
00079     SLAMShapesSensorModel(ShapeSpace &localShS, ShapeSpace &worldShS) :
00080       stdevSq(60*60), addPenalty(50), lShS(localShS), wShS(worldShS),
00081       particleLocalLandmarks(0), particleWorldLandmarks(0)
00082     {}
00083   
00084     //! controls how much weight is given to "near-misses"
00085     float stdevSq;
00086     //! controls how readily new landmarks are added to the map, vs. penalizing the particle for a bad match
00087     float addPenalty;
00088   
00089     //! applies the SLAMParticleShapeEvaluator across a collection of particles and tracks the best weight
00090     virtual void evaluate(particle_collection& particles, index_t& bestIndex) {
00091       float bestWeight=-FLT_MAX;
00092       SLAMParticleShapeEvaluator eval(lShS,wShS,addPenalty);
00093     
00094       if(eval.localLms.size()>particleLocalLandmarks || eval.localLms.size()<particleLocalLandmarks/2)
00095   for(typename particle_collection::iterator it=particles.begin(); it!=particles.end(); ++it)
00096     it->addLocal.resize(particleLocalLandmarks);
00097     
00098       if(eval.worldLms.size()>particleWorldLandmarks || eval.worldLms.size()<particleWorldLandmarks/2)
00099   for(typename particle_collection::iterator it=particles.begin(); it!=particles.end(); ++it)
00100     it->deleteWorld.resize(particleWorldLandmarks);
00101     
00102       for(typename particle_collection::size_type p=0; p<particles.size(); ++p) {
00103   eval.evaluate(particles[p]);
00104   for(unsigned int i=0; i<eval.numMatches; ++i) {
00105 #if USE_LOGWEIGHTS
00106     particles[p].weight += -eval.localScores[i]/stdevSq;
00107 #else
00108     particles[p].weight *= normpdf(eval.localScores[i]);
00109 #endif
00110   }
00111   if(particles[p].weight>bestWeight) {
00112     bestWeight=particles[p].weight;
00113     bestIndex=p;
00114   }
00115       }
00116     }
00117   
00118   ShapeSpace& getLocalShS() const { return lShS; }
00119   ShapeSpace& getWorldShS() const { return wShS; }
00120 
00121   protected:
00122     ShapeSpace &lShS;     //!< Local shape space
00123     ShapeSpace &wShS;     //!< World shape space
00124 
00125     unsigned int particleLocalLandmarks; //!< number of entries in particles' individual addLocal (so we know if we need to resize it in all particles)
00126     unsigned int particleWorldLandmarks; //!<  number of entries in particles' individual deleteWorld (so we know if we need to resize it in all particles)
00127 
00128     //! computes a (non-normalized) gaussian distribution
00129     /*! normalization doesn't matter because it's constant across particles, and so
00130      *  doesn't matter for purposes of comparison between particles */
00131     inline float normpdf(float const distsq) { return std::exp(-distsq/stdevSq); }
00132   };
00133 
00134 
00135 //================ PFShapeSLAM ================
00136 
00137   //! bundles a DeadReckoning motion model and a SLAMShapesSensorModel for easy use of a shape-based particle filter for mapping and localization
00138   class PFShapeSLAM : public ParticleFilter<SLAMShapesParticle> {
00139   public:
00140     //! constructor, must pass local and world shape spaces, which will be used in future calls to update()
00141     PFShapeSLAM(ShapeSpace &localShS, ShapeSpace &worldShS, unsigned int numParticles=2000)
00142       : ParticleFilter<SLAMShapesParticle>(numParticles, new DeadReckoningBehavior<SLAMShapesParticle>),
00143   sensorModel(new SLAMShapesSensorModel<SLAMShapesParticle>(localShS,worldShS))
00144     {
00145       if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion))
00146   motBeh->DoStart();
00147     }
00148     //! destructor
00149     virtual ~PFShapeSLAM() {
00150       if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion)) {
00151   motBeh->DoStop();
00152   // behaviors are reference counted, stopping removes our reference, set to NULL to avoid call to delete in ParticleFilter
00153   motion=NULL;
00154       }
00155       delete sensorModel;
00156     }
00157   
00158     //! update, triggers a particle filter update using the embedded #sensorModel
00159     virtual void update(bool updateMot=true, bool doResample=true) { updateSensors(*sensorModel,updateMot,doResample); }
00160   
00161     //! accessor for #sensorModel
00162     virtual SLAMShapesSensorModel<SLAMShapesParticle>& getSensorModel() const { return *sensorModel; }
00163 
00164     //! replaces the sensor model in use, the particle filter will take responsibility for deallocating the sensor model's memory when destructed or replaced
00165     virtual void setSensorModel(SLAMShapesSensorModel<SLAMShapesParticle>* customSensorModel) { delete sensorModel; sensorModel=customSensorModel; }
00166 
00167     //! updates the mapbuilder's agent's position on worldShS
00168     virtual void setAgent() const;
00169   
00170     //! displays particles on the world map; howmany can either be a percentage (<= 1.0) or a whole number
00171     virtual void displayParticles(float const howmany=100) const;
00172 
00173   protected:
00174     SLAMShapesSensorModel<SLAMShapesParticle> * sensorModel; //!< provides evaluation of particles
00175 
00176   private:
00177     PFShapeSLAM(const PFShapeSLAM&); //!< don't call (copy constructor)
00178     PFShapeSLAM& operator=(const PFShapeSLAM&); //!< don't call (assignment operator)
00179   };
00180 
00181   //! allows display of particles on console
00182   std::ostream& operator<< (std::ostream& os, const SLAMShapesParticle &p);
00183 
00184 } // namespace
00185 
00186 #endif

DualCoding 4.0
Generated Thu Nov 22 00:52:36 2007 by Doxygen 1.5.4