Homepage

Demos

Overview

Downloads

Tutorials

Reference

Credits

ParticleFilter.cc Go to the documentation of this file. //-*-c++-*- #include <iostream> #include <vector> #include <math.h> #include "BaseData.h" // for BoundingBox #include "Particle.h" #include "ParticleShapes.h" #include "ParticleFilter.h" namespace DualCoding { const float ParticleFilter::INITIAL_XY_NOISE; const float ParticleFilter::INITIAL_THETA_NOISE; const float ParticleFilter::NOISE_REDUCTION_XY; const float ParticleFilter::NOISE_REDUCTION_T; const float ParticleFilter::INFINITE_DISTANCE; const float ParticleFilter::STDEV; const float ParticleFilter::ADDITION_PENALTY; const float ParticleFilter::PERCENT_RANDOM; void ParticleFilter::reinitialize() { delete curParticles; delete newParticles; curParticles=NULL; newParticles=NULL; numParticles = NUM_PARTICLES; numParticles = NUM_PARTICLES; numGenerations = NUM_GENERATIONS; numTries = NUM_TRIES; noiseFactorXY = INITIAL_XY_NOISE; noiseFactorT = INITIAL_THETA_NOISE; worldBounds = Shape<PolygonData>(); bestIndex = -1; localScores.clear(); localMatches.clear(); } void ParticleFilter::makeParticles() { delete curParticles; delete newParticles; curParticles = new vector<Particle>(numParticles); newParticles = new vector<Particle>(numParticles); } void ParticleFilter::resizeParticles() { for ( int i=0; i<numParticles; i++ ) { (*curParticles)[i].addLocal.resize(nlocal); (*newParticles)[i].addLocal.resize(nlocal); } particleViewX.resize(nlocal); particleViewY.resize(nlocal); particleViewX2.resize(nlocal); particleViewY2.resize(nlocal); localScores.resize(nlocal); localMatches.resize(nlocal); } void ParticleFilter::loadLms() { PfRoot::deleteLms(localLms); PfRoot::deleteLms(worldLms); localLms = PfRoot::loadLms(localShS.allShapes(), false); worldLms = PfRoot::loadLms(worldShS.allShapes(), true); nlocal = localLms->size(); nworld = worldLms->size(); if ( nlocal==0 || nworld==0 ) { cout << "ParticleFilter::loadLMs found " << nlocal << " local and " << nworld << " world landmarks: can't localize!" << endl; return; } // Determine x,y bounds for resampling. If there is a world bounding // polygon, use its bounding box. if ( worldBounds.isValid() ) { BoundingBox b(worldBounds->getBoundingBox()); xmin = b.xmin; xmax = b.xmax; ymin = b.ymin; ymax = b.ymax; } else { coordinate_t localXmin, localYmin, localXmax, localYmax; coordinate_t worldXmin, worldYmin, worldXmax, worldYmax; PfRoot::findBounds(*localLms, localXmin, localYmin, localXmax, localYmax); PfRoot::findBounds(*worldLms, worldXmin, worldYmin, worldXmax, worldYmax); const coordinate_t localMax = max(fabs(localXmin), max(fabs(localYmin), max(fabs(localXmax), fabs(localYmax)))); xmin = worldXmin - localMax; xmax = worldXmax + localMax; ymin = worldYmin - localMax; ymax = worldYmax + localMax; } xrange = xmax - xmin; yrange = ymax - ymin; // create or resize particles if necessary if ( curParticles == NULL ) { makeParticles(); uniformlyDistribute(); } if ( (int)((*curParticles)[0].addLocal.size()) != nlocal ) resizeParticles(); } void ParticleFilter::uniformlyDistribute() { // Determine space to generate random samples loadLms(); for (int i = 0; i < numParticles; i++) randomizeNewParticle(i); vector<Particle> * const oldParticles = curParticles; curParticles = newParticles; newParticles = oldParticles; } void ParticleFilter::randomizeNewParticle(int const i) { Particle &part = (*newParticles)[i]; while (1) { // loop until particle is acceptable part.dx = float(rand())/RAND_MAX * xrange + xmin; part.dy = float(rand())/RAND_MAX * yrange + ymin; if ( !worldBounds.isValid() || worldBounds->isInside(Point(part.dx,part.dy)) ) break; } part.theta = float(rand())/RAND_MAX * 2 * M_PI; } int ParticleFilter::localize() { loadLms(); if ( nlocal == 0 || nworld == 0 ) return -1; getInitialGuess(); noiseFactorXY = INITIAL_XY_NOISE; noiseFactorT = INITIAL_THETA_NOISE; for ( int gen = 0; gen < numGenerations; gen++ ) { resample(); noiseFactorXY = noiseFactorXY * NOISE_REDUCTION_XY; noiseFactorT = noiseFactorT * NOISE_REDUCTION_T; /* cout << "Generation " << gen << ":" << endl; for ( int i=0; i<40; i++ ) { Particle &part = (*curParticles)[i]; printf(" %3d: %7.2f , %7.2f @ %6.4f = %6.4f\n", i, part.dx, part.dy, (float)part.theta, part.prob); } */ } return bestIndex; } void ParticleFilter::moveBy(float const xdelta, float const ydelta, AngPi const tdelta) { for ( int i = 0; i<numParticles; i++ ) { Particle &part = (*curParticles)[i]; part.dx += xdelta; part.dy += ydelta; part.theta = part.theta + tdelta; } computeParticleScores(); } void ParticleFilter::getInitialGuess() { float const minAcceptableProb = pow(((double)0.3),((double)nlocal)); for ( int tryCounter = 0; tryCounter < numTries; tryCounter++ ) { computeParticleScores(); if ( (*curParticles)[bestIndex].prob >= minAcceptableProb ) break; else uniformlyDistribute(); // no decent match, so randomize all particles and try another pass } } void ParticleFilter::computeParticleScores() { float bestProb = -1; bestIndex = -1; for ( int i = 0; i<numParticles; i++ ) { setParticleView(i); (*curParticles)[i].addLocal.assign(nlocal,false); for ( int j = 0; j<nlocal; j++ ) computeLocalMatch(i,j); determineAdditions(i); // determineDeletions(i); float s = localScores[0]; for ( int j=1; j<nlocal; j++ ) s *= localScores[j]; (*curParticles)[i].prob = s; if ( s > bestProb ) { bestProb = s; bestIndex = i; } } // cout << "computeParticleScores(): best particle = " << bestIndex // << ": " << bestProb << endl; } void ParticleFilter::setParticleView(int const i) { Particle const &part = (*curParticles)[i]; float const partDx = part.dx; float const partDy = part.dy; float const cosT = cos(-part.theta); float const sinT = sin(-part.theta); float const negSinT = -sinT; for ( int j=0; j < nlocal; j++ ) { PfRoot &landmark = *((*localLms)[j]); particleViewX[j] = landmark.x * cosT + landmark.y * sinT + partDx; particleViewY[j] = landmark.x * negSinT + landmark.y * cosT + partDy; if ( landmark.type == lineDataType ) { const PfLine &line = dynamic_cast<PfLine&>(landmark); particleViewX2[j] = line.x2 * cosT + line.y2 * sinT + partDx; particleViewY2[j] = line.x2 * negSinT + line.y2 * cosT + partDy; } } } void ParticleFilter::computeLocalMatch (int const i, int const j) { float distsq = INFINITE_DISTANCE; int worldMatch = -1; for ( int k=0; k<nworld; k++ ) { if ( (*localLms)[j]->type == (*worldLms)[k]->type && (*localLms)[j]->color == (*worldLms)[k]->color ) { float const lx = particleViewX[j]; float const ly = particleViewY[j]; float const wx = (*worldLms)[k]->x; float const wy = (*worldLms)[k]->y; float tempDistsq; switch ( (*localLms)[j]->type ) { case lineDataType: { PfLine &localLine = *dynamic_cast<PfLine*>((*localLms)[j]); PfLine &worldLine = *dynamic_cast<PfLine*>((*worldLms)[k]); float tempDistsq1, tempDistsq2; // If endpoints are valid, compare distance between endpoints. // If not valid, measure perpendicular distance from the local endpoint // to the world line segment, if the projection of the endpoint onto the // segment occurs within the segment, not beyond it. Instead of calculating // the projection we use a heuristic test: either the x or y endpoint value must // lie within the range of the line segment. if ( (localLine.valid1 && worldLine.valid1) || !( lx >= min(worldLine.x,worldLine.x2) && lx <= max(worldLine.x,worldLine.x2) || ly >= min(worldLine.y,worldLine.y2) && ly <= max(worldLine.y,worldLine.y2) ) ) tempDistsq1 = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy); else { float const tempDist1 = distanceFromLine(lx,ly,worldLine); tempDistsq1 = tempDist1 * tempDist1; } float const lx2 = particleViewX2[j]; float const ly2 = particleViewY2[j]; float const wx2 = worldLine.x2; float const wy2 = worldLine.y2; if ( (localLine.valid2 && worldLine.valid2) || !( lx2 >= min(worldLine.x,worldLine.x2) && lx2 <= max(worldLine.x,worldLine.x2) || ly2 >= min(worldLine.y,worldLine.y2) && ly2 <= max(worldLine.y,worldLine.y2) ) ) tempDistsq2 = (lx2-wx2)*(lx2-wx2) + (ly2-wy2)*(ly2-wy2); else { float const tempDist2 = distanceFromLine(lx2,ly2,worldLine); tempDistsq2 = tempDist2 * tempDist2; } AngPi const localOrient = localLine.orientation + (*curParticles)[i].theta; AngPi const odiff = worldLine.orientation - localOrient; float const odist = 500 * sin(odiff); float const odistsq = odist * odist; tempDistsq = tempDistsq1 + tempDistsq2 + odistsq; // plus orientation match term? } break; case ellipseDataType: case pointDataType: case blobDataType: { tempDistsq = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy); break; } default: std::cout << "ParticleFilter::computeMatchScore() can't match landmark type " << (*localLms)[j]->type << std::endl; return; } if ( tempDistsq < distsq ) { distsq = tempDistsq; worldMatch = k; } } } localMatches[j] = worldMatch; if ( worldMatch == -1 ) { (*curParticles)[i].addLocal[j] = true; localScores[j] = 1.0; } else localScores[j] = normpdf(distsq); } float ParticleFilter::distanceFromLine(coordinate_t x0, coordinate_t y0, PfLine &wline) { float const &x1 = wline.x; float const &y1 = wline.y; float const &x2 = wline.x2; float const &y2 = wline.y2; float const &length = wline.length; float const result = fabs((x2-x1)*(y1-y0) - (x1-x0)*(y2-y1)) / length; return result; } void ParticleFilter::resample() { float const normFactor = (*curParticles)[bestIndex].prob; // cout << "resample(): normFactor = " << normFactor << endl; int newParticleCount = 0; (*newParticles)[newParticleCount++] = (*curParticles)[bestIndex]; while (newParticleCount < numParticles) { for ( int i=0; i<numParticles; i++ ) { float spawnChance = float(rand())/RAND_MAX; if ( spawnChance <= 0.8 * (*curParticles)[i].prob / normFactor ) { spawnInto(i, newParticleCount++); if ( newParticleCount == numParticles ) break; } if ( float(rand())/RAND_MAX < PERCENT_RANDOM ) { randomizeNewParticle(newParticleCount++); if ( newParticleCount == numParticles ) break; } } } vector<Particle> * const oldParticles = curParticles; curParticles = newParticles; newParticles = oldParticles; computeParticleScores(); } void ParticleFilter::spawnInto(int const i, int const j) { Particle &parent = (*curParticles)[i]; Particle &spawned = (*newParticles)[j]; while (1) { // loop until we generate an acceptable particle float const xRand = 2*float(rand())/RAND_MAX - 1; float const yRand = 2*float(rand())/RAND_MAX - 1; spawned.dx = parent.dx + xRand * noiseFactorXY; spawned.dy = parent.dy + yRand * noiseFactorXY; if ( !worldBounds.isValid() || worldBounds->isInside(Point(spawned.dx, spawned.dy)) ) { float const tRand = 2*float(rand())/RAND_MAX - 1; spawned.theta = parent.theta + AngTwoPi(tRand * noiseFactorT); break; } } } void ParticleFilter::determineAdditions(int const i) { Particle &part = (*curParticles)[i]; for (int j = 0; j<nlocal; j++) { float const randval = float(rand()) / (RAND_MAX*6); if (randval >= localScores[j]) { part.addLocal[j] = true; localScores[j] = ADDITION_PENALTY; localMatches[j] = -1; } else for (int j2 = (j+1); j2<nlocal; j2++) if (localMatches[j2] == localMatches[j] && localMatches[j2] != -1) if (localScores[j2] > localScores[j]) { part.addLocal[j] = true; localScores[j] = ADDITION_PENALTY; localMatches[j] = -1; } else { part.addLocal[j2] = true; localScores[j2] = ADDITION_PENALTY; localMatches[j2] = -1; } } } /* void ParticleFilter::determineDeletions (int const i) { Particle &part = (*curParticles)[i]; part.deleteWorld.assign(nworld,true); float minXLoc = INFINITEDISTANCE; float minYLoc = INFINITEDISTANCE; float maxXLoc = 0; float maxYLoc = 0; for (int j = 0; j<nlocal; j++) { if ( localMatches[j] != -1 ) part.deleteWorld[localMatches[j]] = false; // don't delete world LM if it matches if ( particleViewX[j] < minXLoc ) minXLoc = particleViewX[j]; else if (particleViewX[j] > maxXLoc) maxXLoc = particleViewX[j]; if (particleViewY[j] < minYLoc) minYLoc = particleViewY[j]; else if (particleViewY[j] > maxYLoc) maxYLoc = particleViewY[j]; } for (int k = 0; k<nworld; k++) if ( !( worldLms[k]->x >= minXLoc && worldLms[k]->x <= maxXLoc && worldLms[k]->y >= minYLoc && worldLms[k]->y <= maxYLoc ) ) part.deleteWorld[k] == false; // don't delete world LM if it was outside local view } */ } // namespace

DualCoding 3.0beta

Generated Wed Oct 4 00:01:54 2006 by Doxygen 1.4.7