48 #include <opencv2/highgui/highgui.hpp> 49 #include <opencv2/imgproc/imgproc.hpp> 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" 61 bool compareContourAreas(
const std::vector<cv::Point>& contour1,
const std::vector<cv::Point>& contour2)
63 double i = fabs(contourArea(cv::Mat(contour1)));
64 double j = fabs(contourArea(cv::Mat(contour2)));
77 typedef opencv_apps::ContourMomentsConfig
Config;
94 low_threshold_ = config_.canny_low_threshold;
97 const std::string&
frameWithDefault(
const std::string& frame,
const std::string& image_frame)
106 doWork(msg, cam_info->header.frame_id);
111 doWork(msg, msg->header.frame_id);
116 need_config_update_ =
true;
119 void doWork(
const sensor_msgs::ImageConstPtr& msg,
const std::string& input_frame_from_msg)
128 opencv_apps::MomentArrayStamped moments_msg;
129 moments_msg.header = msg->header;
134 if (frame.channels() > 1)
136 cv::cvtColor(frame, src_gray, cv::COLOR_BGR2GRAY);
142 cv::blur(src_gray, src_gray, cv::Size(3, 3));
147 cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
150 cv::Mat canny_output;
151 std::vector<std::vector<cv::Point> > contours;
152 std::vector<cv::Vec4i> hierarchy;
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));
164 drawing = cv::Mat::zeros(canny_output.size(), CV_8UC3);
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++)
178 for (
size_t i = 0; i < contours.size(); i++)
180 mu[i] = moments(contours[i],
false);
184 for (
size_t i = 0; i < contours.size(); i++)
186 mc[i] = cv::Point2f(static_cast<float>(mu[i].m10 / mu[i].m00), static_cast<float>(mu[i].m01 / mu[i].m00));
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);
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,
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);
235 cv::imshow(window_name_, drawing);
236 int c = cv::waitKey(1);
244 catch (cv::Exception& e)
246 NODELET_ERROR(
"Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
249 prev_stamp_ = msg->header.stamp;
255 if (config_.use_camera_info)
274 pnh_->param(
"queue_size", queue_size_, 3);
275 pnh_->param(
"debug_view", debug_view_,
false);
282 window_name_ =
"Contours";
283 low_threshold_ = 100;
285 reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
286 dynamic_reconfigure::Server<Config>::CallbackType
f =
288 reconfigure_server_->setCallback(f);
291 msg_pub_ = advertise<opencv_apps::MomentArrayStamped>(*
pnh_,
"moments", 1);
305 ROS_WARN(
"DeprecationWarning: Nodelet contour_moments/contour_moments is deprecated, " 306 "and renamed to opencv_apps/contour_moments.");
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
image_transport::Subscriber img_sub_
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
image_transport::CameraSubscriber cam_sub_
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Demo code to calculate moments.
static bool need_config_update_
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.
image_transport::Publisher img_pub_
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...
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...
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
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.
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...
#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...
static void trackbarCallback(int, void *)
opencv_apps::ContourMomentsConfig Config
PLUGINLIB_EXPORT_CLASS(opencv_apps::ContourMomentsNodelet, nodelet::Nodelet)
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const