00001
00002
00003 #ifndef _LOADED_PFShapeSLAM_h_
00004 #define _LOADED_PFShapeSLAM_h_
00005
00006 #include <vector>
00007 #include <iostream>
00008 #include <cmath>
00009 #include "Behaviors/Services/DeadReckoningBehavior.h"
00010 #include "Shared/LocalizationParticle.h"
00011
00012 #include "PFShapeLocalization.h"
00013
00014 #ifndef USE_LOGWEIGHTS
00015 # define USE_LOGWEIGHTS 1
00016 #endif
00017
00018 namespace DualCoding {
00019
00020 class ShapeSpace;
00021 class SLAMShapesParticleDistributionPolicy;
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 class SLAMShapesParticle : public LocalizationParticle {
00034 public:
00035
00036 typedef LocalizationParticleDistributionPolicy<SLAMShapesParticle> DistributionPolicy;
00037
00038
00039 SLAMShapesParticle() : LocalizationParticle(), addLocal(), deleteWorld() {}
00040
00041 std::vector<bool> addLocal;
00042 std::vector<bool> deleteWorld;
00043 };
00044
00045
00046 class SLAMParticleShapeEvaluator : public ParticleShapeEvaluator {
00047 public:
00048
00049 SLAMParticleShapeEvaluator(ShapeSpace &localShS, ShapeSpace &worldShS, float addPenalty) :
00050 ParticleShapeEvaluator(localShS,worldShS), localMobile(false), worldMobile(false), ADDITION_PENALTY(addPenalty)
00051 {
00052 for ( unsigned int j=0; j<localLms.size(); j++ )
00053 localMobile |= localLms[j]->mobile;
00054 for ( unsigned int k=0; k<worldLms.size(); k++ )
00055 worldMobile |= worldLms[k]->mobile;
00056 }
00057 using ParticleShapeEvaluator::evaluate;
00058 void evaluate(SLAMShapesParticle& part);
00059 protected:
00060 void determineAdditions(SLAMShapesParticle& part);
00061 void determineDeletions(SLAMShapesParticle& part);
00062 bool localMobile;
00063 bool worldMobile;
00064 const float ADDITION_PENALTY;
00065 };
00066
00067
00068
00069
00070
00071
00072 template<typename ParticleT>
00073 class SLAMShapesSensorModel : public ParticleFilter<ParticleT>::SensorModel {
00074 public:
00075 typedef typename ParticleFilter<ParticleT>::SensorModel::index_t index_t;
00076 typedef typename ParticleFilter<ParticleT>::SensorModel::particle_collection particle_collection;
00077
00078
00079 SLAMShapesSensorModel(ShapeSpace &localShS, ShapeSpace &worldShS) :
00080 stdevSq(60*60), addPenalty(50), lShS(localShS), wShS(worldShS),
00081 particleLocalLandmarks(0), particleWorldLandmarks(0)
00082 {}
00083
00084
00085 float stdevSq;
00086
00087 float addPenalty;
00088
00089
00090 virtual void evaluate(particle_collection& particles, index_t& bestIndex) {
00091 float bestWeight=-FLT_MAX;
00092 SLAMParticleShapeEvaluator eval(lShS,wShS,addPenalty);
00093
00094 if(eval.localLms.size()>particleLocalLandmarks || eval.localLms.size()<particleLocalLandmarks/2)
00095 for(typename particle_collection::iterator it=particles.begin(); it!=particles.end(); ++it)
00096 it->addLocal.resize(particleLocalLandmarks);
00097
00098 if(eval.worldLms.size()>particleWorldLandmarks || eval.worldLms.size()<particleWorldLandmarks/2)
00099 for(typename particle_collection::iterator it=particles.begin(); it!=particles.end(); ++it)
00100 it->deleteWorld.resize(particleWorldLandmarks);
00101
00102 for(typename particle_collection::size_type p=0; p<particles.size(); ++p) {
00103 eval.evaluate(particles[p]);
00104 for(unsigned int i=0; i<eval.numMatches; ++i) {
00105 #if USE_LOGWEIGHTS
00106 particles[p].weight += -eval.localScores[i]/stdevSq;
00107 #else
00108 particles[p].weight *= normpdf(eval.localScores[i]);
00109 #endif
00110 }
00111 if(particles[p].weight>bestWeight) {
00112 bestWeight=particles[p].weight;
00113 bestIndex=p;
00114 }
00115 }
00116 }
00117
00118 ShapeSpace& getLocalShS() const { return lShS; }
00119 ShapeSpace& getWorldShS() const { return wShS; }
00120
00121 protected:
00122 ShapeSpace &lShS;
00123 ShapeSpace &wShS;
00124
00125 unsigned int particleLocalLandmarks;
00126 unsigned int particleWorldLandmarks;
00127
00128
00129
00130
00131 inline float normpdf(float const distsq) { return std::exp(-distsq/stdevSq); }
00132 };
00133
00134
00135
00136
00137
00138 class PFShapeSLAM : public ParticleFilter<SLAMShapesParticle> {
00139 public:
00140
00141 PFShapeSLAM(ShapeSpace &localShS, ShapeSpace &worldShS, unsigned int numParticles=2000)
00142 : ParticleFilter<SLAMShapesParticle>(numParticles, new DeadReckoningBehavior<SLAMShapesParticle>),
00143 sensorModel(new SLAMShapesSensorModel<SLAMShapesParticle>(localShS,worldShS))
00144 {
00145 if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion))
00146 motBeh->DoStart();
00147 }
00148
00149 virtual ~PFShapeSLAM() {
00150 if(BehaviorBase* motBeh = dynamic_cast<BehaviorBase*>(motion)) {
00151 motBeh->DoStop();
00152
00153 motion=NULL;
00154 }
00155 delete sensorModel;
00156 }
00157
00158
00159 virtual void update(bool updateMot=true, bool doResample=true) { updateSensors(*sensorModel,updateMot,doResample); }
00160
00161
00162 virtual SLAMShapesSensorModel<SLAMShapesParticle>& getSensorModel() const { return *sensorModel; }
00163
00164
00165 virtual void setSensorModel(SLAMShapesSensorModel<SLAMShapesParticle>* customSensorModel) { delete sensorModel; sensorModel=customSensorModel; }
00166
00167
00168 virtual void setAgent() const;
00169
00170
00171 virtual void displayParticles(float const howmany=100) const;
00172
00173 protected:
00174 SLAMShapesSensorModel<SLAMShapesParticle> * sensorModel;
00175
00176 private:
00177 PFShapeSLAM(const PFShapeSLAM&);
00178 PFShapeSLAM& operator=(const PFShapeSLAM&);
00179 };
00180
00181
00182 std::ostream& operator<< (std::ostream& os, const SLAMShapesParticle &p);
00183
00184 }
00185
00186 #endif