Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PFShapeLocalization.h

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 #ifndef _LOADED_PFShapeLocalization_h_
00004 #define _LOADED_PFShapeLocalization_h_
00005 
00006 #include <vector>
00007 #include <iostream>
00008 #include <cmath>
00009 #include "Shared/LocalizationParticle.h"
00010 #include "ParticleShapes.h"
00011 #include "Behaviors/Services/DeadReckoningBehavior.h"
00012 #include "DualCoding/ShapePolygon.h"
00013 
00014 #ifndef USE_LOGWEIGHTS
00015 #  define USE_LOGWEIGHTS 1
00016 #endif
00017 
00018 namespace DualCoding {
00019   
00020 class ShapeSpace;
00021 
00022   //! provides evaluation of the matching between local and world given a candidate particle
00023   /*! The reason for separating ParticleShapeEvaluator and ShapeSensorModel?  Partly so the
00024    *  fairly lengthy evaluation code can go in the .cc file to avoid repeated recompilation, but also to
00025    *  allow inheritance (e.g. SLAMShapeParticleEvaluator) as a clean way to extend the 
00026    *  evaluation code for particle sub-types. */
00027 class ParticleShapeEvaluator {
00028 public:
00029     //! constructor, pass the local and world shape spaces, these will be used to initialize the appropriate particle-independent fields of the class
00030   ParticleShapeEvaluator(ShapeSpace &localShS, ShapeSpace &worldShS);
00031   virtual ~ParticleShapeEvaluator() {} //!< destructor
00032   
00033   //! the heart of the class, call with a particle, will adjust the weight
00034   void evaluate(LocalizationParticle& part);
00035   
00036   void setMaxDist(float const dist) { maxDist = dist; }
00037   const std::vector<float>& getLocalScores() const { return localScores; }
00038   unsigned int getNumMatches() const { return numMatches; }
00039 
00040   std::vector<PfRoot*> localLms; //!< a vector of the landmarks in the local space
00041   std::vector<PfRoot*> worldLms; //!< a vector of landmarks in the world space
00042   
00043   float maxDist; //!< maximum distance for a landmark to be useful in distance error calculation
00044 
00045   std::vector<int> localMatches;    //!< Index of best matching world landmark for each local landmark according to the currently-selected particle
00046   unsigned int numMatches; //!< number of matches found
00047   std::vector<float> localScores;   //!< Match scores for local landmarks according to this particle, only first #numMatches are filled in (skip non-matching)
00048   
00049   //! helper function which calculates the distance between a point and a line along a perpendicular
00050   static float distanceFromLine(coordinate_t x0, coordinate_t y0, PfLine &wline);
00051   
00052   std::vector<float> particleViewX;   //!< x coords of local landmarks according to the currently-selected particle
00053   std::vector<float> particleViewY;   //!< y coords of local landmarks according to the currently-selected particle
00054   std::vector<float> particleViewX2;    //!< x coords of other point of local line
00055   std::vector<float> particleViewY2;    //!< y coords of other point of local line
00056 };
00057   
00058 
00059 
00060 //================ ShapeSensorModel ================
00061 
00062 //! this wraps the ParticleShapeEvaluator in a ParticleFilter::SensorModel so it can be used by the particle filter
00063 /*! The reason for separating ParticleShapeEvaluator and ShapeSensorModel?  Partly so the
00064  *  fairly length evaluation code can go in the .cc file to avoid repeated recompilation, but also to
00065  *  allow inheritance (e.g. SLAMShapesParticleEvaluator) as a clean way to extend the 
00066  *  evaluation code for particle sub-types.  Ideally, I'd like to combine these classes. */
00067 
00068 template<typename ParticleT>
00069 class ShapeSensorModel : public ParticleFilter<ParticleT>::SensorModel {
00070 public:
00071   typedef typename ParticleFilter<ParticleT>::SensorModel::index_t index_t; //!< convenience typedef
00072   typedef typename ParticleFilter<ParticleT>::SensorModel::particle_collection particle_collection; //!< convenience typedef
00073   
00074   //! constructor, the standard deviation on matches defaults to 60, but you can always reassign #stdevSq directly
00075   ShapeSensorModel(ShapeSpace &localShS, ShapeSpace &worldShS) :
00076     stdevSq(60*60), lShS(localShS), wShS(worldShS)
00077   {}
00078   
00079   //! controls how much weight is given to "near-misses"
00080   float stdevSq;
00081   
00082   //! applies the ParticleShapeEvaluator across a collection of particles and tracks the best weight
00083   virtual void evaluate(particle_collection& particles, index_t& bestIndex) {
00084     float bestWeight=-FLT_MAX;
00085     ParticleShapeEvaluator eval(lShS,wShS);
00086     for(typename particle_collection::size_type p=0; p<particles.size(); ++p) {
00087       eval.evaluate(particles[p]);
00088       for(unsigned int i=0; i<eval.getNumMatches(); ++i) {
00089 #if USE_LOGWEIGHTS
00090   particles[p].weight += -eval.getLocalScores()[i]/stdevSq;
00091 #else
00092   particles[p].weight *= normpdf(eval.getLocalScores()[i]);
00093 #endif
00094       }
00095       if(particles[p].weight>bestWeight) {
00096   bestWeight=particles[p].weight;
00097   bestIndex=p;
00098       }
00099     }
00100   }
00101   
00102   ShapeSpace& getLocalShS() const { return lShS; }
00103   ShapeSpace& getWorldShS() const { return wShS; }
00104 
00105 protected:
00106   ShapeSpace &lShS;     //!< Local shape space
00107   ShapeSpace &wShS;     //!< World shape space
00108   
00109   //!< computes a (non-normalized) gaussian distribution
00110   /*! normalization isn't needed because the scale factor is constant across particles, and so
00111    *  doesn't matter for purposes of comparison between particles */
00112   inline float normpdf(float const distsq) { return std::exp(-distsq/stdevSq); }
00113 };
00114   
00115 
00116 //================ PFShapeDistributionPolicy ================
00117 
00118 template<typename ParticleT>
00119 class PFShapeDistributionPolicy : public LocalizationParticleDistributionPolicy<ParticleT> {
00120 public:
00121   typedef ParticleT particle_type;  //!< just for convenience
00122   typedef typename ParticleFilter<particle_type>::index_t index_t; //!< just for convenience
00123 
00124   PFShapeDistributionPolicy() : LocalizationParticleDistributionPolicy<ParticleT>(), worldBounds() {}
00125 
00126   virtual void randomize(particle_type* begin, index_t num) {
00127     particle_type* end=&begin[num]; 
00128     while(begin!=end) { 
00129       while (1) { // loop until particle is acceptable
00130   begin->x = float(rand())/RAND_MAX * LocalizationParticleDistributionPolicy<ParticleT>::mapWidth + 
00131     LocalizationParticleDistributionPolicy<ParticleT>::mapMinX;
00132   begin->y = float(rand())/RAND_MAX * LocalizationParticleDistributionPolicy<ParticleT>::mapHeight + 
00133     LocalizationParticleDistributionPolicy<ParticleT>::mapMinY;
00134   if ( !worldBounds.isValid() || worldBounds->isInside(Point(begin->x,begin->y)) )
00135     break;
00136       }
00137       begin->theta = float(rand())/RAND_MAX * 2 * M_PI;
00138       ++begin;
00139     }
00140   }
00141 
00142   virtual void jiggle(float var, particle_type* begin, index_t num) {
00143     if(var==0)
00144       return;
00145     particle_type* end=&begin[num]; 
00146     while(begin!=end) {
00147       float dx=0, dy=0;
00148       for (int i=0; i<4; i++) {
00149   float rx = DRanNormalZig32()*LocalizationParticleDistributionPolicy<ParticleT>::positionVariance*var;
00150   float ry = DRanNormalZig32()*LocalizationParticleDistributionPolicy<ParticleT>::positionVariance*var;
00151   if ( !worldBounds.isValid() || worldBounds->isInside(Point(begin->x+rx, begin->y+ry)) ) {
00152     dx = rx;
00153     dy = ry;
00154     break;
00155   }
00156       }
00157       begin->x += dx;
00158       begin->y += dy;
00159       begin->theta+=DRanNormalZig32()*LocalizationParticleDistributionPolicy<ParticleT>::orientationVariance*var;
00160       ++begin;
00161     }
00162   }
00163 
00164   virtual void setWorldBounds(const Shape<PolygonData> bounds) {
00165     worldBounds = bounds;
00166     if ( worldBounds.isValid() ) {
00167       BoundingBox b(worldBounds->getBoundingBox());
00168       LocalizationParticleDistributionPolicy<ParticleT>::mapMinX = b.xmin;
00169       LocalizationParticleDistributionPolicy<ParticleT>::mapWidth = b.xmax - b.xmin;
00170       LocalizationParticleDistributionPolicy<ParticleT>::mapMinY = b.ymin;
00171       LocalizationParticleDistributionPolicy<ParticleT>::mapHeight = b.ymax - b.ymin;
00172     }
00173   }
00174 
00175 protected:
00176   Shape<PolygonData> worldBounds; //!< If valid shape, particles must lie inside it.
00177 };
00178 
00179 //================ PFShapeLocalization ================
00180 
00181 //! bundles a DeadReckoning motion model and a ShapeSensorModel for easy use of a shape-based particle filter for localization
00182 
00183 class PFShapeLocalization : public ParticleFilter<LocalizationParticle> {
00184 public:
00185   //! constructor, must pass local and world shape spaces, which will be used in future calls to update()
00186   PFShapeLocalization(ShapeSpace &localShS, ShapeSpace &worldShS, unsigned int numParticles=2000)
00187     : ParticleFilter<LocalizationParticle>(numParticles, new DeadReckoningBehavior<LocalizationParticle>),
00188       sensorModel(new ShapeSensorModel<LocalizationParticle>(localShS,worldShS))
00189   {
00190     getResamplingPolicy()->setDistributionPolicy(new PFShapeDistributionPolicy<LocalizationParticle>);
00191     if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion))
00192       motBeh->DoStart();
00193   }
00194 
00195   //! destructor
00196   virtual ~PFShapeLocalization() { 
00197     if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion)) {
00198       motBeh->DoStop();
00199       // behaviors are reference counted, stopping removes our reference, set to NULL to avoid call to delete in ParticleFilter
00200       motion=NULL;
00201     }
00202     delete sensorModel;
00203   }
00204   
00205   //! update, triggers a particle filter update using the embedded #sensorModel
00206   virtual void update(bool updateMot=true, bool doResample=true) { updateSensors(*sensorModel,updateMot,doResample); }
00207   
00208   //! accessor for #sensorModel
00209   virtual ShapeSensorModel<LocalizationParticle>& getSensorModel() const { return *sensorModel; }
00210 
00211   //! replaces the sensor model in use, the particle filter will take responsibility for deallocating the sensor model's memory when destructed or replaced
00212   virtual void setSensorModel(ShapeSensorModel<LocalizationParticle>* customSensorModel) {
00213     delete sensorModel; 
00214     sensorModel=customSensorModel;
00215   }
00216 
00217   //! updates the mapbuilder's agent's position on worldShS
00218   virtual void setAgent() const;
00219   
00220   //! resets particles to the specified position and orientation, and optionally jiggles them by variance
00221   virtual void setPosition(float const x, float const y, AngTwoPi const orientation, float variance=0);
00222   using ParticleFilter<LocalizationParticle>::setPosition;
00223 
00224   //! sets boundary within which particles should lie
00225   virtual void setWorldBounds(const Shape<PolygonData> &bounds);
00226 
00227   //! displays particles on the world map; howmany can either be a percentage (<= 1.0) or a whole number
00228   virtual void displayParticles(float const howmany=100) const;
00229 
00230 protected:
00231   ShapeSensorModel<LocalizationParticle> * sensorModel; //!< provides evaluation of particles
00232   
00233 private:
00234   PFShapeLocalization(const PFShapeLocalization&); //!< don't call (copy constructor)
00235   PFShapeLocalization& operator=(const PFShapeLocalization&); //!< don't call (assignment operator)
00236 };
00237 
00238 } // namespace
00239 
00240 #endif

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