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

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

File:
1 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} 
Note: See TracChangeset for help on using the changeset viewer.