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