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 | |
---|
24 | using namespace std; |
---|
25 | |
---|
26 | //int w=50; |
---|
27 | //int h=80; |
---|
28 | |
---|
29 | int w=20; |
---|
30 | int h=30; |
---|
31 | |
---|
32 | App::App(const string &filename) : |
---|
33 | m_PF(500), |
---|
34 | frame(NULL), |
---|
35 | frame_copy(NULL), |
---|
36 | m_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 | |
---|
58 | App::~App() |
---|
59 | { |
---|
60 | } |
---|
61 | |
---|
62 | static 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 | |
---|
75 | void 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 | |
---|
105 | void 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 | |
---|
112 | void 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 | |
---|
119 | void 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 | |
---|
126 | void 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 | } |
---|