Optimizing a particle filter weighted selection step I have a vector with 10000 particles that I use in a Particle Filter. The problem is that it's taking too long to compute the weighted selection of the particles.

I want to use a vector of weights (probabilities) to select elements from a vector (particles) to generate a new vector (new_particles) with the same size as the original, which has the most random items from the original set based on their weights.

Is there any way I can optimize this?

// util::NUMBER_OF_PARTICLES is a const int equal to 10000

// vector that holds particles weights
std::vector&lt; double &gt; probabilities;
probabilities.reserve(util::NUMBER_OF_PARTICLES);

// vector that will hold new particles
std::vector&lt; GameParticle &gt; new_particles;
new_particles.reserve(util::NUMBER_OF_PARTICLES);

// sum of all particle's weights
double sum_prob_all_particles = 0;

// calculate particle weights and save in vector
for(std::vector&lt; GameParticle &gt;::iterator it = game_particles_.begin(); it != game_particles_.end(); ++it)
{
    double probability = util::getProbOfMeasurementGivenParticle(*it, measurement);
    probabilities.push_back(probability);
    sum_prob_all_particles += probability;
}

ROS_ERROR_STREAM_COND(sum_prob_all_particles == 0, "Error, all particles have a zero probability of being correct");

// select new particle group
// counter used to count number of selected particles
int particlesCounter = util::NUMBER_OF_PARTICLES;
// random variable used to select particle value from 0 to sum_prob_all_particles
double random_variable = ( std::rand() / (double) RAND_MAX ) * sum_prob_all_particles;
double sum_probs = 0;

// counter of current particle to select
int counter = util::NUMBER_OF_PARTICLES - 1;

// while there are particles to select
while (particlesCounter &gt; 0)
{
    // loops through all old particles and their weights
    int counter = util::NUMBER_OF_PARTICLES - 1;
    for(std::vector&lt; double &gt;::reverse_iterator it = probabilities.rbegin(); it != probabilities.rend(); ++it)
    {
        sum_probs += (double) *it;
        // if random variable selected this weight, get this particle
        while(sum_probs &gt;= random_variable)
        {
            particlesCounter--;
            new_particles.push_back(game_particles_[counter]);
            random_variable += ( std::rand() / (double) RAND_MAX ) * sum_prob_all_particles;

            // get out of loops if no more particles
            if(particlesCounter == 0)
                goto AFTER_PARTICLES_SELECTED;
        }
        counter--;
    }
}

AFTER_PARTICLES_SELECTED:

ROS_INFO_STREAM("Observation step complete");


The part which is actually taking too long is after the // select new particle group comment. It takes an average of 0.984703 seconds for 5000 particles and 3.89787 seconds for 10000 particles, which is just too much.

The part before that comment only takes an average of 0.00142544 seconds for 10000 particles, so it's good enough.
I don't see any need for that goto, and it should be avoided whenever possible.  It would be better to just perform the action after the condition, then break from the loop.

if (particlesCounter == 0)
{
    ROS_INFO_STREAM("Observation step complete");
    break;
}


Although this will only break from the for loop, the state of particlesCounter at that point will stop the while loop as well.