/**
* @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
}
