C++ ParticleSet::push_back方法代码示例

/**
 * @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
//.........这里部分代码省略.........