Optimizing a particle filter weighted selection step

Question

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< double > probabilities;
probabilities.reserve(util::NUMBER_OF_PARTICLES);

// vector that will hold new particles
std::vector< GameParticle > 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< GameParticle >::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 > 0)
{
    // loops through all old particles and their weights
    int counter = util::NUMBER_OF_PARTICLES - 1;
    for(std::vector< double >::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 >= 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.

Answer 1

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.

Answer 2

There are some things that can improve the code by removing the C cast to double (unnecessary) and the goto.

The killer that will improve your algorithm and thus speed is to cache the partial sums of your probabilities.

I notice your loop then continues after you've "hit" the point where you increase your random_variable.

As you know the sum to the current point, you also know all the partial sums from this point onward.

Your vector of partial sums will be sorted and it will be \$O(log N)\$ to find how far you need to iterate through these then just add it to sum_probs in one go. You now add something to random_variable, however, if you've stored the original value that you had at the start of the loop, you know the new total before you have to increase it again.

If the total sum of all your probabilities (sum_prob_all_particles? or just the last element in your sum) doesn't "reach" random_variable at this point, just add that to sum_probs (that you started with) before you loop again.