Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

ParticleFilter.h Go to the documentation of this file. //-*-c++-*- #ifndef LOADED_ParticleFilter_h #define LOADED_ParticleFilter_h #include <vector> #include <string> #include "Particle.h" #include "ParticleShapes.h" #include "ShapePolygon.h" using namespace std; namespace DualCoding { //! ParticleFilter localizes the robot by matching local shapes against world shapes. /*! Create an instance of ParticleFilter, and set its parameters by modifying member variables such as num_particles and num_generations. Then call its localize() method to determine the robot's location. Subsequent calls will start with the current particle distribution unless you call uniformly_distribute() to randomize things. */ class ParticleFilter { public: static const int NUM_PARTICLES = 2000; static const int NUM_GENERATIONS = 15; static const int NUM_TRIES = 5; static const float INITIAL_XY_NOISE = 100; static const float INITIAL_THETA_NOISE = 30 * M_PI/180; static const float NOISE_REDUCTION_XY = 0.85; static const float NOISE_REDUCTION_T = 0.9; static const float INFINITE_DISTANCE = 10000000; static const float STDEV = 50; static const float ADDITION_PENALTY = 0.1; static const float PERCENT_RANDOM = 0.1; //!< Percent of random particles in each resampling ShapeSpace &localShS; //!< Local shape space ShapeSpace &worldShS; //!< World shape space int numParticles; //!< Number of particles to use int numGenerations; //!< Maximum number of generations of resampling int numTries; //!< Number of times to restart if no good initial guess float noiseFactorXY; //!< Scale of noise for translation parameters x,y float noiseFactorT; //!< Scale of noise for rotation parameter theta Shape<PolygonData> worldBounds; //!< If valid shape, particles must lie inside it. int nlocal; //!< Current number of local landmarks int nworld; //!< Current number of world landmarks //! Bound box of world shapes //@{ float xmin, xmax, xrange; float ymin, ymax, yrange; //@} vector<Particle> *curParticles; //!< Pointer to vector holding the current set of particles int bestIndex; //!< Index of highest scoring particle from most recent resampling. //private: vector<Particle> *newParticles; //!< Pointer to vector where new particles are generated by resampling vector<PfRoot*> *localLms; //!< Pointer to current vector of local landmarks vector<PfRoot*> *worldLms; //!< Pointer to current vector of world landmarks vector<float> particleViewX; //!< x coords of local landmarks according to the currently-selected particle vector<float> particleViewY; //!< y coords of local landmarks according to the currently-selected particle vector<float> particleViewX2; //!< x coords of other point of local line vector<float> particleViewY2; //!< y coords of other point of local line vector<float> localScores; //!< Match scores for local landmarks according to the currently-selected particle vector<int> localMatches; //!< Index of best matching world landmark for each local landmark according to the currently-selected particle public: ParticleFilter(ShapeSpace &LocalShS, ShapeSpace &WorldShS) : localShS(LocalShS), worldShS(WorldShS), numParticles(NUM_PARTICLES), numGenerations(NUM_GENERATIONS), numTries(NUM_TRIES), noiseFactorXY(INITIAL_XY_NOISE), noiseFactorT(INITIAL_THETA_NOISE), worldBounds(), nlocal(0), nworld(0), xmin(0), xmax(0), xrange(0), ymin(0), ymax(0), yrange(0), curParticles(NULL), bestIndex(-1), newParticles(NULL), localLms(), worldLms(), particleViewX(), particleViewY(), particleViewX2(), particleViewY2(), localScores(), localMatches() {}; //! Reset particle filter and restore default settings void reinitialize(); //! Constrain new particles to world boundaries defined by a polygon void setWorldBounds(const Shape<PolygonData> &poly) { worldBounds = poly; } //! Invoke the particle filter: determine best match of local view to world map. Returns index of best particle, or -1 if failure. int localize(); //! Uniformly distribute particles throughout the region spanned by the world landmarks void uniformlyDistribute(); //! Update the current particles when the robot moves void moveBy(float const xdelta, float const ydelta, AngPi const tdelta); //! Create two Particle vectors of length numParticles, stored in curParticles and newParticles. /*! This function is called once, automatically, by localize() to initialize the Particle vectors. It should only be called explicitly when the value of numParticles has changed. */ void makeParticles(); //private: //! Load the local and world landmarks from their shape spaces, and create or resize particles if necessary. void loadLms(); //! Set the size of each Particle's addLocal vector to nlocal, and resize some internal vectors void resizeParticles(); //! Set new Particle i to random dx, dy, and theta values. Called by uniformlyDistribute(). void randomizeNewParticle(int const i); //! Check particle scores, and randomize and repeat if no particle is close to a solution void getInitialGuess(); //! Calculate the score for each Particle p, leaving it in p.prob void computeParticleScores(); //! Set up particleView vectors based on the parameters of the i-th Particle void setParticleView(int const i); //! Match each local landmark against the world landmarks void computeLocalMatch(int const i, int const j); static float distanceFromLine(coordinate_t x, coordinate_t y, PfLine &wline); //! Generate newParticles from curParticles, then swap the two vectors and compute scores. void resample(); void spawnInto(int const i, int const j); void determineAdditions(int const i); // void determineDeletions(int const i); public: static inline float normpdf(float const distsq) { return exp(-distsq/(STDEV*STDEV)); } private: ParticleFilter& operator=(const ParticleFilter&); //!< don't call this ParticleFilter(const ParticleFilter&); //!< don't call this }; } // namespace #endif

DualCoding 3.0beta

Generated Wed Oct 4 00:01:54 2006 by Doxygen 1.4.7