Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

ShapeBasedParticleFilter.h

Go to the documentation of this file.
00001 #ifndef _LOADED_ShapeBasedParticleFilter_h_
00002 #define _LOADED_ShapeBasedParticleFilter_h_
00003 
00004 #include <vector>
00005 #include <iostream>
00006 #include <cmath>
00007 
00008 #include "DualCoding/ShapePolygon.h"
00009 #include "Localization/LocalizationParticle.h"
00010 
00011 #include "Shared/RobotInfo.h"
00012 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00013 #  include "Localization/CreateMotionModel.h"
00014 #elif defined(TGT_IS_KOBUKI)
00015 #  include "Localization/KobukiMotionModel.h"
00016 #else
00017 #  include "Localization/DeadReckoningBehavior.h"
00018 #endif
00019 
00020 #include "ShapeSensorModel.h"
00021 
00022 namespace DualCoding {
00023   
00024   // Defined in this file: first ShapeParticleDistributionPolicy, then
00025   // ShapeBasedParticleFilter, which uses this policy
00026   
00027 
00028   //================ ShapeParticleDistributionPolicy ================
00029 
00030   template<typename ParticleT>
00031   class ShapeParticleDistributionPolicy : public LocalizationParticleDistributionPolicy<ParticleT> {
00032   public:
00033     typedef ParticleT particle_type;  //!< just for convenience
00034     typedef typename ParticleFilter<particle_type>::index_t index_t; //!< just for convenience
00035 
00036     ShapeParticleDistributionPolicy() : LocalizationParticleDistributionPolicy<ParticleT>(), worldBounds() {}
00037 
00038       // Override LocalizationParticleDistributionPolicy::randomize() so we can restrict particles
00039       // to fit within the worldBounds polygon if one is defined.
00040     virtual void randomize(particle_type* begin, index_t num) {
00041       particle_type* end=&begin[num]; 
00042       for(particle_type* p=begin; p!=end; p++) { 
00043         while (1) { // loop until particle is acceptable
00044           p->x = float(rand())/RAND_MAX * LocalizationParticleDistributionPolicy<ParticleT>::mapWidth + 
00045             LocalizationParticleDistributionPolicy<ParticleT>::mapMinX;
00046           p->y = float(rand())/RAND_MAX * LocalizationParticleDistributionPolicy<ParticleT>::mapHeight + 
00047             LocalizationParticleDistributionPolicy<ParticleT>::mapMinY;
00048           if ( !worldBounds.isValid() || worldBounds->isInside(Point(begin->x,begin->y)) )
00049             break;
00050         }
00051         p->theta = direction_t(rand())/RAND_MAX * 2 * direction_t(M_PI);
00052       }
00053     }
00054 
00055     virtual void jiggle(float var, particle_type* begin, index_t num) {
00056       if(var==0)
00057   return;
00058       particle_type* end=&begin[num]; 
00059       while(begin!=end) {
00060         float dx=0, dy=0;
00061         for (int i=0; i<4; i++) {
00062           float rx = (float)DRanNormalZig32()*LocalizationParticleDistributionPolicy<ParticleT>::positionVariance*var;
00063           float ry = (float)DRanNormalZig32()*LocalizationParticleDistributionPolicy<ParticleT>::positionVariance*var;
00064           if ( !worldBounds.isValid() || worldBounds->isInside(Point(begin->x+rx, begin->y+ry)) ) {
00065             dx = rx;
00066             dy = ry;
00067             break;
00068           }
00069         }
00070         begin->x += dx;
00071         begin->y += dy;
00072         begin->theta+=(float)DRanNormalZig32()*LocalizationParticleDistributionPolicy<ParticleT>::orientationVariance*var;
00073         ++begin;
00074       }
00075     }
00076 
00077     virtual void setWorldBounds(float minX, float width, float minY, float height) {
00078       LocalizationParticleDistributionPolicy<ParticleT>::mapMinX = minX;
00079       LocalizationParticleDistributionPolicy<ParticleT>::mapWidth = width;
00080       LocalizationParticleDistributionPolicy<ParticleT>::mapMinY = minY;
00081       LocalizationParticleDistributionPolicy<ParticleT>::mapHeight = height;
00082     }
00083 
00084     virtual void setWorldBounds(const Shape<PolygonData> bounds) {
00085       worldBounds = bounds;
00086       if ( worldBounds.isValid() ) {
00087         BoundingBox2D b(worldBounds->getBoundingBox());
00088         setWorldBounds(b.min[0], b.getDimension(0), b.min[1], b.getDimension(1));
00089       }
00090     }
00091 
00092   protected:
00093     Shape<PolygonData> worldBounds; //!< If valid shape, particles must lie inside it.
00094   };
00095 
00096   //================ ShapeBasedParticleFilter ================
00097 
00098   //! Bundles a motion model (DeadReckoningBehavior or CreateMotionModel) and a ShapeSensorModel for easy use of a shape-based particle filter for localization
00099 
00100   class ShapeBasedParticleFilter : public ParticleFilter<LocalizationParticle> {
00101   public:
00102     //! constructor, must pass local and world shape spaces, which will be used in future calls to update()
00103 #if defined(TGT_IS_CREATE) || defined(TGT_IS_CREATE2)
00104     ShapeBasedParticleFilter(ShapeSpace &camShS, ShapeSpace &localShS, ShapeSpace &worldShS, unsigned int numParticles=2000)
00105       : ParticleFilter<LocalizationParticle>(numParticles, new CreateMotionModel<LocalizationParticle>),
00106   sensorModel(new ShapeSensorModel<LocalizationParticle>(camShS,localShS,worldShS))
00107     {
00108       getResamplingPolicy()->setDistributionPolicy(new ShapeParticleDistributionPolicy<LocalizationParticle>);
00109       if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion))
00110   motBeh->start();
00111     }
00112 #elif defined(TGT_IS_KOBUKI)
00113     ShapeBasedParticleFilter(ShapeSpace &camShS, ShapeSpace &localShS, ShapeSpace &worldShS, unsigned int numParticles=2000)
00114       : ParticleFilter<LocalizationParticle>(numParticles, new KobukiMotionModel<LocalizationParticle>),
00115   sensorModel(new ShapeSensorModel<LocalizationParticle>(camShS,localShS,worldShS))
00116     {
00117       getResamplingPolicy()->setDistributionPolicy(new ShapeParticleDistributionPolicy<LocalizationParticle>);
00118       if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion))
00119   motBeh->start();
00120     }
00121 #else
00122     ShapeBasedParticleFilter(ShapeSpace &camShS, ShapeSpace &localShS, ShapeSpace &worldShS, unsigned int numParticles=2000)
00123       : ParticleFilter<LocalizationParticle>(numParticles, new DeadReckoningBehavior<LocalizationParticle>),
00124   sensorModel(new ShapeSensorModel<LocalizationParticle>(camShS,localShS,worldShS))
00125     {
00126       // std::cout<<"ShapeBasedParticleFilter using DeadReckoningBehavior\n";
00127       getResamplingPolicy()->setDistributionPolicy(new ShapeParticleDistributionPolicy<LocalizationParticle>);
00128       if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion))
00129   motBeh->start();
00130     }
00131 #endif
00132   
00133 
00134     //! destructor
00135     virtual ~ShapeBasedParticleFilter() { 
00136       if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion)) {
00137   motBeh->stop();
00138   // behaviors are reference counted, stopping removes our reference, set to NULL to avoid call to delete in ParticleFilter
00139   motion=NULL;
00140       }
00141       delete sensorModel;
00142     }
00143   
00144     virtual void updateFromLocal() {
00145       getSensorModel().updateFromLocal(particles, estimate);
00146     }
00147   
00148     virtual void updateFromCamera() {
00149       getSensorModel().updateFromCamera(particles, estimate);
00150     }
00151   
00152     //! accessor for #sensorModel
00153     virtual ShapeSensorModel<LocalizationParticle>& getSensorModel() const { return *sensorModel; }
00154 
00155     //! replaces the sensor model in use, the particle filter will take responsibility for deallocating the sensor model's memory when destructed or replaced
00156     virtual void setSensorModel(ShapeSensorModel<LocalizationParticle>* customSensorModel) {
00157       delete sensorModel; 
00158       sensorModel=customSensorModel;
00159     }
00160 
00161     using ParticleFilter<LocalizationParticle>::resetFilter;
00162 
00163     //! randomizes all the particles
00164     virtual void resetFilter();
00165   
00166     //! Adjusts the size of the particle collection -- more particles gives better coverage, but more computation
00167     /*! You may wish to shrink the number of particles when the confidence interval is small or
00168      *  particle weights are high, and increase particles when the filter is getting "lost". */
00169     virtual void resizeParticles(unsigned int numParticles);
00170 
00171     //! Synchs the estimate to the agent's current position and heading in worldShS. During odometry, the Pilot will use the estimate to update the agent's position. 
00172     virtual void synchEstimateToAgent();
00173   
00174     //! Resets particles to the specified position and orientation, and optionally jiggles them by variance
00175     virtual void setPosition(float const x, float const y, AngTwoPi const orientation, float variance=0);
00176     using ParticleFilter<LocalizationParticle>::setPosition;
00177 
00178     //! Computes the weighted variance in the position and heading estimates of the particle collection.
00179     virtual void computeVariance();
00180 
00181     //! Sets boundary within which particles should lie
00182     virtual void setWorldBounds(const Shape<PolygonData> &bounds);
00183 
00184     //! Sets boundary within which particles should lie
00185     virtual void setWorldBounds(float minX, float width, float minY, float height);
00186 
00187     //! Deletes particle shapes and "particle" GraphicsData object so we can prepare a new display
00188     static void deleteParticleDisplay(ShapeSpace &wShS);
00189 
00190     //! Displays particles on the world map using a GraphicsData shape; howmany can either be a percentage (<= 1.0) or a whole number
00191     virtual void displayParticles(float const howmany=100);
00192 
00193     //! Displays individual particles on the world map using LocalizationParticle shapes; howmany can either be a percentage (<= 1.0) or a whole number
00194     virtual void displayIndividualParticles(float const howmany=100);
00195 
00196   protected:
00197     ShapeSensorModel<LocalizationParticle> * sensorModel; //!< provides evaluation of particles
00198   
00199   private:
00200     ShapeBasedParticleFilter(const ShapeBasedParticleFilter&); //!< don't call (copy constructor)
00201     ShapeBasedParticleFilter& operator=(const ShapeBasedParticleFilter&); //!< don't call (assignment operator)
00202   }; 
00203 
00204 } // namespace
00205 
00206 #endif

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:50 2016 by Doxygen 1.6.3