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
00013 class LocalizationParticle : public ParticleBase {
00014 public:
00015 float x;
00016 float y;
00017 AngTwoPi theta;
00018
00019
00020 typedef LocalizationParticleDistributionPolicy<LocalizationParticle> DistributionPolicy;
00021
00022
00023 LocalizationParticle() : ParticleBase(), x(0), y(0), theta(0) {}
00024
00025
00026 LocalizationParticle(float xp, float yp, AngTwoPi tp) : ParticleBase(), x(xp), y(yp), theta(tp) {}
00027
00028
00029 template<typename ParticleT> float sumSqErr(const ParticleT& lp) const {
00030
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
00039 template<typename ParticleT>
00040 class LocalizationParticleDistributionPolicy : public ParticleFilter<ParticleT>::DistributionPolicy {
00041 public:
00042 typedef ParticleT particle_type;
00043 typedef typename ParticleFilter<ParticleT>::index_t index_t;
00044
00045 float mapMinX;
00046 float mapWidth;
00047 float mapMinY;
00048 float mapHeight;
00049 float positionVariance;
00050 float orientationVariance;
00051
00052
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
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
00077 ++begin;
00078 }
00079 }
00080 };
00081
00082
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
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105 #endif