Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

LocalizationParticle.h

Go to the documentation of this file.
00001 #ifndef INCLUDED_LocalizationParticle_h
00002 #define INCLUDED_LocalizationParticle_h
00003 
00004 #include "ParticleFilter.h"
00005 #include "Shared/Measures.h"
00006 #include "zignor.h"
00007 #include <iostream>
00008 #include <cmath>
00009 
00010 template<typename ParticleT> class LocalizationParticleDistributionPolicy;
00011 
00012 //! Each Particle represents a hypothesis about the match of the local map to the world map.
00013 class LocalizationParticle : public ParticleBase {
00014 public:
00015   float x; //!< X position of particle in world
00016   float y; //!< Y position of particle in world
00017   AngTwoPi theta; //!< Orientation of particle in world
00018   
00019   //! defines a default DistributionPolicy for the particle type
00020   typedef LocalizationParticleDistributionPolicy<LocalizationParticle> DistributionPolicy;
00021   
00022   //! constructor
00023   LocalizationParticle() : ParticleBase(), x(0), y(0), theta(0) {}
00024   
00025   //! constructor, allows you to define the particle's position
00026   LocalizationParticle(float xp, float yp, AngTwoPi tp) : ParticleBase(), x(xp), y(yp), theta(tp) {}
00027   
00028   //! returns a straightforward sum squared error of each of the fields
00029   template<typename ParticleT> float sumSqErr(const ParticleT& lp) const {
00030     //const LocalizationParticle& lp = static_cast<const LocalizationParticle&>(p);
00031     float dx=x-lp.x;
00032     float dy=y-lp.y;
00033     float dt=theta-lp.theta;
00034     return dx*dx+dy*dy+dt*dt;
00035   }
00036 };
00037 
00038 //! Provides parameters and methods for randomizing and tweaking LocalizationParticles
00039 template<typename ParticleT>
00040 class LocalizationParticleDistributionPolicy : public ParticleFilter<ParticleT>::DistributionPolicy {
00041 public:
00042   typedef ParticleT particle_type;  //!< just for convenience
00043   typedef typename ParticleFilter<ParticleT>::index_t index_t; //!< just for convenience
00044   
00045   float mapMinX; //!< specifies the low end of x coordinates during randomize()
00046   float mapWidth; //!< along with #mapMinX, specifies the range of x coordinates to be used during randomize()
00047   float mapMinY; //!< specifies the low end of y coordinates during randomize()
00048   float mapHeight; //!< along with #mapMinY, specifies the range of y coordinates to be used during randomize()
00049   float positionVariance; //!< controls how much the x and y parameters will be modified during jiggle()
00050   float orientationVariance; //!< controls how much the orientation (theta) parameter will be modified during jiggle()
00051   
00052   //! constructor -- by default, coordinates will range from -1000 to 1000 for x and y, with variance of 50 and 0.18 for position and orientation
00053   LocalizationParticleDistributionPolicy()
00054     : mapMinX(-1000), mapWidth(2000), mapMinY(-1000), mapHeight(2000),
00055     positionVariance(50), orientationVariance(0.18f)
00056   {}
00057   
00058   virtual void randomize(particle_type* begin, index_t num) {
00059     particle_type* end=&begin[num]; 
00060     while(begin!=end) { 
00061       begin->x = float(rand())/RAND_MAX * mapWidth + mapMinX;
00062       begin->y = float(rand())/RAND_MAX * mapHeight + mapMinY;
00063       begin->theta = float(rand())/RAND_MAX * 2 * M_PI;
00064       //(begin++)->randomize();
00065       ++begin;
00066     }
00067   }
00068   virtual void jiggle(float var, particle_type* begin, index_t num) {
00069     if(var==0)
00070       return;
00071     particle_type* end=&begin[num]; 
00072     while(begin!=end) {
00073       begin->x+=DRanNormalZig32()*positionVariance*var;
00074       begin->y+=DRanNormalZig32()*positionVariance*var;
00075       begin->theta+=DRanNormalZig32()*orientationVariance*var;
00076       //(begin++)->jiggle(var);
00077       ++begin;
00078     }
00079   }
00080 };
00081 
00082 //! dump a particle's state
00083 inline std::ostream& operator << (std::ostream& os, const LocalizationParticle &p) {
00084   os << "Particle(p=" << p.weight
00085      << ", dx=" << p.x
00086      << ", dy=" << p.y
00087      << ", th=" << p.theta
00088      << ")";
00089   return os;
00090 }
00091 
00092 
00093 
00094 /*! @file
00095 * @brief 
00096 * @author ejt (Creator)
00097 *
00098 * $Author: ejt $
00099 * $Name: tekkotsu-4_0 $
00100 * $Revision: 1.5 $
00101 * $State: Exp $
00102 * $Date: 2007/03/02 23:59:20 $
00103 */
00104 
00105 #endif

Tekkotsu v4.0
Generated Thu Nov 22 00:54:53 2007 by Doxygen 1.5.4