00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00042 #include <ros/ros.h>
00043 #include "opencv_apps/nodelet.h"
00044 #include <image_transport/image_transport.h>
00045 #include <sensor_msgs/image_encodings.h>
00046 #include <cv_bridge/cv_bridge.h>
00047
00048 #include <opencv2/highgui/highgui.hpp>
00049 #include <opencv2/imgproc/imgproc.hpp>
00050
00051 #include <dynamic_reconfigure/server.h>
00052 #include "opencv_apps/ContourMomentsConfig.h"
00053 #include "opencv_apps/Moment.h"
00054 #include "opencv_apps/MomentArray.h"
00055 #include "opencv_apps/MomentArrayStamped.h"
00056
00057 namespace contour_moments {
00058 class ContourMomentsNodelet : public opencv_apps::Nodelet
00059 {
00060 image_transport::Publisher img_pub_;
00061 image_transport::Subscriber img_sub_;
00062 image_transport::CameraSubscriber cam_sub_;
00063 ros::Publisher msg_pub_;
00064
00065 boost::shared_ptr<image_transport::ImageTransport> it_;
00066
00067 typedef contour_moments::ContourMomentsConfig Config;
00068 typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00069 Config config_;
00070 boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00071
00072 bool debug_view_;
00073 ros::Time prev_stamp_;
00074
00075 int low_threshold_;
00076
00077 std::string window_name_;
00078 static bool need_config_update_;
00079
00080 void reconfigureCallback(Config &new_config, uint32_t level)
00081 {
00082 config_ = new_config;
00083 low_threshold_ = config_.canny_low_threshold;
00084 }
00085
00086 const std::string &frameWithDefault(const std::string &frame, const std::string &image_frame)
00087 {
00088 if (frame.empty())
00089 return image_frame;
00090 return frame;
00091 }
00092
00093 void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
00094 {
00095 do_work(msg, cam_info->header.frame_id);
00096 }
00097
00098 void imageCallback(const sensor_msgs::ImageConstPtr& msg)
00099 {
00100 do_work(msg, msg->header.frame_id);
00101 }
00102
00103 static void trackbarCallback( int, void* )
00104 {
00105 need_config_update_ = true;
00106 }
00107
00108 void do_work(const sensor_msgs::ImageConstPtr& msg, const std::string input_frame_from_msg)
00109 {
00110
00111 try
00112 {
00113
00114 cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
00115
00116
00117 opencv_apps::MomentArrayStamped moments_msg;
00118 moments_msg.header = msg->header;
00119
00120
00121 cv::Mat src_gray;
00123 if ( frame.channels() > 1 ) {
00124 cv::cvtColor( frame, src_gray, cv::COLOR_BGR2GRAY );
00125 } else {
00126 src_gray = frame;
00127 }
00128 cv::blur( src_gray, src_gray, cv::Size(3,3) );
00129
00131 if( debug_view_) {
00132 cv::namedWindow( window_name_, cv::WINDOW_AUTOSIZE );
00133 }
00134
00135 cv::Mat canny_output;
00136 std::vector<std::vector<cv::Point> > contours;
00137 std::vector<cv::Vec4i> hierarchy;
00138 cv::RNG rng(12345);
00139
00141 cv::Canny( src_gray, canny_output, low_threshold_ , low_threshold_ *2, 3 );
00143 cv::findContours( canny_output, contours, hierarchy, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
00144
00146 std::vector<cv::Moments> mu(contours.size() );
00147 for( size_t i = 0; i < contours.size(); i++ )
00148 { mu[i] = moments( contours[i], false ); }
00149
00151 std::vector<cv::Point2f> mc( contours.size() );
00152 for( size_t i = 0; i < contours.size(); i++ )
00153 { mc[i] = cv::Point2f( static_cast<float>(mu[i].m10/mu[i].m00) , static_cast<float>(mu[i].m01/mu[i].m00) ); }
00154
00156 cv::Mat drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 );
00157 for( size_t i = 0; i< contours.size(); i++ )
00158 {
00159 cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
00160 cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() );
00161 cv::circle( drawing, mc[i], 4, color, -1, 8, 0 );
00162 }
00163
00165 printf("\t Info: Area and Contour Length \n");
00166 for( size_t i = 0; i< contours.size(); i++ )
00167 {
00168 NODELET_INFO(" * Contour[%d] - Area (M_00) = %.2f - Area OpenCV: %.2f - Length: %.2f \n", (int)i, mu[i].m00, cv::contourArea(contours[i]), cv::arcLength( contours[i], true ) );
00169 cv::Scalar color = cv::Scalar( rng.uniform(0, 255), rng.uniform(0,255), rng.uniform(0,255) );
00170 cv::drawContours( drawing, contours, (int)i, color, 2, 8, hierarchy, 0, cv::Point() );
00171 cv::circle( drawing, mc[i], 4, color, -1, 8, 0 );
00172
00173 opencv_apps::Moment moment_msg;
00174 moment_msg.m00 = mu[i].m00;
00175 moment_msg.m10 = mu[i].m10;
00176 moment_msg.m01 = mu[i].m01;
00177 moment_msg.m20 = mu[i].m20;
00178 moment_msg.m11 = mu[i].m11;
00179 moment_msg.m02 = mu[i].m02;
00180 moment_msg.m30 = mu[i].m30;
00181 moment_msg.m21 = mu[i].m21;
00182 moment_msg.m12 = mu[i].m12;
00183 moment_msg.m03 = mu[i].m03;
00184 moment_msg.mu20 = mu[i].mu20;
00185 moment_msg.mu11 = mu[i].mu11;
00186 moment_msg.mu02 = mu[i].mu02;
00187 moment_msg.mu30 = mu[i].mu30;
00188 moment_msg.mu21 = mu[i].mu21;
00189 moment_msg.mu12 = mu[i].mu12;
00190 moment_msg.mu03 = mu[i].mu03;
00191 moment_msg.nu20 = mu[i].nu20;
00192 moment_msg.nu11 = mu[i].nu11;
00193 moment_msg.nu02 = mu[i].nu02;
00194 moment_msg.nu30 = mu[i].nu30;
00195 moment_msg.nu21 = mu[i].nu21;
00196 moment_msg.nu12 = mu[i].nu12;
00197 moment_msg.nu03 = mu[i].nu03;
00198 opencv_apps::Point2D center_msg;
00199 center_msg.x = mc[i].x;
00200 center_msg.y = mc[i].y;
00201 moment_msg.center = center_msg;
00202 moment_msg.area = cv::contourArea(contours[i]);
00203 moment_msg.length = cv::arcLength(contours[i], true);
00204 moments_msg.moments.push_back(moment_msg);
00205 }
00206
00207 if( debug_view_) {
00208 cv::imshow( window_name_, drawing );
00209 int c = cv::waitKey(1);
00210 }
00211
00212
00213 sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, "bgr8", drawing).toImageMsg();
00214 img_pub_.publish(out_img);
00215 msg_pub_.publish(moments_msg);
00216 }
00217 catch (cv::Exception &e)
00218 {
00219 NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00220 }
00221
00222 prev_stamp_ = msg->header.stamp;
00223 }
00224
00225 void subscribe()
00226 {
00227 NODELET_DEBUG("Subscribing to image topic.");
00228 if (config_.use_camera_info)
00229 cam_sub_ = it_->subscribeCamera("image", 3, &ContourMomentsNodelet::imageCallbackWithInfo, this);
00230 else
00231 img_sub_ = it_->subscribe("image", 3, &ContourMomentsNodelet::imageCallback, this);
00232 }
00233
00234 void unsubscribe()
00235 {
00236 NODELET_DEBUG("Unsubscribing from image topic.");
00237 img_sub_.shutdown();
00238 cam_sub_.shutdown();
00239 }
00240
00241 public:
00242 virtual void onInit()
00243 {
00244 Nodelet::onInit();
00245 it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(*nh_));
00246
00247 pnh_->param("debug_view", debug_view_, false);
00248 if (debug_view_) {
00249 always_subscribe_ = true;
00250 }
00251 prev_stamp_ = ros::Time(0, 0);
00252
00253 window_name_ = "Contours";
00254 low_threshold_ = 100;
00255
00256 reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
00257 dynamic_reconfigure::Server<Config>::CallbackType f =
00258 boost::bind(&ContourMomentsNodelet::reconfigureCallback, this, _1, _2);
00259 reconfigure_server_->setCallback(f);
00260
00261 img_pub_ = advertiseImage(*pnh_, "image", 1);
00262 msg_pub_ = advertise<opencv_apps::MomentArrayStamped>(*pnh_, "moments", 1);
00263 onInitPostProcess();
00264 }
00265 };
00266 bool ContourMomentsNodelet::need_config_update_ = false;
00267 }
00268
00269 #include <pluginlib/class_list_macros.h>
00270 PLUGINLIB_EXPORT_CLASS(contour_moments::ContourMomentsNodelet, nodelet::Nodelet);