iCub-main
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 
77 typedef struct TemplateStruct
78 {
79  float w;
80  yarp::sig::ImageOf<yarp::sig::PixelRgb> *templ;
82 
83 
84 int particle_cmp( const void* p1, const void* p2 );
85 
86 
87 class PARTICLEThread : public yarp::os::Thread
88 {
89 public:
90  typedef struct histogram
91  {
92  float histo[NH*NS + NV]; /* histogram array */
93  int n; /* length of histogram array */
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 */
111 
112  float average;
113 
114 private:
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 
198 public:
199  /* class methods */
200  PARTICLEThread();
201  ~PARTICLEThread();
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 
214 class PARTICLEManager : public yarp::os::PeriodicThread
215 {
216 private:
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 
233 public:
234  PARTICLEManager();
237 
238  void setName(std::string module);
239  bool threadInit();
240  void threadRelease();
241  void run();
242 };
243 
244 
245 class 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 
258 public:
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)
struct PARTICLEThread::particle particle
struct PARTICLEThread::histogram histogram
void pushTarget(yarp::sig::Vector &target, yarp::os::Stamp &stamp)
int n
#define NS
struct TemplateStruct TemplateStruct
int particle_cmp(const void *p1, const void *p2)
#define NH
#define MAX_OBJECTS
#define NV
float histo[NH *NS+NV]
yarp::sig::ImageOf< yarp::sig::PixelRgb > * templ