/** * @brief Resample the particles based on vision observations * NOTE: Assume given a swarm with normalized weights */ void ParticleFilter::resample() { // Map each normalized weight to the corresponding particle. std::map<float, Particle> cdf; float prev = 0.0f; ParticleIt iter; for(iter = particles.begin(); iter != particles.end(); ++iter) { cdf[prev + iter->getWeight()] = (*iter); prev += iter->getWeight(); } boost::mt19937 rng; rng.seed(static_cast<unsigned>(std::time(0))); boost::uniform_01<boost::mt19937> gen(rng); float rand; ParticleSet newParticles; // Sample numParticles particles with replacement according to the // normalized weights, and place them in a new particle set. for(int i = 0; i < parameters.numParticles; ++i) { rand = (float)gen(); newParticles.push_back(cdf.upper_bound(rand)->second); } // FUN IDEA: Create a particle that equals the belief // ***TEMP*** This is used for testing, to only select the BEST particle // newParticles.clear(); // Particle best; // float bestWeight =0.f; // for(iter = particles.begin(); iter != particles.end(); ++iter) // { // Particle particle = (*iter); // if(particle.getWeight() > bestWeight) // { // best = particle; // bestWeight = particle.getWeight(); // } // } // for (int i=0; i<parameters.numParticles; i++) // newParticles.push_back(best); // ***TEMP*** always choose best particle particles = newParticles; }
/** * A resampling algorithm to construct the posterior belief distribution * from the prior belief. The "fittest" particles survive and are * the most prevalent in the resulting set of particles. Replaces the * existing particle set in the filter with a newly generated one. */ void ParticleFilter::resample() { // Normalize the particle weights, and find the average weight. float sum = 0.0f; ParticleIt iter; for(iter = particles.begin(); iter != particles.end(); ++iter) sum += (*iter).getWeight(); if(sum == 0) { std::cout << "\n\n\nZERO SUM!\n\n\n" << std::endl; return; } averageWeight = sum/(((float)parameters.numParticles)*1.0f); for(iter = particles.begin(); iter != particles.end(); ++iter) { float weight = (*iter).getWeight(); (*iter).setWeight(weight/sum); } // // Update exponential filters for long-term and short-term weights. // wSlow = wSlow + parameters.alpha_slow*(averageWeight - wSlow); // wFast = wFast + parameters.alpha_fast*(averageWeight - wFast); // float injectionProb = std::max(0.0f, 1.0f - wFast/wSlow); // float confidence = 1.0f - wFast/wSlow; // // std::cout << "The confidence of the particles swarm is: " // // << confidence << std::endl; //if(injectionProb > 0) // std::cout << injectionProb << std::endl; // Map each normalized weight to the corresponding particle. std::map<float, LocalizationParticle> cdf; float prev = 0.0f; for(iter = particles.begin(); iter != particles.end(); ++iter) { LocalizationParticle particle = (*iter); cdf[prev + particle.getWeight()] = particle; prev += particle.getWeight(); } // std::cout << "sum = " << sum << std::endl; // std::cout << "tot = " << prev << std::endl; boost::mt19937 rng; rng.seed(static_cast<unsigned>(std::time(0))); boost::uniform_01<boost::mt19937> gen(rng); // For random particle injection. // boost::uniform_real<float> xBounds(0.0f, width); // boost::uniform_real<float> yBounds(0.0f, height); // boost::uniform_real<float> angleBounds(0, // 2.0f*boost::math::constants::pi<float>()); // boost::variate_generator<boost::mt19937&, // boost::uniform_real<float> > xGen(rng, xBounds); // boost::variate_generator<boost::mt19937&, // boost::uniform_real<float> > yGen(rng, yBounds); // boost::variate_generator<boost::mt19937&, // boost::uniform_real<float> > angleGen(rng, angleBounds); float rand; ParticleSet newParticles; int numParticlesInjected = 0; // Sample numParticles particles with replacement according to the // normalized weights, and place them in a new particle set. for(int i = 0; i < parameters.numParticles; ++i) { rand = (float)gen(); // if(rand <= injectionProb) // numParticlesInjected++; //{ //LocalizationParticle p(Location(xGen(), yGen(), angleGen()), // 0.0f); //newParticles.push_back(p); //} //else newParticles.push_back(cdf.upper_bound(rand)->second); } particles = newParticles; if(numParticlesInjected > 0) { //std::cout << "Injected " << numParticlesInjected // << " random particles." // << std::endl; } #ifdef DEBUG_LOCALIZATION LocalizationParticle best = getBestParticle(); std::cout << "Best particle: " << best << std::endl; std::cout << "Resampled particles: " << std::endl; for(iter = particles.begin(); iter != particles.end(); ++iter) { std::cout << "Particle (" << (*iter).getLocation().x << ", " << (*iter).getLocation().y << ", " << (*iter).getLocation().heading << ") with weight " << (*iter).getWeight() << std::endl; } #endif }