contour_moments_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/tutorial_code/ShapeDescriptors/moments_demo.cpp
42 #include <ros/ros.h>
43 #include "opencv_apps/nodelet.h"
46 #include <cv_bridge/cv_bridge.h>
47 
48 #include <opencv2/highgui/highgui.hpp>
49 #include <opencv2/imgproc/imgproc.hpp>
50 
51 #include <dynamic_reconfigure/server.h>
52 #include "opencv_apps/ContourMomentsConfig.h"
53 #include "opencv_apps/Moment.h"
54 #include "opencv_apps/MomentArray.h"
55 #include "opencv_apps/MomentArrayStamped.h"
56 
57 namespace opencv_apps
58 {
59 // https://stackoverflow.com/questions/13495207/opencv-c-sorting-contours-by-their-contourarea
60 // comparison function object
61 bool compareContourAreas(const std::vector<cv::Point>& contour1, const std::vector<cv::Point>& contour2)
62 {
63  double i = fabs(contourArea(cv::Mat(contour1)));
64  double j = fabs(contourArea(cv::Mat(contour2)));
65  return (i > j);
66 }
67 
69 {
74 
76 
77  typedef opencv_apps::ContourMomentsConfig Config;
78  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
79  Config config_;
81 
85 
87 
88  std::string window_name_;
89  static bool need_config_update_;
90 
91  void reconfigureCallback(Config& new_config, uint32_t level)
92  {
93  config_ = new_config;
94  low_threshold_ = config_.canny_low_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, msg->encoding)->image;
126 
127  // Messages
128  opencv_apps::MomentArrayStamped moments_msg;
129  moments_msg.header = msg->header;
130 
131  // Do the work
132  cv::Mat src_gray;
134  if (frame.channels() > 1)
135  {
136  cv::cvtColor(frame, src_gray, cv::COLOR_BGR2GRAY);
137  }
138  else
139  {
140  src_gray = frame;
141  }
142  cv::blur(src_gray, src_gray, cv::Size(3, 3));
143 
145  if (debug_view_)
146  {
147  cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
148  }
149 
150  cv::Mat canny_output;
151  std::vector<std::vector<cv::Point> > contours;
152  std::vector<cv::Vec4i> hierarchy;
153  cv::RNG rng(12345);
154 
156  cv::Canny(src_gray, canny_output, low_threshold_, low_threshold_ * 2, 3);
158  cv::findContours(canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0));
159 
161  cv::Mat drawing;
162  if (debug_view_)
163  {
164  drawing = cv::Mat::zeros(canny_output.size(), CV_8UC3);
165  }
166 
168  NODELET_INFO("\t Info: Area and Contour Length");
169 
170  // https://stackoverflow.com/questions/13495207/opencv-c-sorting-contours-by-their-contourarea
171  std::sort(contours.begin(), contours.end(), compareContourAreas);
172 
173  std::vector<cv::Moments> mu(contours.size());
174  std::vector<cv::Point2f> mc(contours.size());
175  for (size_t i = 0; i < contours.size(); i++)
176  {
178  for (size_t i = 0; i < contours.size(); i++)
179  {
180  mu[i] = moments(contours[i], false);
181  }
182 
184  for (size_t i = 0; i < contours.size(); i++)
185  {
186  mc[i] = cv::Point2f(static_cast<float>(mu[i].m10 / mu[i].m00), static_cast<float>(mu[i].m01 / mu[i].m00));
187  }
188 
189  if (debug_view_)
190  {
191  cv::Scalar color = cv::Scalar(rng.uniform(0, 255), rng.uniform(0, 255), rng.uniform(0, 255));
192  cv::drawContours(drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point());
193  cv::circle(drawing, mc[i], 4, color, -1, 8, 0);
194  }
195  NODELET_INFO(" * Contour[%d] - Area (M_00) = %.2f - Area OpenCV: %.2f - Length: %.2f - Center (%.2f, %.2f)",
196  (int)i, mu[i].m00, cv::contourArea(contours[i]), cv::arcLength(contours[i], true), mc[i].x,
197  mc[i].y);
198 
199  opencv_apps::Moment moment_msg;
200  moment_msg.m00 = mu[i].m00;
201  moment_msg.m10 = mu[i].m10;
202  moment_msg.m01 = mu[i].m01;
203  moment_msg.m20 = mu[i].m20;
204  moment_msg.m11 = mu[i].m11;
205  moment_msg.m02 = mu[i].m02;
206  moment_msg.m30 = mu[i].m30;
207  moment_msg.m21 = mu[i].m21;
208  moment_msg.m12 = mu[i].m12;
209  moment_msg.m03 = mu[i].m03;
210  moment_msg.mu20 = mu[i].mu20;
211  moment_msg.mu11 = mu[i].mu11;
212  moment_msg.mu02 = mu[i].mu02;
213  moment_msg.mu30 = mu[i].mu30;
214  moment_msg.mu21 = mu[i].mu21;
215  moment_msg.mu12 = mu[i].mu12;
216  moment_msg.mu03 = mu[i].mu03;
217  moment_msg.nu20 = mu[i].nu20;
218  moment_msg.nu11 = mu[i].nu11;
219  moment_msg.nu02 = mu[i].nu02;
220  moment_msg.nu30 = mu[i].nu30;
221  moment_msg.nu21 = mu[i].nu21;
222  moment_msg.nu12 = mu[i].nu12;
223  moment_msg.nu03 = mu[i].nu03;
224  opencv_apps::Point2D center_msg;
225  center_msg.x = mc[i].x;
226  center_msg.y = mc[i].y;
227  moment_msg.center = center_msg;
228  moment_msg.area = cv::contourArea(contours[i]);
229  moment_msg.length = cv::arcLength(contours[i], true);
230  moments_msg.moments.push_back(moment_msg);
231  }
232 
233  if (debug_view_)
234  {
235  cv::imshow(window_name_, drawing);
236  int c = cv::waitKey(1);
237  }
238 
239  // Publish the image.
240  sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", drawing).toImageMsg();
241  img_pub_.publish(out_img);
242  msg_pub_.publish(moments_msg);
243  }
244  catch (cv::Exception& e)
245  {
246  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
247  }
248 
249  prev_stamp_ = msg->header.stamp;
250  }
251 
252  void subscribe() // NOLINT(modernize-use-override)
253  {
254  NODELET_DEBUG("Subscribing to image topic.");
255  if (config_.use_camera_info)
256  cam_sub_ = it_->subscribeCamera("image", queue_size_, &ContourMomentsNodelet::imageCallbackWithInfo, this);
257  else
258  img_sub_ = it_->subscribe("image", queue_size_, &ContourMomentsNodelet::imageCallback, this);
259  }
260 
261  void unsubscribe() // NOLINT(modernize-use-override)
262  {
263  NODELET_DEBUG("Unsubscribing from image topic.");
264  img_sub_.shutdown();
265  cam_sub_.shutdown();
266  }
267 
268 public:
269  virtual void onInit() // NOLINT(modernize-use-override)
270  {
271  Nodelet::onInit();
273 
274  pnh_->param("queue_size", queue_size_, 3);
275  pnh_->param("debug_view", debug_view_, false);
276  if (debug_view_)
277  {
278  always_subscribe_ = true;
279  }
280  prev_stamp_ = ros::Time(0, 0);
281 
282  window_name_ = "Contours";
283  low_threshold_ = 100; // only for canny
284 
285  reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
286  dynamic_reconfigure::Server<Config>::CallbackType f =
287  boost::bind(&ContourMomentsNodelet::reconfigureCallback, this, _1, _2);
288  reconfigure_server_->setCallback(f);
289 
290  img_pub_ = advertiseImage(*pnh_, "image", 1);
291  msg_pub_ = advertise<opencv_apps::MomentArrayStamped>(*pnh_, "moments", 1);
293  }
294 };
296 } // namespace opencv_apps
297 
299 {
301 {
302 public:
303  virtual void onInit() // NOLINT(modernize-use-override)
304  {
305  ROS_WARN("DeprecationWarning: Nodelet contour_moments/contour_moments is deprecated, "
306  "and renamed to opencv_apps/contour_moments.");
308  }
309 };
310 } // namespace contour_moments
311 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
image_transport::CameraSubscriber cam_sub_
f
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Definition: nodelet.h:70
Demo code to calculate moments.
Definition: nodelet.h:48
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
void reconfigureCallback(Config &new_config, uint32_t level)
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
Definition: nodelet.h:250
#define ROS_WARN(...)
boost::shared_ptr< ReconfigureServer > reconfigure_server_
void doWork(const sensor_msgs::ImageConstPtr &msg, const std::string &input_frame_from_msg)
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
Definition: nodelet.h:273
dynamic_reconfigure::Server< Config > ReconfigureServer
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
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
Definition: nodelet.h:245
boost::shared_ptr< image_transport::ImageTransport > it_
bool compareContourAreas(const std::vector< cv::Point > &contour1, const std::vector< cv::Point > &contour2)
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
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
#define NODELET_INFO(...)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
opencv_apps::ContourMomentsConfig Config
PLUGINLIB_EXPORT_CLASS(opencv_apps::ContourMomentsNodelet, nodelet::Nodelet)
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const


opencv_apps
Author(s): Kei Okada
autogenerated on Sat Aug 22 2020 03:35:08