particlefilter.h

Go to the documentation of this file.

#ifndef PARTICLEFILTER_H
#define PARTICLEFILTER_H
#include <stdlib.h>
#include <sys/types.h>
#include <vector>
#include <utility>
#include <cmath>
#include <limits>
#include <gmapping/utils/gvalues.h>


typedef std::pair<uint,uint> UIntPair;

template <class OutputIterator, class Iterator>
double toNormalForm(OutputIterator& out, const Iterator & begin, const Iterator & end){
        //determine the maximum
        double lmax = -std::numeric_limits<double>::max();
        for (Iterator it=begin; it!=end; it++){
                lmax=lmax>((double)(*it))? lmax: (double)(*it);
        }
        //convert to raw form
        for (Iterator it=begin; it!=end; it++){
                *out=exp((double)(*it)-lmax);
                out++;
        }
        return lmax;
}

template <class OutputIterator, class Iterator, class Numeric>
void toLogForm(OutputIterator& out, const Iterator & begin, const Iterator & end, Numeric lmax){
        //determine the maximum
        for (Iterator it=begin; it!=end; it++){
                *out=log((Numeric)(*it))-lmax;
                out++;
        }
        return lmax;
}

template <class WeightVector>
void resample(std::vector<int>& indexes, const WeightVector& weights, unsigned int nparticles=0){
        double cweight=0;
        
        //compute the cumulative weights
        unsigned int n=0;
        for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it){
                cweight+=(double)*it;
                n++;
        }

        if (nparticles>0)
                n=nparticles;
        
        //compute the interval
        double interval=cweight/n;

        //compute the initial target weight
        double target=interval*::drand48();
        //compute the resampled indexes

        cweight=0;
        indexes.resize(n);
        
        n=0;
        unsigned int i=0;
        for (typename WeightVector::const_iterator it=weights.begin(); it!=weights.end(); ++it, ++i){
                cweight+=(double)* it;
                while(cweight>target){
                        indexes[n++]=i;
                        target+=interval;
                }
        }
}

template <typename Vector>
void repeatIndexes(Vector& dest, const std::vector<int>& indexes, const Vector& particles){
        assert(indexes.size()==particles.size());
        dest.resize(particles.size());
        unsigned int i=0;
        for (std::vector<int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
                dest[i]=particles[*it];
                i++;
        }
}


template <class Iterator>
double neff(const Iterator& begin, const Iterator& end){
        double sum=0;
        for (Iterator it=begin; it!=end; ++it){
                sum+=*it;
        }
        double cum=0;
        for (Iterator it=begin; it!=end; ++it){
                double w=*it/sum;
                cum+=w*w;
        }
        return 1./cum;
}

template <class Iterator>
void normalize(const Iterator& begin, const Iterator& end){
        double sum=0;
        for (Iterator it=begin; it!=end; ++it){
                sum+=*it;
        }
        for (Iterator it=begin; it!=end; ++it){
                *it=*it/sum;
        }
}

template <class OutputIterator, class Iterator>
void rle(OutputIterator& out, const Iterator & begin, const Iterator & end){
        unsigned int current=0;
        unsigned int count=0;
        for (Iterator it=begin; it!=end; it++){
                if (it==begin){
                        current=*it;
                        count=1;
                        continue;
                }
                if (((uint)*it) ==current)
                        count++;
                if (((uint)*it)!=current){
                        *out=std::make_pair(current,count);
                        out++;
                        current=*it;
                        count=1;
                }
        }
        if (count>0)
                *out=std::make_pair(current,count);
                out++;
}

//BEGIN legacy
template <class Particle, class Numeric>
struct uniform_resampler{
        std::vector<unsigned int> resampleIndexes(const std::vector<Particle> & particles, int nparticles=0) const;
        std::vector<Particle> resample(const std::vector<Particle> & particles, int nparticles=0) const;
        Numeric neff(const std::vector<Particle> & particles) const;
};

/*Implementation of the above stuff*/
template <class Particle, class Numeric>
std::vector<unsigned int> uniform_resampler<Particle, Numeric>:: resampleIndexes(const std::vector<Particle>& particles, int nparticles) const{
        Numeric cweight=0;

        //compute the cumulative weights
        unsigned int n=0;
        for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
                cweight+=(Numeric)*it;
                n++;
        }

        if (nparticles>0)
                n=nparticles;
        
        //compute the interval
        Numeric interval=cweight/n;

        //compute the initial target weight
        Numeric target=interval*::drand48();
        //compute the resampled indexes

        cweight=0;
        std::vector<unsigned int> indexes(n);
        n=0;
        unsigned int i=0;
        for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
                cweight+=(Numeric)* it;
                while(cweight>target){
                        indexes[n++]=i;
                        target+=interval;
                }
        }
        return indexes;
}

template <class Particle, class Numeric>
std::vector<Particle> uniform_resampler<Particle,Numeric>::resample
  (const typename std::vector<Particle>& particles, int nparticles) const{
        Numeric cweight=0;

        //compute the cumulative weights
        unsigned int n=0;
        for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
                cweight+=(Numeric)*it;
                n++;
        }

        if (nparticles>0)
                n=nparticles;
        
        //weight of the particles after resampling
        double uw=1./n;

        //compute the interval
        Numeric interval=cweight/n;

        //compute the initial target weight
        Numeric target=interval*::drand48();
        //compute the resampled indexes

        cweight=0;
        std::vector<Particle> resampled;
        n=0;
        unsigned int i=0;
        for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, ++i){
                cweight+=(Numeric)*it;
                while(cweight>target){
                        resampled.push_back(*it);
                        resampled.back().setWeight(uw);
                        target+=interval;
                }
        }
        return resampled;
}

template <class Particle, class Numeric>
Numeric uniform_resampler<Particle,Numeric>::neff(const std::vector<Particle> & particles) const{
        double cum=0;
        double sum=0;
        for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it){
                Numeric w=(Numeric)*it;
                cum+=w*w;
                sum+=w;
        }
        return sum*sum/cum;
}


/*

The following are patterns for the evolution and the observation classes
The user should implement classes having the specified meaning

template <class State, class Numeric, class Observation>
struct observer{
        Observation& observation
        Numeric observe(const class State&) const;
};

template <class State, class Numeric, class Input>
struct evolver{
        Input& input;
        State& evolve(const State& s);
};
*/


template <class Particle, class EvolutionModel>
struct evolver{
        EvolutionModel evolutionModel;
        void evolve(std::vector<Particle>& particles);
        void evolve(std::vector<Particle>& dest, const std::vector<Particle>& src);
};

template <class Particle, class EvolutionModel>
void evolver<Particle, EvolutionModel>::evolve(std::vector<Particle>& particles){
        for (typename std::vector<Particle>::iterator it=particles.begin(); it!=particles.end(); ++it){
                *it=evolutionModel.evolve(*it);
        }
}

template <class Particle, class EvolutionModel>
void evolver<Particle, EvolutionModel>::evolve(std::vector<Particle>& dest, const std::vector<Particle>& src){
        dest.clear();
        for (typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it)
                dest.push_back(evolutionModel.evolve(*it));
}


template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
struct auxiliary_evolver{
        EvolutionModel evolutionModel;
        QualificationModel qualificationModel;
        LikelyhoodModel likelyhoodModel;
        void evolve(std::vector<Particle>& particles);
        void evolve(std::vector<Particle>& dest, const std::vector<Particle>& src);
};

template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
void auxiliary_evolver<Particle, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
  (std::vector<Particle>&particles){
        std::vector<Numeric> observationWeights(particles.size());
        unsigned int i=0;
        for (typename std::vector<Particle>::const_iterator it=particles.begin(); it!=particles.end(); ++it, i++){
                observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
        }
        uniform_resampler<Numeric, Numeric> resampler;
        std::vector<unsigned int> indexes(resampler.resampleIndexes(observationWeights));
        for (typename std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
                Particle & particle=particles[*it];
                particle=evolutionModel.evolve(particle);
                particle.setWeight(likelyhoodModel.likelyhood(particle)/observationWeights[*it]);
        }
}

template <class Particle, class Numeric, class QualificationModel, class EvolutionModel, class LikelyhoodModel>
void auxiliary_evolver<Particle, Numeric, QualificationModel, EvolutionModel, LikelyhoodModel>::evolve
  (std::vector<Particle>& dest, const std::vector<Particle>& src){
        dest.clear();
        std::vector<Numeric> observationWeights(src.size());
        unsigned int i=0;
        for (typename std::vector<Particle>::const_iterator it=src.begin(); it!=src.end(); ++it, i++){
                observationWeights[i]=likelyhoodModel.likelyhood(qualificationModel.evolve(*it));
        }
        uniform_resampler<Numeric, Numeric> resampler;
        std::vector<unsigned int> indexes(resampler.resampleIndexes(observationWeights));
        for (typename std::vector<unsigned int>::const_iterator it=indexes.begin(); it!=indexes.end(); ++it){
                Particle & particle=src[*it];
                dest.push_back(evolutionModel.evolve(particle));
                dest.back().weight*=likelyhoodModel.likelyhood(particle)/observationWeights[*it];
        }
}
//END legacy

#endif