Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PFShapeSLAM.cc

Go to the documentation of this file.
00001 //-*-c++-*-
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"  // for mapBuilder
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     // cout << "computeParticleScores(): best particle = " << bestIndex
00025     // << ": " << bestProb << endl;
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;  // don't delete world LM if it matches
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; // don't delete world LM if not mobile, or it was outside local view
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 } // namespace

DualCoding 4.0
Generated Thu Nov 22 00:52:36 2007 by Doxygen 1.5.4