iCub-main
Loading...
Searching...
No Matches
particleFilter.h
Go to the documentation of this file.
1/*
2 * Copyright (C) 2009 RobotCub Consortium, European Commission FP6 Project IST-004370
3 * Authors: Vadim Tikhanoff
4 * email: vadim.tikhanoff@iit.it
5 * website: www.robotcub.org
6 * Permission is granted to copy, distribute, and/or modify this program
7 * under the terms of the GNU General Public License, version 2 or any
8 * later version published by the Free Software Foundation.
9 *
10 * A copy of the license can be found at
11 * http://www.robotcub.org/icub/license/gpl.txt
12 *
13 * This program is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16 * Public License for more details
17 */
18
19#ifndef __ICUB_PARTICLE_MOD_H__
20#define __ICUB_PARTICLE_MOD_H__
21
22#include <yarp/os/BufferedPort.h>
23#include <yarp/os/RFModule.h>
24#include <yarp/os/Network.h>
25#include <yarp/os/Thread.h>
26#include <yarp/os/PeriodicThread.h>
27#include <yarp/os/Time.h>
28#include <yarp/os/Stamp.h>
29#include <yarp/sig/Vector.h>
30#include <yarp/sig/Image.h>
31
32#include <yarp/os/RpcClient.h>
33
34#include <gsl/gsl_rng.h>
35#include <gsl/gsl_randist.h>
36
37#include <mutex>
38#include <time.h>
39#include <string>
40#include <iostream>
41#include <iomanip>
42#include <deque>
43#include <opencv2/core/core_c.h>
44#include <opencv2/imgproc/imgproc_c.h>
45
46/* default number of particles */
47#define PARTICLES 1000
48/* maximum number of objects to be tracked */
49#define MAX_OBJECTS 1
50/* number of bins of HSV in histogram */
51#define NH 10
52#define NS 10
53#define NV 10
54/* low thresholds on saturation and value for histogramming */
55#define S_THRESH 0.1
56#define V_THRESH 0.2
57/* max HSV values */
58#define H_MAX 360.0
59#define S_MAX 1.0
60#define V_MAX 1.0
61/* standard deviations for gaussian sampling in transition model */
62#define TRANS_X_STD 10.0f
63#define TRANS_Y_STD 7.5f
64#define TRANS_S_STD 0.001f
65/* autoregressive dynamics parameters for transition model */
66#define pfot_A1 2.0f
67#define pfot_A2 -1.0f
68#define pfot_B0 1.0000f
69/* distribution parameter */
70#define LAMBDA 10
71
72/* templates list parameters */
73#define TEMP_LIST_SIZE 10
74#define TEMP_LIST_PARTICLE_THRES_HIGH 0.9
75#define TEMP_LIST_PARTICLE_THRES_LOW 0.5
76
77typedef struct TemplateStruct
78{
79 float w;
80 yarp::sig::ImageOf<yarp::sig::PixelRgb> *templ;
82
83
84int particle_cmp( const void* p1, const void* p2 );
85
86
87class PARTICLEThread : public yarp::os::Thread
88{
89public:
90 typedef struct histogram
91 {
92 float histo[NH*NS + NV]; /* histogram array */
93 int n; /* length of histogram array */
94 } histogram;
95
96 typedef struct particle
97 {
98 float x; /* current x coordinate */
99 float y; /* current y coordinate */
100 float s; /* scale */
101 float xp; /* previous x coordinate */
102 float yp; /* previous y coordinate */
103 float sp; /* previous scale */
104 float x0; /* original x coordinate */
105 float y0; /* original y coordinate */
106 int width; /* original width of region described by particle */
107 int height; /* original height of region described by particle */
108 histogram* histo; /* reference histogram describing region being tracked */
109 float w; /* weight */
110 } particle;
111
112 float average;
113
114private:
115 /*port name strings*/
116 std::string inputPortName;
117 std::string outputPortName;
118 std::string outputPortNameBlob;
119
120 yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > imageIn;
121 yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelBgr> > imageOut;
122 yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelMono> > imageOutBlob;
123
124 CvPoint minloc, maxloc;
125 double minval, maxval;
126
127 bool init;
128 bool getImage, getTemplate, gotTemplate, sendTarget;
129 yarp::sig::ImageOf<yarp::sig::PixelRgb> *iCubImage;
130
131 IplImage *temp, *res_left, *res_right, *res;
132 yarp::sig::ImageOf<yarp::sig::PixelRgb> *tpl;
133
134 IplImage* frame, *frame_blob;
135 int width, height, tpl_width, tpl_height, res_width, res_height;
136 double scale;
137 IplImage* img_hsv;
138 gsl_rng* rng;
139 bool firstFrame;
140 CvScalar color;
141 CvRect** regions;
142 int num_objects;
143 float s;
144 int i, j, k, w, h, x, y;
145 int num_particles;
146 yarp::sig::Vector targetTemp;
147 //string containing module name
148 std::string moduleName;
149 std::mutex targetMutex, averageMutex, templateMutex;
150
151 std::deque< TemplateStruct > tempList;
152
153 bool updateNeeded;
154 TemplateStruct bestTempl;
155 yarp::os::Stamp targetStamp;
156
157 typedef struct params
158 {
159 CvPoint loc1[MAX_OBJECTS];
160 CvPoint loc2[MAX_OBJECTS];
161 IplImage* objects[MAX_OBJECTS];
162 char* win_name;
163 IplImage* orig_img;
164 IplImage* cur_img;
165 int n;
166 } params;
167
168 int total;
169
170 histogram** ref_histos;
171 particle* particles, * new_particles;
172
173 void free_histos( histogram** histo, int n );
174 void free_regions( CvRect** regions, int n);
175
176 histogram** compute_ref_histos( IplImage* img, CvRect* rect, int n );
177 histogram* calc_histogram( IplImage** imgs, int n );
178 particle transition( const particle &p, int w, int h, gsl_rng* rng );
179 particle* init_distribution( CvRect* regions, histogram** histos, int n, int p);
180 IplImage* bgr2hsv( IplImage* bgr );
181 float likelihood( IplImage* img, int r, int c, int w, int h, histogram* ref_histo );
182 void normalize_weights( particle* particles, int n );
183 float histo_dist_sq( histogram* h1, histogram* h2 );
184 int histo_bin( float h, float s, float v );
185 float pixval32f(IplImage* img, int r, int c);
186 void setpix32f(IplImage* img, int r, int c, float val);
187 int get_regions( IplImage* frame, CvRect** regions );
188 int get_regionsImage( IplImage* frame, CvRect** regions );
189 particle* resample( particle* particles, int n );
190 void display_particle( IplImage* img, const particle &p, CvScalar color, yarp::sig::Vector& target );
191 void display_particleBlob( IplImage* img, const particle &p, yarp::sig::Vector& target );
192 void trace_template( IplImage* img, const particle &p );
193 void normalize_histogram( histogram* histo );
194 void initAll();
195 void runAll(IplImage *img);
196
197
198public:
199 /* class methods */
202
203 bool threadInit();
204 void threadRelease();
205 void run();
206 void setName(std::string module);
207 void setTemplate(yarp::sig::ImageOf<yarp::sig::PixelRgb> *tpl);
208 void pushTarget(yarp::sig::Vector &target, yarp::os::Stamp &stamp);
209 float getAverage();
211};
212
213
214class PARTICLEManager : public yarp::os::PeriodicThread
215{
216private:
217
218 std::string inputPortNameTemp;
219 std::string outputPortNameTarget;
220
221 yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > imageInTemp;
222 yarp::os::Port target; //vector containing the tracked target 2D and its probability
223 yarp::os::Port disparityPort;
224 yarp::os::Port target3D;
225
226 PARTICLEThread *particleThreadLeft;
227 PARTICLEThread *particleThreadRight;
228
229 yarp::sig::ImageOf<yarp::sig::PixelRgb> *tpl;
230 std::string moduleName;
231
232
233public:
237
238 void setName(std::string module);
239 bool threadInit();
240 void threadRelease();
241 void run();
242};
243
244
245class PARTICLEModule:public yarp::os::RFModule
246{
247
248 /* module parameters */
249 std::string moduleName;
250 std::string handlerPortName;
251
252 yarp::os::Port handlerPort; //a port to handle messages
253
254 /* pointer to a new thread to be created and started in configure() and stopped in close() */
255 PARTICLEManager *particleManager;
256
257
258public:
259
260 bool configure(yarp::os::ResourceFinder &rf); // configure all the module parameters and return true if successful
261 bool interruptModule(); // interrupt, e.g., the ports
262 bool close(); // close and shut down the module
263 bool respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply);
264 double getPeriod();
265 bool updateModule();
266};
267#endif
268//empty line to make gcc happy
269
void setName(std::string module)
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
bool configure(yarp::os::ResourceFinder &rf)
TemplateStruct getBestTemplate()
void setName(std::string module)
void setTemplate(yarp::sig::ImageOf< yarp::sig::PixelRgb > *tpl)
void pushTarget(yarp::sig::Vector &target, yarp::os::Stamp &stamp)
int n
#define NS
int particle_cmp(const void *p1, const void *p2)
#define NH
#define MAX_OBJECTS
#define NV
yarp::sig::ImageOf< yarp::sig::PixelRgb > * templ