clientimg_detect.cpp
Go to the documentation of this file.
1 /**************************************************************************************************
2  Software License Agreement (BSD License)
3 
4  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
5  All rights reserved.
6 
7  Redistribution and use in source and binary forms, with or without modification, are permitted
8  provided that the following conditions are met:
9 
10  *Redistributions of source code must retain the above copyright notice, this list of
11  conditions and the following disclaimer.
12  *Redistributions in binary form must reproduce the above copyright notice, this list of
13  conditions and the following disclaimer in the documentation and/or other materials provided
14  with the distribution.
15  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
16  endorse or promote products derived from this software without specific prior written permission.
17 
18  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
19  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
20  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
21  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
24  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
25  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***************************************************************************************************/
27 #define _MAIN_C_
28 #include "peddetect.h"
29 #include <opencv2/highgui/highgui.hpp>
30 #include <opencv2/opencv.hpp>
31 #include <image_transport/image_transport.h>
32 #include <opencv/cvwimage.h>
33 #include <opencv/highgui.h>
34 
35 
37 {
38  ros::NodeHandle nh_;
39  image_transport::ImageTransport it_;
40  image_transport::Subscriber image_sub_;
41  image_transport::Publisher image_pub_;
42  int ImgCount;
43  cv_bridge::CvImagePtr cv_ptr;
45  CvRect region;
46  Size minSize;
48  long long int Pedcount, nPedcount;
52  vector<Rect> rectangles;
53 
54 
55 public:
56 
58  nh_("~"),
59  it_ (nh_),
60  Pedcount(0),
61  nPedcount(0)
62  {
63 
64  ImgCount = 0;
65  image_pub_ = it_.advertise ("Image_Out", 1);
66 // cout<<"subscribede to :"<<ros::names::remap("image_in")<<endl;
67  image_sub_ = it_.subscribe (ros::names::remap("image_in"), 1, &PedestrianDetect::imageCb,this);
68 // image_sub_ = it_.subscribe ("Image_In", 1, &PedestrianDetect::imageCb,this);
69 // image_sub_ = it_.subscribe ("image_input", 1, &PedestrianDetect::imageCb, this);
70 // it_.subscribe ("Image_In", 1, &PedestrianDetect::imageCb, this);
71 
72  string classifier_file;
73  nh_.param<string>("classifier",classifier_file,string("not_found"));
74 
75  region.x = 0; region.y = 0; region.width = DWWIDTH; region.height = DWHEIGHT;
76  minSize.width=DWWIDTH; minSize.height=DWHEIGHT;
77 
79  boost_classifier.load(classifier_file.c_str());
80 // boost_classifier.load("/home/pedrobatista/workingcopy/lar3/perception/pedestrians/PedestrianDetect/trained_boost_15Kf_2000w_19Ks_m8_M64_boot3.xml");
81 
82  }
83 
85  {
86 
87 
88 
89  }
90 
91  void imageCb (const sensor_msgs::ImageConstPtr & msg)
92  {
93  try
94  {
95  cv_ptr = cv_bridge::toCvCopy (msg, "rgb8");
96 
97  cv_ptr->image.convertTo (cv_ptr->image, CV_8UC1);
98 
99  }
100  catch (cv_bridge::Exception & e)
101  {
102  ROS_ERROR ("cv_bridge exception: %s", e.what ());
103  return;
104  }
105  Mat Img = cv_ptr->image;
106 
107 // imshow("/Image_Out",Img);
108 // if( waitKey (30) >= 0)
109 
110  if (Img.rows == 0)
111  return;
112 
113 // if (Img.rows<162)
114 // {
115 // Size win; win.width=DWWIDTH; win.height=DWHEIGHT;
116 // resize(Img, Img, win , 0, 0, INTER_LINEAR);
117 // }
118 // else
119 // {
120 // Size win; win.width=640; win.height=480;
121 // resize(Img, Img, win , 0, 0, INTER_LINEAR);
122 // }
123 
124  double secs =ros::Time::now().toSec(); // inicia a contagem do tempo
125 
127  cvtColor (Img, Img, CV_BGR2RGB, 0);
128 
129  features.clear ();
130  PedRect.clear();
131  rectangles.clear();
132  PedRect_Post.clear();
134 
135  cout << "a imagem contem peoes?" << PedRect.size() << endl;
136  if (PedRect.size()!=0)
137  {
138  cout << "sim" << endl;
139  }
140  else {cout << "nao" << endl;}
141 
142 // for (uint n=0; n<PedRect.size(); n++) // processa o quantos pedestres encontrou e arranja um rectangulo para cada um
143 // {
144 //
145 // double FR = (double)Img.rows / (double)PedRect[n].Scale.height, FC=(double)Img.cols / (double)PedRect[n].Scale.width;
146 // cv::Rect BB(Point((double)PedRect[n].x*FC,(double)PedRect[n].y*FR),Point((double)PedRect[n].x*FC+(double)DWWIDTH*FC,(double)PedRect[n].y*FR+(double)DWHEIGHT*FR));
147 //
148 // rectangles.push_back(BB);
149 //
150 // }
151 
152 
153 
154 // if (rectangles.size()!=0) groupRectangles(rectangles, 1, 0.2);
155 
156 
157 // for (uint n=0; n<rectangles.size();n++)
158 // {
159 // rectangle(Img,rectangles[n],Scalar(0,255,0), 2, 8, 0);
160 // // cout<<"width: "<<rectangles[n].width<<" height: "<<rectangles[n].height<<" x: "<<rectangles[n].x<<" y: "<<rectangles[n].y<<endl;
161 //
162 // }
163 // //
164 // // PostProcess(Img, rectangles, randparams, boost_classifier, PedRect_Post);
165 //
166 // Pedcount=PedRect.size();
167 //
168 // nPedcount=features.size()/NRFEATURE-PedRect.size();
169 //
170 // cout<<"Ped: "<<Pedcount<<" nPed: "<<nPedcount<<endl;
171 //
172 // ROS_INFO ("Image nr %d\n", ImgCount++);
173 // // cout<<endl<<features.size()<<endl;
174 // // cout<<endl<<Img.rows<<" x "<<Img.cols<<endl;
175 //
176 // //////////////////////////////////////////
177 //
178 // // cvtColor (Img, Img, CV_RGB2BGR, 0);
179 //
180 // cout << "iniciacao da escrita" << endl;
181 // std::ostringstream oss;
182 // oss<<ImgCount;
183 // // imwrite( "/home/rui/workingcopies/tutorials/src/pedestrian_detection"+oss.str()+".png", Img );
184 // // imwrite( "/home/rui/Documents/2013_PedroSilva/PedestrianDetect/Results/RuiExamples/Frame"+oss.str()+".png", Img );
185 //
186 //
187 // // cout << "image gravada" << endl;
188 // cv_ptr->image = Img;
189 //
190 // sensor_msgs::ImagePtr msg_out = cv_ptr->toImageMsg ();
191 // msg_out->encoding = "rgb8";
192 // msg_out->header.frame_id = "Image_Out";
193 // msg_out->header.stamp = ros::Time::now ();
194 //
195 //
196 // image_pub_.publish (msg_out);
197 
198 // cvtColor(Img, Img, CV_BGR2RGB);
199 
200  cout << "duracao: " << ros::Time::now().toSec() -secs << endl; // fornece o tempo total que o programa demora a correr uma imagem
201 
202 // imshow("processed image",Img);
203 // waitKey(30);
204 
205  }
206 
207 };
208 
209 int main (int argc, char **argv)
210 {
211 // fstream outfile("/home/pedrobatista/workingcopy/lar3/perception/pedestrians/PedestrianDetect/train_seed1234_MIN8_MAX64.csv");
212  ros::init (argc, argv, "image_converter");
213  PedestrianDetect ic;
214 
215  ros::spin ();
216  return 0;
217 
218 }
219 
220 
221 
222 
223 
224 
225 
226 
227 
228 
229 
230 
231 
232 /**************************************************************************************************
233  Software License Agreement (BSD License)
234 
235  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
236  All rights reserved.
237 
238  Redistribution and use in source and binary forms, with or without modification, are permitted
239  provided that the following conditions are met:
240 
241  *Redistributions of source code must retain the above copyright notice, this list of
242  conditions and the following disclaimer.
243  *Redistributions in binary form must reproduce the above copyright notice, this list of
244  conditions and the following disclaimer in the documentation and/or other materials provided
245  with the distribution.
246  *Neither the name of the University of Aveiro nor the names of its contributors may be used to
247  endorse or promote products derived from this software without specific prior written permission.
248 
249  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
250  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
251  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
252  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
253  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
254  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
255  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
256  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
257 ***************************************************************************************************/
258 // #define _MAIN_C_
259 // #include "peddetect.h"
260 //
261 // class PedestrianDetect
262 // {
263 // ros::NodeHandle nh_;
264 // image_transport::ImageTransport it_;
265 // image_transport::Subscriber image_sub_;
266 // image_transport::Publisher image_pub_;
267 // int ImgCount;
268 // cv_bridge::CvImagePtr cv_ptr;
269 // DVector features;
270 // CvRect region;
271 // Size minSize;
272 // FtrVecParams2 randparams;
273 // long long int Pedcount, nPedcount;
274 // PedRoiVec PedRect;
275 // PedRoiVec PedRect_Post;
276 // CvBoost boost_classifier;
277 // vector<Rect> rectangles;
278 //
279 //
280 // public:
281 //
282 // PedestrianDetect ():
283 // nh_("~"),
284 // it_ (nh_)
285 // {
286 //
287 // ImgCount = 0;
288 // image_pub_ = it_.advertise ("Image_Out", 1);
289 // image_sub_ = it_.subscribe ("image_input", 1, &PedestrianDetect::imageCb, this);
290 // // it_.subscribe ("Image_In", 1, &PedestrianDetect::imageCb, this);
291 //
292 // string classifier_file;
293 // nh_.param<string>("classifier",classifier_file,string("not_found"));
294 //
295 // region.x = 0; region.y = 0; region.width = DWWIDTH; region.height = DWHEIGHT;
296 // minSize.width=DWWIDTH; minSize.height=DWHEIGHT;
297 //
298 // GetRandParams(SEED,NRFEATURE2, randparams, region);
299 // boost_classifier.load(classifier_file.c_str());
300 // // boost_classifier.load("/home/pedrobatista/workingcopy/lar3/perception/pedestrians/PedestrianDetect/trained_boost_15Kf_2000w_19Ks_m8_M64_boot3.xml");
301 //
302 // Pedcount=0;
303 // nPedcount=0;
304 //
305 // }
306 //
307 // ~PedestrianDetect ()
308 // {
309 //
310 //
311 //
312 // }
313 //
314 // void imageCb (const sensor_msgs::ImageConstPtr & msg)
315 // {
316 //
317 // try
318 // {
319 //
320 // cv_ptr = cv_bridge::toCvCopy (msg, "rgb8");
321 //
322 // cv_ptr->image.convertTo (cv_ptr->image, CV_8UC1);
323 //
324 // }
325 // catch (cv_bridge::Exception & e)
326 // {
327 // ROS_ERROR ("cv_bridge exception: %s", e.what ());
328 // return;
329 // }
330 // Mat Img = cv_ptr->image;
331 //
332 // if (Img.rows == 0)
333 // return;
334 //
335 // if (Img.rows<162)
336 // {
337 // Size win; win.width=DWWIDTH; win.height=DWHEIGHT;
338 // resize(Img, Img, win , 0, 0, INTER_LINEAR);
339 // }
340 // else
341 // {
342 // Size win; win.width=640; win.height=480;
343 // resize(Img, Img, win , 0, 0, INTER_LINEAR);
344 // }
345 //
346 //
347 //
348 // ///////USE OPENCV TO PROCESS IMAGE////////
349 //
350 // cvtColor (Img, Img, CV_BGR2RGB, 0);
351 //
352 // features.clear ();
353 // PedRect.clear();
354 // rectangles.clear();
355 // PedRect_Post.clear();
356 //
357 //
358 // GetChnFtrsOverImagePyramid(Img , region , features ,NOCTUP, minSize, SCALEPOCT , randparams,PedRect, boost_classifier);
359 //
360 //
361 // for (uint n=0; n<PedRect.size(); n++)
362 // {
363 //
364 // double FR = (double)Img.rows / (double)PedRect[n].Scale.height, FC=(double)Img.cols / (double)PedRect[n].Scale.width;
365 // cv::Rect BB(Point((double)PedRect[n].x*FC,(double)PedRect[n].y*FR),Point((double)PedRect[n].x*FC+(double)DWWIDTH*FC,(double)PedRect[n].y*FR+(double)DWHEIGHT*FR));
366 //
367 // rectangles.push_back(BB);
368 //
369 // }
370 //
371 //
372 // if (rectangles.size()!=0) groupRectangles(rectangles, 1, 0.2);
373 //
374 // for (uint n=0; n<rectangles.size();n++)
375 // {
376 // rectangle(Img,rectangles[n],Scalar(0,255,0), 2, 8, 0);
377 // // cout<<"width: "<<rectangles[n].width<<" height: "<<rectangles[n].height<<" x: "<<rectangles[n].x<<" y: "<<rectangles[n].y<<endl;
378 //
379 // }
380 // //
381 // // PostProcess(Img, rectangles, randparams, boost_classifier, PedRect_Post);
382 //
383 // // // Pedcount+=rectangles.size();
384 // //
385 // // // cout<<"Ped: "<<Pedcount<<" "<<PedRect.size()<<endl;
386 //
387 // Pedcount+=PedRect.size();
388 // nPedcount+=features.size()/NRFEATURE-PedRect.size();
389 // cout<<"Ped: "<<Pedcount<<" nPed: "<<nPedcount<<endl;
390 //
391 // ROS_INFO ("Image nr %d\n", ImgCount++);
392 // // cout<<endl<<features.size()<<endl;
393 // // cout<<endl<<Img.rows<<" x "<<Img.cols<<endl;
394 //
395 // //////////////////////////////////////////
396 //
397 // // cvtColor (Img, Img, CV_RGB2BGR, 0);
398 //
399 // string classifier_result_file;
400 // nh_.param<string>("classifier_result",classifier_result_file,string("not_found"));
401 //
402 // std::ostringstream oss;
403 // oss<<ImgCount;
404 // imwrite(classifier_result_file.c_str()+oss.str()+".png", Img );
405 //
406 // cv_ptr->image = Img;
407 //
408 //
409 //
410 // sensor_msgs::ImagePtr msg_out = cv_ptr->toImageMsg ();
411 // msg_out->encoding = "rgb8";
412 // msg_out->header.frame_id = "Image_Out";
413 // msg_out->header.stamp = ros::Time::now ();
414 //
415 //
416 // image_pub_.publish (msg_out);
417 //
418 //
419 //
420 // }
421 //
422 // };
423 //
424 // int main (int argc, char **argv)
425 // {
426 // // fstream outfile("/home/pedrobatista/workingcopy/lar3/perception/pedestrians/PedestrianDetect/train_seed1234_MIN8_MAX64.csv");
427 // ros::init (argc, argv, "image_converter");
428 // PedestrianDetect ic;
429 // ros::spin ();
430 // return 0;
431 //
432 // }
vector< PedRoi > PedRoiVec
Definition: peddetect.h:91
#define DWHEIGHT
Definition: peddetect.h:56
void GetRandParams(int seed, int NrFtrs, FtrVecParams2 &RandParams, Rect region)
Definition: pedfuncs.cpp:216
void GetChnFtrsOverImagePyramid(Mat Image, CvRect &region, vector< float > &features, int nOctUp, Size minSize, int nPerOct, FtrVecParams2 Params, PedRoiVec &PedRect, CvBoost &boost_classifier)
Definition: pedfuncs.cpp:273
#define NRFEATURE2
Definition: peddetect.h:55
int main(int argc, char **argv)
void imageCb(const sensor_msgs::ImageConstPtr &msg)
FtrVecParams2 randparams
vector< Rect > rectangles
#define NOCTUP
Definition: peddetect.h:63
vector< float > DVector
Definition: peddetect.h:97
#define SCALEPOCT
Definition: peddetect.h:64
long long int nPedcount
long long int Pedcount
#define SEED
Definition: peddetect.h:58
#define DWWIDTH
Definition: peddetect.h:57
image_transport::ImageTransport it_
image_transport::Publisher image_pub_
image_transport::Subscriber image_sub_
vector< FtrParams2 > FtrVecParams2
Definition: peddetect.h:96
ros::NodeHandle nh_
cv_bridge::CvImagePtr cv_ptr


multimodal_pedestrian_detect
Author(s): Rui Azevedo
autogenerated on Mon Mar 2 2015 01:32:27