people_detect_nodelet.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, Kei Okada.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Kei Okada nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 // https://github.com/Itseez/opencv/blob/2.4/samples/cpp/peopledetect.cpp
41 #include <ros/ros.h>
42 #include "opencv_apps/nodelet.h"
44 #include <cv_bridge/cv_bridge.h>
46 
47 #include <opencv2/highgui/highgui.hpp>
48 #include <opencv2/imgproc/imgproc.hpp>
49 #include <opencv2/objdetect/objdetect.hpp>
50 
51 #include <dynamic_reconfigure/server.h>
52 #include "opencv_apps/PeopleDetectConfig.h"
53 #include "opencv_apps/Rect.h"
54 #include "opencv_apps/RectArrayStamped.h"
55 
56 namespace opencv_apps
57 {
59 {
64 
66 
67  typedef opencv_apps::PeopleDetectConfig Config;
68  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
69  Config config_;
71 
75 
76  std::string window_name_;
77  static bool need_config_update_;
78 
79  cv::HOGDescriptor hog_;
80 
83  int padding_;
84  double scale0_;
86 
87  void reconfigureCallback(Config& new_config, uint32_t level)
88  {
89  config_ = new_config;
90  hit_threshold_ = config_.hit_threshold;
91  win_stride_ = config_.win_stride;
92  padding_ = config_.padding;
93  scale0_ = config_.scale0;
94  group_threshold_ = config_.group_threshold;
95  }
96 
97  const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame)
98  {
99  if (frame.empty())
100  return image_frame;
101  return frame;
102  }
103 
104  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
105  {
106  doWork(msg, cam_info->header.frame_id);
107  }
108 
109  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
110  {
111  doWork(msg, msg->header.frame_id);
112  }
113 
114  static void trackbarCallback(int /*unused*/, void* /*unused*/)
115  {
116  need_config_update_ = true;
117  }
118 
119  void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg)
120  {
121  // Work on the image.
122  try
123  {
124  // Convert the image into something opencv can handle.
125  cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
126 
127  // Messages
128  opencv_apps::RectArrayStamped found_msg;
129  found_msg.header = msg->header;
130 
131  // Do the work
132  if (debug_view_)
133  {
134  cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
135  }
136 
137  std::vector<cv::Rect> found, found_filtered;
138  double t = (double)cv::getTickCount();
139  // run the detector with default parameters. to get a higher hit-rate
140  // (and more false alarms, respectively), decrease the hitThreshold and
141  // groupThreshold (set groupThreshold to 0 to turn off the grouping completely).
142  hog_.detectMultiScale(frame, found, hit_threshold_, cv::Size(win_stride_, win_stride_),
143  cv::Size(padding_, padding_), scale0_, group_threshold_);
144  t = (double)cv::getTickCount() - t;
145  NODELET_INFO("tdetection time = %gms", t * 1000. / cv::getTickFrequency());
146  size_t i, j;
147  for (i = 0; i < found.size(); i++)
148  {
149  cv::Rect r = found[i];
150  for (j = 0; j < found.size(); j++)
151  if (j != i && (r & found[j]) == r)
152  break;
153  if (j == found.size())
154  found_filtered.push_back(r);
155  }
156  for (i = 0; i < found_filtered.size(); i++)
157  {
158  cv::Rect r = found_filtered[i];
159  // the HOG detector returns slightly larger rectangles than the real objects.
160  // so we slightly shrink the rectangles to get a nicer output.
161  r.x += cvRound(r.width * 0.1);
162  r.width = cvRound(r.width * 0.8);
163  r.y += cvRound(r.height * 0.07);
164  r.height = cvRound(r.height * 0.8);
165  cv::rectangle(frame, r.tl(), r.br(), cv::Scalar(0, 255, 0), 3);
166 
167  opencv_apps::Rect rect_msg;
168  rect_msg.x = r.x;
169  rect_msg.y = r.y;
170  rect_msg.width = r.width;
171  rect_msg.height = r.height;
172  found_msg.rects.push_back(rect_msg);
173  }
174 
175  //-- Show what you got
176  if (debug_view_)
177  {
178  cv::imshow(window_name_, frame);
179  int c = cv::waitKey(1);
180  }
181 
182  // Publish the image.
183  sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, frame).toImageMsg();
184  img_pub_.publish(out_img);
185  msg_pub_.publish(found_msg);
186  }
187  catch (cv::Exception& e)
188  {
189  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
190  }
191 
192  prev_stamp_ = msg->header.stamp;
193  }
194 
195  void subscribe() // NOLINT(modernize-use-override)
196  {
197  NODELET_DEBUG("Subscribing to image topic.");
198  if (config_.use_camera_info)
199  cam_sub_ = it_->subscribeCamera("image", queue_size_, &PeopleDetectNodelet::imageCallbackWithInfo, this);
200  else
201  img_sub_ = it_->subscribe("image", queue_size_, &PeopleDetectNodelet::imageCallback, this);
202  }
203 
204  void unsubscribe() // NOLINT(modernize-use-override)
205  {
206  NODELET_DEBUG("Unsubscribing from image topic.");
207  img_sub_.shutdown();
208  cam_sub_.shutdown();
209  }
210 
211 public:
212  virtual void onInit() // NOLINT(modernize-use-override)
213  {
214  Nodelet::onInit();
216 
217  pnh_->param("queue_size", queue_size_, 3);
218  pnh_->param("debug_view", debug_view_, false);
219  if (debug_view_)
220  {
221  always_subscribe_ = true;
222  }
223  prev_stamp_ = ros::Time(0, 0);
224 
225  window_name_ = "people detector";
226 
227  reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
228  dynamic_reconfigure::Server<Config>::CallbackType f =
229  boost::bind(&PeopleDetectNodelet::reconfigureCallback, this, _1, _2);
230  reconfigure_server_->setCallback(f);
231 
232  hog_.setSVMDetector(cv::HOGDescriptor::getDefaultPeopleDetector());
233 
234  img_pub_ = advertiseImage(*pnh_, "image", 1);
235  msg_pub_ = advertise<opencv_apps::RectArrayStamped>(*pnh_, "found", 1);
236 
238  }
239 };
241 } // namespace opencv_apps
242 
243 namespace people_detect
244 {
246 {
247 public:
248  virtual void onInit() // NOLINT(modernize-use-override)
249  {
250  ROS_WARN("DeprecationWarning: Nodelet people_detect/people_detect is deprecated, "
251  "and renamed to opencv_apps/people_detect.");
253  }
254 };
255 } // namespace people_detect
256 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
boost::shared_ptr< image_transport::ImageTransport > it_
opencv_apps::PeopleDetectConfig Config
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
f
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Definition: nodelet.h:70
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
Demo code to calculate moments.
Definition: nodelet.h:48
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
Definition: nodelet.h:250
#define ROS_WARN(...)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
void doWork(const sensor_msgs::ImageConstPtr &msg, const std::string &input_frame_from_msg)
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
Definition: nodelet.h:273
virtual void onInitPostProcess()
Post processing of initialization of nodelet. You need to call this method in order to use always_sub...
Definition: nodelet.cpp:57
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
Definition: nodelet.cpp:40
void publish(const sensor_msgs::Image &message) const
PLUGINLIB_EXPORT_CLASS(opencv_apps::PeopleDetectNodelet, nodelet::Nodelet)
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
Definition: nodelet.h:245
image_transport::Subscriber img_sub_
void reconfigureCallback(Config &new_config, uint32_t level)
image_transport::Publisher advertiseImage(ros::NodeHandle &nh, const std::string &topic, int queue_size)
Advertise an image topic and watch the publisher. Publishers which are created by this method...
Definition: nodelet.h:180
dynamic_reconfigure::Server< Config > ReconfigureServer
#define NODELET_INFO(...)
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
image_transport::Publisher img_pub_
static void trackbarCallback(int, void *)
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
image_transport::CameraSubscriber cam_sub_
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const
boost::shared_ptr< ReconfigureServer > reconfigure_server_


opencv_apps
Author(s): Kei Okada
autogenerated on Wed Apr 24 2019 03:00:17