00001
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
00023
00024
00025
00026
00027 class ParticleShapeEvaluator {
00028 public:
00029
00030 ParticleShapeEvaluator(ShapeSpace &localShS, ShapeSpace &worldShS);
00031 virtual ~ParticleShapeEvaluator() {}
00032
00033
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;
00041 std::vector<PfRoot*> worldLms;
00042
00043 float maxDist;
00044
00045 std::vector<int> localMatches;
00046 unsigned int numMatches;
00047 std::vector<float> localScores;
00048
00049
00050 static float distanceFromLine(coordinate_t x0, coordinate_t y0, PfLine &wline);
00051
00052 std::vector<float> particleViewX;
00053 std::vector<float> particleViewY;
00054 std::vector<float> particleViewX2;
00055 std::vector<float> particleViewY2;
00056 };
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068 template<typename ParticleT>
00069 class ShapeSensorModel : public ParticleFilter<ParticleT>::SensorModel {
00070 public:
00071 typedef typename ParticleFilter<ParticleT>::SensorModel::index_t index_t;
00072 typedef typename ParticleFilter<ParticleT>::SensorModel::particle_collection particle_collection;
00073
00074
00075 ShapeSensorModel(ShapeSpace &localShS, ShapeSpace &worldShS) :
00076 stdevSq(60*60), lShS(localShS), wShS(worldShS)
00077 {}
00078
00079
00080 float stdevSq;
00081
00082
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;
00107 ShapeSpace &wShS;
00108
00109
00110
00111
00112 inline float normpdf(float const distsq) { return std::exp(-distsq/stdevSq); }
00113 };
00114
00115
00116
00117
00118 template<typename ParticleT>
00119 class PFShapeDistributionPolicy : public LocalizationParticleDistributionPolicy<ParticleT> {
00120 public:
00121 typedef ParticleT particle_type;
00122 typedef typename ParticleFilter<particle_type>::index_t index_t;
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) {
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;
00177 };
00178
00179
00180
00181
00182
00183 class PFShapeLocalization : public ParticleFilter<LocalizationParticle> {
00184 public:
00185
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
00196 virtual ~PFShapeLocalization() {
00197 if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion)) {
00198 motBeh->DoStop();
00199
00200 motion=NULL;
00201 }
00202 delete sensorModel;
00203 }
00204
00205
00206 virtual void update(bool updateMot=true, bool doResample=true) { updateSensors(*sensorModel,updateMot,doResample); }
00207
00208
00209 virtual ShapeSensorModel<LocalizationParticle>& getSensorModel() const { return *sensorModel; }
00210
00211
00212 virtual void setSensorModel(ShapeSensorModel<LocalizationParticle>* customSensorModel) {
00213 delete sensorModel;
00214 sensorModel=customSensorModel;
00215 }
00216
00217
00218 virtual void setAgent() const;
00219
00220
00221 virtual void setPosition(float const x, float const y, AngTwoPi const orientation, float variance=0);
00222 using ParticleFilter<LocalizationParticle>::setPosition;
00223
00224
00225 virtual void setWorldBounds(const Shape<PolygonData> &bounds);
00226
00227
00228 virtual void displayParticles(float const howmany=100) const;
00229
00230 protected:
00231 ShapeSensorModel<LocalizationParticle> * sensorModel;
00232
00233 private:
00234 PFShapeLocalization(const PFShapeLocalization&);
00235 PFShapeLocalization& operator=(const PFShapeLocalization&);
00236 };
00237
00238 }
00239
00240 #endif