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

Revision 260, 3.6 KB checked in by dave, 10 years ago (diff)

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

Line 
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                {{255,255,255}}
73    };
74
75void App::Run()
76{
77        frame = cvQueryFrame( m_Capture );
78    if( !frame )
79        {
80                cerr<<"no frame captured"<<endl;
81                return;
82        }
83       
84        if( !frame_copy )
85        frame_copy = cvCreateImage( cvSize(frame->width,frame->height),
86                                    IPL_DEPTH_8U, frame->nChannels );
87    if( frame->origin == IPL_ORIGIN_TL )
88        cvCopy( frame, frame_copy, 0 );
89    else
90        cvFlip( frame, frame_copy, 0 );
91       
92        Update(frame_copy);
93       
94        m_FrameNum++;
95#ifdef SAVE_FRAMES
96        char name[256];
97        sprintf(name,"out-%0.4d.jpg",m_FrameNum);
98        cerr<<"saving "<<name<<endl;
99        cvSaveImage(name,frame_copy);
100#endif
101
102        cvShowImage("pf", frame_copy);
103}
104
105void Plot(IplImage *Image, ParticleFilter::State State, int colour,int size)
106{
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]); 
124}
125
126void App::Update(IplImage *camera)
127{       
128        int key=cvWaitKey(10);
129       
130        cvRectangle(camera, cvPoint(0,0), cvPoint(camera->width,camera->height), colors[8], -1);       
131       
132        m_PF.Predict();
133       
134        // Our actual state
135        ParticleFilter::State RealState;
136        RealState.x=50*sin(m_FrameNum*0.01f);
137        RealState.y=-50;
138       
139        // Create an observation of the state
140        ParticleFilter::Observation Obs = RealState.Observe();
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);
146       
147        const vector<ParticleFilter::Particle> &p = m_PF.GetParticles();
148        for (vector<ParticleFilter::Particle>::const_iterator i=p.begin();
149                i!=p.end(); ++i)
150        {
151                Plot(camera,i->m_State,0,i->m_Weight*20);
152        }
153}
Note: See TracBrowser for help on using the repository browser.