source: libs/magicsquares/in-progress/pf/src/App.cpp @ 258

Revision 258, 2.9 KB checked in by dave, 11 years ago (diff)

added particle filter example

RevLine 
[258]1// Copyright (C) 2009 foam
2//
3// This program is free software; you can redistribute it and/or modify
4// it under the terms of the GNU General Public License as published by
5// the Free Software Foundation; either version 2 of the License, or
6// (at your option) any later version.
7//
8// This program is distributed in the hope that it will be useful,
9// but WITHOUT ANY WARRANTY; without even the implied warranty of
10// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11// GNU General Public License for more details.
12//
13// You should have received a copy of the GNU General Public License
14// along with this program; if not, write to the Free Software
15// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
16
17#include <assert.h>
18#include <iostream>
19#include "App.h"
20#include "ParticleFilter.h"
21
22//#define SAVE_FRAMES
23
24using namespace std;
25
26//int w=50;
27//int h=80;
28
29int w=20;
30int h=30;
31
32App::App(const string &filename) :
33m_PF(500),
34frame(NULL),
35frame_copy(NULL),
36m_FrameNum(0)
37{
38        if (filename=="")
39        {
40                m_Capture = cvCaptureFromCAM(0);
41        }
42        else
43        {
44                m_Capture = cvCaptureFromAVI(filename.c_str());
45        }
46       
47        assert(m_Capture);
48       
49        cvInitFont( &m_Font, CV_FONT_HERSHEY_PLAIN, 0.5, 0.5, 0, 1, CV_AA );
50        cvInitFont( &m_LargeFont, CV_FONT_HERSHEY_PLAIN, 25, 25, 0, 10, CV_AA );
51
52        m_PF.SetNoiseLevels(1,0.01,0.01);
53        m_PF.SetResampleWeight(0.01);
54
55        cvNamedWindow( "pf", 1 );
56}
57
58App::~App()
59{
60}
61
62static CvScalar colors[] =
63    {
64        {{0,0,255}},
65        {{0,128,255}},
66        {{0,255,255}},
67        {{0,255,0}},
68        {{255,128,0}},
69        {{255,255,0}},
70        {{255,0,0}},
71        {{255,0,255}},
72    };
73
74void App::Run()
75{
76        frame = cvQueryFrame( m_Capture );
77    if( !frame )
78        {
79                cerr<<"no frame captured"<<endl;
80                return;
81        }
82       
83        if( !frame_copy )
84        frame_copy = cvCreateImage( cvSize(frame->width,frame->height),
85                                    IPL_DEPTH_8U, frame->nChannels );
86    if( frame->origin == IPL_ORIGIN_TL )
87        cvCopy( frame, frame_copy, 0 );
88    else
89        cvFlip( frame, frame_copy, 0 );
90       
91        Update(frame_copy);
92       
93        m_FrameNum++;
94#ifdef SAVE_FRAMES
95        char name[256];
96        sprintf(name,"out-%0.4d.jpg",m_FrameNum);
97        cerr<<"saving "<<name<<endl;
98        cvSaveImage(name,frame_copy);
99#endif
100
101        cvShowImage("pf", frame_copy);
102}
103
104void Plot(IplImage *Image, ParticleFilter::State State, int colour)
105{
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]);     
109}
110
111void App::Update(IplImage *camera)
112{       
113        int key=cvWaitKey(10);
114       
115        m_PF.Predict();
116       
117        // our actual state
118        ParticleFilter::State RealState;
119        RealState.x=50*sin(m_FrameNum*0.01f);
120        RealState.y=50;
121       
122        ParticleFilter::Observation Obs = RealState.Observe();
123
124        Plot(camera,RealState,1);       
125               
126        m_PF.Update(Obs);
127       
128        const vector<ParticleFilter::Particle> &p = m_PF.GetParticles();
129       
130        for (vector<ParticleFilter::Particle>::const_iterator i=p.begin();
131                i!=p.end(); ++i)
132        {
133                Plot(camera,i->m_State,0);     
134        }
135}
Note: See TracBrowser for help on using the repository browser.