Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

PFShapeLocalization.cc

Go to the documentation of this file.
00001 //-*-c++-*-
00002 
00003 #include <cmath>
00004 #include <iostream>
00005 
00006 #include "MapBuilder.h"
00007 #include "PFShapeLocalization.h"
00008 #include "ShapeLocalizationParticle.h"
00009 #include "ShapeSpace.h"
00010 #include "ShapeFuns.h"
00011 #include "VRmixin.h"  // for mapBuilder
00012 
00013 using namespace std;
00014 
00015 namespace DualCoding {
00016 
00017   ParticleShapeEvaluator::ParticleShapeEvaluator(ShapeSpace &localShS, ShapeSpace &worldShS) : 
00018     localLms(), worldLms(),
00019     maxDist(0),
00020     localMatches(), numMatches(0), localScores(),
00021     particleViewX(), particleViewY(), particleViewX2(), particleViewY2()
00022   {
00023     PfRoot::loadLms(localShS.allShapes(), false, localLms);
00024     PfRoot::loadLms(worldShS.allShapes(), true, worldLms);
00025     if ( localLms.size()==0 || worldLms.size()==0 ) {
00026       cout << "ParticleFilter::loadLms found " << localLms.size() << " local and "
00027      << worldLms.size() << " world landmarks: can't localize!" << endl;
00028       return;
00029     }
00030     unsigned int nlocal = localLms.size();
00031     localScores.resize(nlocal);
00032     localMatches.resize(nlocal);
00033     particleViewX.resize(nlocal);
00034     particleViewY.resize(nlocal);
00035     particleViewX2.resize(nlocal);
00036     particleViewY2.resize(nlocal);
00037   }
00038 
00039   void ParticleShapeEvaluator::evaluate(LocalizationParticle& part) {
00040     // determine position of local space landmark in world given the current particle
00041     float const cosT = cos(-part.theta);
00042     float const sinT = sin(-part.theta);
00043     float const negSinT = -sinT;
00044     // constructor has already verified that localLms > 0 and worldLms > 0
00045     for ( unsigned int j=0; j < localLms.size(); j++ ) {
00046       PfRoot &landmark = *(localLms[j]);
00047       particleViewX[j] = landmark.x * cosT + landmark.y * sinT + part.x; 
00048       particleViewY[j] = landmark.x * negSinT + landmark.y * cosT + part.y;
00049       if ( landmark.type == lineDataType ) {
00050   const PfLine &line = static_cast<PfLine&>(landmark);
00051   particleViewX2[j] = line.x2 * cosT + line.y2 * sinT + part.x;
00052   particleViewY2[j] = line.x2 * negSinT + line.y2 * cosT + part.y;
00053       }
00054     }
00055   
00056     // now compute score for the particle by finding matches between local landmarks and world landmarks
00057     numMatches=0;
00058     for ( unsigned int j = 0; j<localLms.size(); j++ ) {
00059       float distsq = HUGE_VAL; // HUGE_VAL is defined in math.h
00060       localMatches[j] = -1;
00061       for ( unsigned int k=0; k<worldLms.size(); k++ ) {
00062   if ( localLms[j]->type == worldLms[k]->type &&
00063        localLms[j]->color == worldLms[k]->color ) {
00064     float const lx = particleViewX[j];
00065     float const ly = particleViewY[j];
00066     float const wx = worldLms[k]->x;
00067     float const wy = worldLms[k]->y;
00068     float tempDistsq;
00069     switch ( localLms[j]->type ) {
00070     case lineDataType: {
00071       PfLine &localLine = *static_cast<PfLine*>(localLms[j]);
00072       PfLine &worldLine = *static_cast<PfLine*>(worldLms[k]);
00073       float tempDistsq1, tempDistsq2;
00074       // If endpoints are valid, compare distance between endpoints.
00075       // If not valid, measure perpendicular distance from the local endpoint
00076       // to the world line segment, if the projection of the endpoint onto the
00077       // segment occurs within the segment, not beyond it.  Instead of calculating
00078       // the projection we use a heuristic test: either the x or y endpoint value must
00079       // lie within the range of the line segment.
00080       if ( (localLine.valid1 && worldLine.valid1) ||
00081      !( lx >= min(worldLine.x,worldLine.x2) &&
00082         lx <= max(worldLine.x,worldLine.x2) ||
00083         ly >= min(worldLine.y,worldLine.y2) &&
00084         ly <= max(worldLine.y,worldLine.y2) ) )
00085         tempDistsq1 = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy);
00086       else {
00087         float const tempDist1 = distanceFromLine(lx,ly,worldLine);
00088         tempDistsq1 = tempDist1 * tempDist1;
00089       }
00090       float const lx2 = particleViewX2[j];
00091       float const ly2 = particleViewY2[j];
00092       float const wx2 = worldLine.x2;
00093       float const wy2 = worldLine.y2;
00094       if ( (localLine.valid2 && worldLine.valid2) ||
00095      !( lx2 >= min(worldLine.x,worldLine.x2) &&
00096         lx2 <= max(worldLine.x,worldLine.x2) ||
00097         ly2 >= min(worldLine.y,worldLine.y2) &&
00098         ly2 <= max(worldLine.y,worldLine.y2) ) )
00099         tempDistsq2 = (lx2-wx2)*(lx2-wx2) + (ly2-wy2)*(ly2-wy2);
00100       else {
00101         float const tempDist2 = distanceFromLine(lx2,ly2,worldLine);
00102         tempDistsq2 = tempDist2 * tempDist2;
00103       }
00104       AngPi const localOrient = localLine.orientation + part.theta;
00105       AngPi const odiff = worldLine.orientation - localOrient;
00106       float const odist = 500 * sin(odiff);
00107       float const odistsq = odist * odist;
00108       tempDistsq = tempDistsq1 + tempDistsq2 + odistsq; // plus orientation match term?
00109     }
00110       break;
00111     case ellipseDataType:
00112     case pointDataType:
00113     case blobDataType: {
00114       tempDistsq = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy);
00115       break;
00116     }
00117     default:
00118       std::cout << "ParticleFilter::computeMatchScore() can't match landmark type "
00119           << localLms[j]->type << std::endl;
00120       return;
00121     }
00122     if ( tempDistsq < distsq ) {
00123       distsq = tempDistsq;
00124       localMatches[j] = k;
00125     }
00126   }
00127       }
00128     
00129       if ( localMatches[j] != -1 || !localLms[j]->mobile )
00130   localScores[numMatches++]=distsq;
00131     }
00132   }
00133 
00134   float ParticleShapeEvaluator::distanceFromLine(coordinate_t x0, coordinate_t y0, PfLine &wline) {
00135     float const &x1 = wline.x;
00136     float const &y1 = wline.y;
00137     float const &x2 = wline.x2;
00138     float const &y2 = wline.y2;
00139     float const &length = wline.length;
00140     float const result = fabs((x2-x1)*(y1-y0) - (x1-x0)*(y2-y1)) / length;
00141     return result;
00142   }
00143 
00144   void PFShapeLocalization::setAgent() const {
00145     const PFShapeLocalization::particle_type& best = particles[bestIndex];
00146     VRmixin::mapBuilder.setAgent(Point(best.x,best.y,0,allocentric), best.theta);
00147   }
00148 
00149   void PFShapeLocalization::setPosition(float const x, float const y, AngTwoPi const orientation, float variance) {
00150     LocalizationParticle part(x, y, orientation);
00151     setPosition(part, variance);
00152   }
00153 
00154   void PFShapeLocalization::setWorldBounds(const Shape<PolygonData> &bounds) {
00155     PFShapeDistributionPolicy<LocalizationParticle> *dist =
00156       dynamic_cast<PFShapeDistributionPolicy<LocalizationParticle> *>(&(getResamplingPolicy()->getDistributionPolicy()));
00157     if ( dist != NULL )
00158       dist->setWorldBounds(bounds);
00159     else
00160       cout << "Error: setWorldBounds found wrong type of DistributionPolicy" << endl;
00161   }
00162 
00163   void PFShapeLocalization::displayParticles(float const howmany) const {
00164     ShapeSpace &wShS = sensorModel->getWorldShS();
00165     wShS.deleteShapes<LocalizationParticleData>();
00166     NEW_SHAPE(best, LocalizationParticleData, new LocalizationParticleData(wShS,particles[bestIndex]));
00167     best->setColor(VRmixin::mapBuilder.getAgent()->getColor());
00168     if ( howmany <= 0 ) return;
00169     unsigned int increment;
00170     if ( howmany <= 1.0 )
00171       increment = (unsigned int)ceil(1.0/howmany);
00172     else
00173       increment = (unsigned int)ceil(particles.size()/howmany);
00174     for (unsigned int i=0; i<particles.size(); i+=increment)
00175       if ( i != bestIndex ) {
00176   NEW_SHAPE(pt, LocalizationParticleData, new LocalizationParticleData(wShS,particles[i]));
00177       }
00178   }
00179 
00180 } // namespace

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