00001
00002
00003 #include <iostream>
00004 #include <cmath>
00005
00006 #include "ShapeSpace.h"
00007 #include "MapBuilder.h"
00008 #include "PFShapeSLAM.h"
00009 #include "ShapeLocalizationParticle.h"
00010 #include "VRmixin.h"
00011
00012 using namespace std;
00013
00014 namespace DualCoding {
00015
00016 void SLAMParticleShapeEvaluator::evaluate(SLAMShapesParticle& part) {
00017 ParticleShapeEvaluator::evaluate(part);
00018 for(unsigned int i=0; i<localMatches.size(); i++)
00019 part.addLocal[i] = ( localMatches[i] == -1 && localLms[i]->mobile );
00020 if ( localMobile )
00021 determineAdditions(part);
00022 if ( worldMobile )
00023 determineDeletions(part);
00024
00025
00026 }
00027
00028 void SLAMParticleShapeEvaluator::determineAdditions(SLAMShapesParticle& part) {
00029 for (unsigned int j = 0; j<localLms.size(); j++) {
00030 if ( localLms[j]->mobile ) {
00031 float const randval = float(rand()) / (RAND_MAX*6);
00032 if (randval >= localScores[j]) {
00033 part.addLocal[j] = true;
00034 localScores[numMatches++] = ADDITION_PENALTY;
00035 localMatches[j] = -1;
00036 continue;
00037 }
00038 for (unsigned int j2 = (j+1); j2<localLms.size(); j2++) {
00039 if (localMatches[j2] != localMatches[j] || localMatches[j2] == -1)
00040 continue;
00041 if (localScores[j2] > localScores[j]) {
00042 part.addLocal[j] = true;
00043 localScores[numMatches++] = ADDITION_PENALTY;
00044 localMatches[j] = -1;
00045 } else {
00046 part.addLocal[j2] = true;
00047 localScores[numMatches++] = ADDITION_PENALTY;
00048 localMatches[j2] = -1;
00049 }
00050 }
00051 }
00052 }
00053 }
00054
00055 void SLAMParticleShapeEvaluator::determineDeletions(SLAMShapesParticle& part) {
00056 part.deleteWorld.assign(part.deleteWorld.size(),true);
00057
00058 float minXLoc = HUGE_VAL;
00059 float minYLoc = HUGE_VAL;
00060 float maxXLoc = -HUGE_VAL;
00061 float maxYLoc = -HUGE_VAL;
00062 for (unsigned int j = 0; j<localLms.size(); j++) {
00063 if ( localMatches[j] != -1 )
00064 part.deleteWorld[localMatches[j]] = false;
00065 if ( particleViewX[j] < minXLoc )
00066 minXLoc = particleViewX[j];
00067 else if (particleViewX[j] > maxXLoc)
00068 maxXLoc = particleViewX[j];
00069 if (particleViewY[j] < minYLoc)
00070 minYLoc = particleViewY[j];
00071 else if (particleViewY[j] > maxYLoc)
00072 maxYLoc = particleViewY[j];
00073 }
00074
00075 for (unsigned int k = 0; k<worldLms.size(); k++)
00076 if ( ! worldLms[k]->mobile ||
00077 !( worldLms[k]->x >= minXLoc && worldLms[k]->x <= maxXLoc &&
00078 worldLms[k]->y >= minYLoc && worldLms[k]->y <= maxYLoc ) )
00079 part.deleteWorld[k] = false;
00080 }
00081
00082
00083 void PFShapeSLAM::setAgent() const {
00084 const PFShapeSLAM::particle_type& best = particles[bestIndex];
00085 VRmixin::mapBuilder.setAgent(Point(best.x,best.y), best.theta);
00086 }
00087
00088 void PFShapeSLAM::displayParticles(float const howmany) const {
00089 ShapeSpace &wShS = sensorModel->getWorldShS();
00090 wShS.deleteShapes<LocalizationParticleData>();
00091 NEW_SHAPE(best, LocalizationParticleData, new LocalizationParticleData(wShS,particles[bestIndex]));
00092 best->setColor(VRmixin::mapBuilder.getAgent()->getColor());
00093 if ( howmany <= 0 ) return;
00094 int increment;
00095 if ( howmany <= 1.0 )
00096 increment = (int)ceil(1.0/howmany);
00097 else
00098 increment = (int)ceil(particles.size()/howmany);
00099 for (unsigned int i=0; i<particles.size(); i+=increment)
00100 if ( i != bestIndex ) {
00101 NEW_SHAPE(pt, LocalizationParticleData, new LocalizationParticleData(wShS,particles[i]));
00102 }
00103 }
00104
00105 ostream& operator << (ostream& os, const SLAMShapesParticle &p){
00106 os << "Particle(p=" << p.weight
00107 << ", dx=" << p.x
00108 << ", dy=" << p.y
00109 << ", th=" << p.theta;
00110
00111 os << ", add=";
00112 for (unsigned int i = 0; i<p.addLocal.size(); i++)
00113 os << p.addLocal[i];
00114
00115 os << ", del=";
00116 for (unsigned int i = 0; i<p.deleteWorld.size(); i++)
00117 os << p.deleteWorld[i];
00118
00119 os << ")";
00120 return os;
00121 }
00122
00123 }