Changeset 260


Ignore:
Timestamp:
01/04/2010 02:38:45 PM (11 years ago)
Author:
dave
Message:

generate weighted averages, calculate the estimate, and visualise all this stuff

Location:
libs/magicsquares/in-progress/pf
Files:
4 edited

Legend:

Unmodified
Added
Removed
  • libs/magicsquares/in-progress/pf/src/App.cpp

    r258 r260  
    7070        {{255,0,0}}, 
    7171        {{255,0,255}}, 
     72                {{255,255,255}} 
    7273    }; 
    7374 
     
    102103} 
    103104 
    104 void Plot(IplImage *Image, ParticleFilter::State State, int colour) 
     105void Plot(IplImage *Image, ParticleFilter::State State, int colour,int size) 
    105106{ 
    106         int x = State.x + 100; 
    107         int y = State.y + 100; 
    108         cvRectangle(Image, cvPoint(x,y), cvPoint(x+1,y+1), colors[colour]);      
     107        int x = State.x*2 + 200; 
     108        int y = State.y*2 + 200; 
     109        cvRectangle(Image, cvPoint(x-size,y-size), cvPoint(x+size,y+size), colors[colour]);      
     110} 
     111 
     112void PlotReal(IplImage *Image, ParticleFilter::State State, int colour) 
     113{ 
     114        int x = State.x*2 + 200; 
     115        int y = State.y*2 + 200; 
     116        cvRectangle(Image, cvPoint(x-1,y-1), cvPoint(x+1,y+1), colors[colour]);  
     117} 
     118 
     119void PlotEst(IplImage *Image, ParticleFilter::State State, int colour) 
     120{ 
     121        int x = State.x*2 + 200; 
     122        int y = State.y*2 + 200; 
     123        cvLine(Image, cvPoint(200,200), cvPoint(x,y), colors[colour]);   
    109124} 
    110125 
     
    113128        int key=cvWaitKey(10); 
    114129         
     130        cvRectangle(camera, cvPoint(0,0), cvPoint(camera->width,camera->height), colors[8], -1);         
     131         
    115132        m_PF.Predict(); 
    116133         
    117         // our actual state 
     134        // Our actual state 
    118135        ParticleFilter::State RealState; 
    119136        RealState.x=50*sin(m_FrameNum*0.01f); 
    120         RealState.y=50; 
     137        RealState.y=-50; 
    121138         
     139        // Create an observation of the state 
    122140        ParticleFilter::Observation Obs = RealState.Observe(); 
    123  
    124         Plot(camera,RealState,1);        
    125                  
    126         m_PF.Update(Obs); 
     141        PlotReal(camera,RealState,1); 
     142         
     143        // Feed the observation in and return the estimated state 
     144        ParticleFilter::State Estimate = m_PF.Update(Obs); 
     145        PlotEst(camera,Estimate,3); 
    127146         
    128147        const vector<ParticleFilter::Particle> &p = m_PF.GetParticles(); 
    129          
    130148        for (vector<ParticleFilter::Particle>::const_iterator i=p.begin(); 
    131149                i!=p.end(); ++i) 
    132150        { 
    133                 Plot(camera,i->m_State,0);       
     151                Plot(camera,i->m_State,0,i->m_Weight*20); 
    134152        } 
    135153} 
  • libs/magicsquares/in-progress/pf/src/ParticleFilter.cpp

    r258 r260  
    2222using namespace std; 
    2323 
     24/////////////////////////////////////////////////////////////// 
     25// Some util functions 
     26 
     27float FloatNoise() 
     28{ 
     29        return rand()%INT_MAX/float(INT_MAX)*2-1; 
     30} 
     31 
     32float GaussianNoise() 
     33{ 
     34        float l=0; 
     35        float x=0; 
     36        float y=0; 
     37        while (l>=1 || l==0) 
     38        { 
     39                x=FloatNoise(); 
     40                y=FloatNoise(); 
     41                l=x*x+y*y; 
     42        } 
     43        return sqrt(-2*log(l)/l)*x; 
     44} 
     45 
     46// gets the angle of a vector from 0 to 360 degrees 
     47float GetAngle(float x, float y) 
     48{ 
     49        if(y==0) // prevent division by 0 
     50        { 
     51                if(x>=0) return 0; 
     52                else return 180; 
     53        } 
     54        else  
     55        { 
     56                if (y>0) return 90-atan(x/y)*180/M_PI; 
     57                else return 270-atan(x/y)*180/M_PI; 
     58        }  
     59} 
     60 
     61//////////////////////////////////////////////////////////////// 
     62// The particle filter 
    2463 
    2564ParticleFilter::ParticleFilter(unsigned int NumParticles) 
     
    3675} 
    3776 
    38 float ParticleFilter::FloatNoise() 
    39 { 
    40         return rand()%INT_MAX/float(INT_MAX)*2-1; 
    41 } 
    42  
    43 float ParticleFilter::GaussianNoise() 
    44 { 
    45         float l=0; 
    46         float x=0; 
    47         float y=0; 
    48         while (l>=1 || l==0) 
    49         { 
    50                 x=FloatNoise(); 
    51                 y=FloatNoise(); 
    52                 l=x*x+y*y; 
    53         } 
    54         return sqrt(-2*log(l)/l)*x; 
    55 } 
    5677 
    5778ParticleFilter::Observation ParticleFilter::State::Observe() 
    5879{ 
    5980        Observation NewObs; 
    60         NewObs.Angle=atan(y/x); 
     81        NewObs.Angle=GetAngle(x,y); 
    6182        NewObs.Dist=sqrt(x*x+y*y); 
    6283        return NewObs; 
     
    85106} 
    86107         
    87 void ParticleFilter::Update(const Observation &Obs) 
     108ParticleFilter::State ParticleFilter::Update(const Observation &Obs) 
    88109{ 
     110        float TotalWeight = 0; 
     111 
    89112        for (vector<Particle>::iterator i=m_Particles.begin();  
    90113                i!=m_Particles.end(); ++i) 
     
    93116                Observation PObs=i->m_State.Observe(); 
    94117                 
    95                 float AngErr = Obs.Angle-PObs.Angle+(m_ObsAngleNoiseLevel*GaussianNoise()); 
    96                 float DistErr = Obs.Dist-PObs.Dist+(m_ObsDistNoiseLevel*GaussianNoise()); 
     118                // todo: angle error will be wrong around 360 -> 0 boundary 
     119                float AngErr = (Obs.Angle-PObs.Angle)+(m_ObsAngleNoiseLevel*GaussianNoise()); 
     120                float DistErr = (Obs.Dist-PObs.Dist)+(m_ObsDistNoiseLevel*GaussianNoise()); 
    97121                 
    98                 i->m_Weight =1/(AngErr*AngErr*100 + DistErr*DistErr); 
     122                i->m_Weight =1/(AngErr*AngErr*0.1 + DistErr*DistErr); 
     123                TotalWeight+=i->m_Weight; 
     124        } 
     125         
     126        // Normalise the weights 
     127        for (vector<Particle>::iterator i=m_Particles.begin();  
     128                i!=m_Particles.end(); ++i) 
     129        {        
     130                i->m_Weight/=TotalWeight; 
     131        } 
     132         
     133        // Find the weighted average (this is our result) 
     134        State ret; 
     135        ret.x=0; ret.y=0; 
     136         
     137        for (vector<Particle>::iterator i=m_Particles.begin();  
     138                i!=m_Particles.end(); ++i) 
     139        {        
     140                ret.x += i->m_State.x * i->m_Weight; 
     141                ret.y += i->m_State.y * i->m_Weight; 
    99142        } 
    100143         
    101144        Resample(); 
     145         
     146        return ret; 
    102147} 
    103148 
  • libs/magicsquares/in-progress/pf/src/ParticleFilter.h

    r258 r260  
    6262        // Set the particle weights according to the current real observation, 
    6363        // and resample particles with low weights, which 'tightens' the particles in. 
    64         void Update(const Observation &Obs); 
     64        State Update(const Observation &Obs); 
    6565 
    6666        void SetNoiseLevels(float Prediction, float ObsAngle, float ObsDist); 
     
    7171 
    7272private: 
    73         float FloatNoise(); 
    74         float GaussianNoise(); 
    7573        void Resample(); 
    7674         
Note: See TracChangeset for help on using the changeset viewer.