CParticleFilter.h

Go to the documentation of this file.

#ifndef C_PARTICLE_FILTER_H_
#define C_PARTICLE_FILTER_H_

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <string.h>
#include <vector>
#include <Eigen/Core>

class ownRandom{
    public:
        float *normalRandomCache;
        bool isOk;
        int size;

        ownRandom(){
            isOk=false;
            size = 0;
            allocate(10000);
            srand( (unsigned)time( NULL ) ); 
        }
        ~ownRandom(){

        }
        void allocate(int m_size){
            if(size != 0){ 
                if(normalRandomCache)free(normalRandomCache);
            }

            normalRandomCache = (float *) malloc(sizeof(float)*m_size);
            isOk=true;
            size = m_size;
        }

        void fillCache(){
            if(size > 0 && normalRandomCache != NULL){
                for(int i=0;i<size;i++){
                    normalRandomCache[i] = normalRandom(); 
                    isOk = true;     
                }
            }else{
                allocate(10000);
                fillCache();
            }
        }
        float getCachedUniformRandom(){
            int rv;
            float scale;

            if(!isOk) fillCache();

            scale = (float) size / (float)RAND_MAX;
            rv = rand(); 
            rv = (int) ((float) rv * scale);
            if(rv >=size){
                fprintf(stderr,"ERROR in getCachedUniformRandom(). rv=%d\n",rv);
                rv = size-1; 
            }
            return normalRandomCache[rv];
        }

        float uniformRandom(void){
            return ( ((float) rand() / (float) RAND_MAX));
        }

        float normalRandom()
        {
            static float gset;
            static int randomStored = 0;
            float fac,rsq,v1,v2;

            if(randomStored){
                return gset;
            }

            do {
                v1=2.0*uniformRandom()-1.0; //pick two uniform numbers in the square extending
                //from -1 to +1 in each direction, 
                v2=2.0*uniformRandom()-1.0;
                rsq=v1*v1+v2*v2;                //see if they are in the unit circle,
            } while (rsq >= 1.0 || rsq == 0.0); //if not try again

            fac=sqrt(-2.0*log(rsq)/rsq);
            gset=v1*fac; // can be used also as normal distributed random number!!
            return v2*fac;
        }

        void covRandom(float& xRand, float& yRand, float cov[3]){
            float a,b,c;
            float eig1,eig2;
            float vx1,vy1,vx2,vy2;
            float d;
            float x,y;

            xRand = normalRandom();
            yRand = normalRandom();
            // calculate eigen values
            a = 1.0;
            b = -(cov[0]+cov[2]);
            c = cov[0]*cov[2] - cov[1]*cov[1];

            eig1 = (-b + sqrt(b*b-4*a*c))/(2*a);
            eig2 = (-b - sqrt(b*b-4*a*c))/(2*a);

            // calculate eigen vectors
            vx1 = cov[1];
            vy1 = eig1 - cov[0];
            d = sqrt(vx1*vx1+vy1+vy1);
            vx1 /= d;
            vy1 /= d;

            vx2 = cov[1];
            vy2 = eig2 - cov[0];
            d = sqrt(vx2*vx2+vy2+vy2);
            vx2 /= d;
            vy2 /= d;

            // Transform the random variable according to the covariance
            x = xRand * sqrt(eig1);
            y = yRand * sqrt(eig2);

            xRand = x * vx1 + y * vx2;
            yRand = x * vy1 + y * vy2;
        }
};



namespace mcl{
    struct scan{
        public:
            float *r;  
            float *a;  
            int N;     
            scan(){
                N=0;
                r=NULL;
                a=NULL;
            }
            scan(const scan &s) {
                *this = s;
            }
            scan &operator=(const scan &s) {
                s.copy(*this);
                return *this;
            }/*
                ~scan(){
                if(r)free(r);
                if(a)free(a);
                r=NULL;a=NULL;
                }*/
            void allocate(int n){
                if(r) free(r);
                if(a) free(a);
                r = (float *) malloc(n*sizeof(float));
                a = (float *) malloc(n*sizeof(float));
                N=n;
            }
            void copy(struct scan &new_scan) const {
                new_scan.r = (float *) malloc(N*sizeof(float));
                new_scan.a = (float *) malloc(N*sizeof(float));
                new_scan.N = N;
                memcpy(new_scan.r,r,N*sizeof(float));
                memcpy(new_scan.a,a,N*sizeof(float));
            }
            void set(float *rr,float *aa){
                if(N<=0) return;
                for(int i=0;i<N;i++){
                    r[i]=rr[i];
                    a[i]=aa[i];
                }
            }
    };
    struct pose{
        public:
            float x; 
            float y; 
            float a; 
            pose(){
                x=0;
                y=0;
                a=0;
            }
            pose(float _x,float _y,float _a){
                x=_x;y=_y;a=_a;
            }
            void set(pose &p){
                x=p.x;
                y=p.y;
                a=p.a;
            }
            void set(float xx,float yy, float aa){
                x=xx;
                y=yy;
                a=aa;
            }
            void setToDifferentialPose(pose odo_cur,pose odo_ref){                                      
                float ddx,ddy,dist,alpha;
                pose tmp;
                ddx = odo_cur.x - odo_ref.x;
                ddy = odo_cur.y - odo_ref.y;
                alpha = atan2(ddy, ddx);
                dist = sqrt(ddx*ddx+ddy*ddy);

                tmp.x = dist * cos( alpha - odo_ref.a );
                tmp.y = dist * sin( alpha - odo_ref.a );
                tmp.a = (float)fmod((float)(odo_cur.a - odo_ref.a), (float)(2.0*M_PI)); 
                if(tmp.a < 0) tmp.a += 2*(float)M_PI;
                x = tmp.x;
                y= tmp.y;
                a = tmp.a;              

            }
            const pose integrateDifferential(const pose diff){
                pose result;
                float l,phii;

                l = sqrt(diff.x*diff.x + diff.y*diff.y);
                phii = atan2(diff.y,diff.x);

                result.x = x + (float)cos(a+phii)*l;
                result.y = y + (float)sin(a+phii)*l;

                // Update angle
                result.a = (float)fmod((float)(diff.a + a),(float)( 2*M_PI));
                // fmod accepts also negative values
                if(result.a < 0) result.a += 2*(float)M_PI;
                return result; 
            }
            void to2PI(){
                a = (float)fmod((float)(a),(float)( 2*M_PI));
                if(a < 0) a += 2*(float)M_PI;
            }
            void toPI(){
                int cnt=0;
                if(a>M_PI) while(a>M_PI){ 
                    a-=2.0*M_PI;
                    cnt++;
                    //if(cnt%10000==0) fprintf(stderr,"Loopping alot a=%.2f cnt=%d\n",a,cnt);
                }
                else if(a<-M_PI) while(a<-M_PI){ 
                    a+=2.0*M_PI;
                    cnt++;
                    //if(cnt%10000==0) fprintf(stderr,"Loopping alot a=%.2f cnt=%d\n",a,cnt);
                }
            }

            ~pose(){}

    };


    struct TPoseParticle{
        public:
            float x;    // X - coordinate of the particle
            float y;    // Y - coordinate of the particle
            float a;    // Heading
            float p;    // Probability of the particle 
            float lik;  // Likelihood of the particle
            TPoseParticle(){
                x=0;
                y=0;
                a=0;
                p=0;
                lik=0;
            }
            void to2PI(){
                a = (float)fmod((float)(a),(float)( 2*M_PI));
                if(a < 0) a += 2*(float)M_PI;
            }
            void toPI(){
                if(a>M_PI) while(a>M_PI) a-=2.0*M_PI;
                else if(a<-M_PI) while(a<-M_PI) a+=2.0*M_PI;
            }
    };




    class CParticleFilter{
        public:
            TPoseParticle *Particles;          
            float Lik;                                               
            float outLiers;                                  
            int NumOfParticles;                          
            int size;                                                  
            mcl::pose average;                
            mcl::pose variance;              
            bool isAvgSet;                   
            ownRandom myrand;                 

            CParticleFilter();               
            ~CParticleFilter();              

            void allocate(int num_particles);
            void myfree();

            mcl::pose getDistributionMean(bool doWeighting=false);
            mcl::pose averageOverNBestAndRandomize(int N, int M=0,float vx=0,float vy=0,float va=0);
            Eigen::Matrix3d getDistributionVariances();

            void initializeNormalRandom(mcl::pose p0, mcl::pose variance,int size);

            void initializeUniform(mcl::pose Pmin, mcl::pose Pmax, mcl::pose dP);

            void SIRUpdate();

            void normalize();

            void predict(mcl::pose dP, mcl::pose std);

            void predict(mcl::pose dP, float Q[4]);

            void updateLikelihood(float *lik);

            void resize(int n);

            void print();
            void saveToFile(int Fileind);

        private:
            TPoseParticle *tmp; 

            void hpsrt(int * indx);

    };
}
#endif