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

Revision 291, 5.2 KB checked in by dave, 10 years ago (diff)

made the particle filter generic and added it to the lib

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.Initialise();
53        m_PF.SetNoiseLevels(1);//,0.01,0.01);
54        m_PF.SetResampleWeight(0.005);
55
56        cvNamedWindow( "pf", 1 );
57}
58
59App::~App()
60{
61}
62
63static CvScalar colors[] =
64    {
65        {{0,0,255}},
66        {{0,128,255}},
67        {{0,255,255}},
68        {{0,255,0}},
69        {{255,128,0}},
70        {{255,255,0}},
71        {{255,0,0}},
72        {{255,0,255}},
73                {{0,0,0}},
74                {{255,255,255}}
75    };
76
77void App::Run()
78{
79        frame = cvQueryFrame( m_Capture );
80    if( !frame )
81        {
82                cerr<<"no frame captured"<<endl;
83                return;
84        }
85       
86        if( !frame_copy )
87        frame_copy = cvCreateImage( cvSize(frame->width,frame->height),
88                                    IPL_DEPTH_8U, frame->nChannels );
89    if( frame->origin == IPL_ORIGIN_TL )
90        cvCopy( frame, frame_copy, 0 );
91    else
92        cvFlip( frame, frame_copy, 0 );
93       
94        Update(frame_copy);
95       
96        m_FrameNum++;
97#ifdef SAVE_FRAMES
98        char name[256];
99        sprintf(name,"out-%0.4d.jpg",m_FrameNum);
100        cerr<<"saving "<<name<<endl;
101        cvSaveImage(name,frame_copy);
102#endif
103
104        cvShowImage("pf", frame_copy);
105}
106
107void PlotPoint(IplImage *Image, RadarParticleFilter::RadarState State, int colour,int size)
108{
109        int x = State.x*2 + 200;
110        int y = State.y*2 + 200;
111        cvRectangle(Image, cvPoint(x-size,y-size), cvPoint(x+size,y+size), colors[colour]);
112}
113
114void PlotRect(IplImage *Image, RadarParticleFilter::RadarState State, int colour)
115{
116        int x = State.x*2 + 200;
117        int y = State.y*2 + 200;
118        cvRectangle(Image, cvPoint(x-1,y-1), cvPoint(x+1,y+1), colors[colour]);
119}
120
121void PlotXHairs(IplImage *Image, RadarParticleFilter::RadarState State, int colour)
122{
123        int x = State.x*2 + 200;
124        int y = State.y*2 + 200;
125        cvLine(Image, cvPoint(x,0), cvPoint(x,400), colors[colour]);
126        cvLine(Image, cvPoint(0,y), cvPoint(400,y), colors[colour]);
127}
128
129void PlotRadar(IplImage *Image, RadarParticleFilter::RadarState State, int colour)
130{
131        int x = State.x*2 + 200;
132        int y = State.y*2 + 200;
133        cvLine(Image, cvPoint(200,200), cvPoint(x,y), colors[colour]); 
134}
135
136float avnoise=0;
137float averror=0;
138
139float avx = 0;
140float avy = 0;
141
142void App::Update(IplImage *camera)
143{       
144        int key=cvWaitKey();
145       
146        cvRectangle(camera, cvPoint(0,0), cvPoint(camera->width,camera->height), colors[8], -1);       
147       
148        m_PF.Predict();
149       
150        // Our actual state
151        RadarParticleFilter::RadarState RealState;
152        // We'll move the target on a sine wave - the particle state is not
153        // able to model this behaviour very well, as it only copes with
154        // linear velocity
155        RealState.x=50*sin(m_FrameNum*0.02f);
156        RealState.y=-50;
157       
158        // Create an observation of the state
159        RadarParticleFilter::RadarObservation *Obs =
160        static_cast<RadarParticleFilter::RadarObservation*>(RealState.Observe());
161        // Add noise to the observation
162        Obs->Angle+=GaussianNoise()*4;
163        Obs->Dist+=GaussianNoise()*2;
164        // Recalculate the state for this observation as we want to plot it
165        RadarParticleFilter::RadarState ToPF;
166        GetPos(Obs->Angle, Obs->Dist, ToPF.x, ToPF.y);
167       
168       
169        const vector<RadarParticleFilter::Particle> &p = m_PF.GetParticles();
170        for (vector<RadarParticleFilter::Particle>::const_iterator i=p.begin();
171                i!=p.end(); ++i)
172        {
173                PlotPoint(camera,*((RadarParticleFilter::RadarState*)i->m_State),2,i->m_Weight*100);
174        }
175       
176        // Feed the observation in and return the estimated state
177        RadarParticleFilter::RadarState *Estimate =
178        (RadarParticleFilter::RadarState*)m_PF.Update(Obs);
179       
180        PlotRadar(camera,*Estimate,4);
181       
182        // Plot what is being sent to the filter
183        PlotXHairs(camera,ToPF,3);
184       
185        // Plot the real state we are trying to find
186        PlotXHairs(camera,RealState,1);
187
188        float blend=0.9;       
189       
190        avx=(avx*blend)+(ToPF.x*(1-blend));
191        avy=(avy*blend)+(ToPF.y*(1-blend));
192        //cerr<<"average filter inputs: "<<Distance(avx, avy, RealState.x, RealState.y)<<endl;
193        avnoise=(avnoise*blend)+(Distance(ToPF.x, ToPF.y, RealState.x, RealState.y)*(1-blend));
194        averror=(averror*blend)+(Distance(Estimate->x, Estimate->y, RealState.x, RealState.y)*(1-blend));
195        //cerr<<"average filter error: "<<averror<<endl;
196        cerr<<"performance: "<<avnoise-averror<<endl;
197       
198    delete Estimate;
199    delete Obs;
200}
Note: See TracBrowser for help on using the repository browser.