19 #ifndef __ICUB_PARTICLE_MOD_H__
20 #define __ICUB_PARTICLE_MOD_H__
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>
32 #include <yarp/os/RpcClient.h>
34 #include <gsl/gsl_rng.h>
35 #include <gsl/gsl_randist.h>
43 #include <opencv2/core/core_c.h>
44 #include <opencv2/imgproc/imgproc_c.h>
47 #define PARTICLES 1000
62 #define TRANS_X_STD 10.0f
63 #define TRANS_Y_STD 7.5f
64 #define TRANS_S_STD 0.001f
68 #define pfot_B0 1.0000f
73 #define TEMP_LIST_SIZE 10
74 #define TEMP_LIST_PARTICLE_THRES_HIGH 0.9
75 #define TEMP_LIST_PARTICLE_THRES_LOW 0.5
80 yarp::sig::ImageOf<yarp::sig::PixelRgb> *
templ;
116 std::string inputPortName;
117 std::string outputPortName;
118 std::string outputPortNameBlob;
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;
124 CvPoint minloc, maxloc;
125 double minval, maxval;
128 bool getImage, getTemplate, gotTemplate, sendTarget;
129 yarp::sig::ImageOf<yarp::sig::PixelRgb> *iCubImage;
131 IplImage *temp, *res_left, *res_right, *res;
132 yarp::sig::ImageOf<yarp::sig::PixelRgb> *tpl;
134 IplImage* frame, *frame_blob;
135 int width, height, tpl_width, tpl_height, res_width, res_height;
144 int i, j, k, w, h, x, y;
146 yarp::sig::Vector targetTemp;
148 std::string moduleName;
149 std::mutex targetMutex, averageMutex, templateMutex;
151 std::deque< TemplateStruct > tempList;
155 yarp::os::Stamp targetStamp;
157 typedef struct params
171 particle* particles, * new_particles;
174 void free_regions( CvRect** regions,
int n);
176 histogram** compute_ref_histos( IplImage* img, CvRect* rect,
int n );
177 histogram* calc_histogram( IplImage** imgs,
int n );
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 );
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 );
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 );
195 void runAll(IplImage *img);
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);
218 std::string inputPortNameTemp;
219 std::string outputPortNameTarget;
221 yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > imageInTemp;
222 yarp::os::Port target;
223 yarp::os::Port disparityPort;
224 yarp::os::Port target3D;
229 yarp::sig::ImageOf<yarp::sig::PixelRgb> *tpl;
230 std::string moduleName;
238 void setName(std::string module);
249 std::string moduleName;
250 std::string handlerPortName;
252 yarp::os::Port handlerPort;
260 bool configure(yarp::os::ResourceFinder &rf);
263 bool respond(
const yarp::os::Bottle& command, yarp::os::Bottle& reply);
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)
struct TemplateStruct TemplateStruct
int particle_cmp(const void *p1, const void *p2)
yarp::sig::ImageOf< yarp::sig::PixelRgb > * templ