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
00043
00044 #include <pal_vision_segmentation/TemplateSegmentConfig.h>
00045 #include <pal_vision_segmentation/image_processing.h>
00046
00047
00048 #include <ros/ros.h>
00049 #include <image_transport/image_transport.h>
00050 #include <cv_bridge/cv_bridge.h>
00051 #include <sensor_msgs/image_encodings.h>
00052 #include <dynamic_reconfigure/server.h>
00053
00054
00055 #include <opencv2/core/core.hpp>
00056 #include <opencv2/imgproc/imgproc.hpp>
00057 #include <opencv2/highgui/highgui.hpp>
00058
00059
00060 image_transport::Publisher mask_pub;
00061 int window_width;
00062 int window_height;
00063 int coeffs_to_cancel;
00064 cv::Mat image_rect;
00065 image_transport::Publisher image_pub;
00066 image_transport::Publisher debug_pub;
00067 cv::Mat target_template;
00068 cv::Mat raw_template;
00069 int offset_x, offset_y;
00070
00071
00072 void imageCb(const sensor_msgs::ImageConstPtr& msg)
00073 {
00074 if(image_pub.getNumSubscribers() > 0 ||
00075 mask_pub.getNumSubscribers() > 0 ||
00076 debug_pub.getNumSubscribers() > 0)
00077 {
00078 cv_bridge::CvImagePtr cv_ptr;
00079 try
00080 {
00081 cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
00082 }
00083 catch (cv_bridge::Exception& e)
00084 {
00085 ROS_ERROR("cv_bridge exception: %s", e.what());
00086 return;
00087 }
00088
00089 image_rect = cv_ptr->image;
00090
00091 double ticksBefore = cv::getTickCount();
00092
00093 cv::Mat matched;
00094 cv::Mat dct_d;
00095 pal_vision_util::dctNormalization(image_rect, dct_d, coeffs_to_cancel);
00096 cv::matchTemplate(dct_d, target_template, matched, CV_TM_CCORR_NORMED);
00097
00098 double min_val, max_val;
00099 cv::Point max_loc, min_loc;
00100 cv::minMaxLoc(matched, &min_val, &max_val, &min_loc, &max_loc, cv::Mat());
00101
00102 cv::Mat mask;
00103 mask = cv::Mat::zeros(image_rect.rows, image_rect.cols, CV_8UC1);
00104
00105 const int start_col = (max_loc.x + offset_x - window_width/2 < 0) ?
00106 0 : (max_loc.x + offset_x - window_width/2);
00107 const int start_row = (max_loc.y + offset_y - window_height/2 < 0) ?
00108 0 : (max_loc.y + offset_y - window_height/2);
00109 const int end_col = (window_width/2 + max_loc.x + offset_x > mask.rows) ?
00110 mask.rows : (window_width/2 + offset_x + max_loc.x);
00111 const int end_row = (window_height/2 + max_loc.y + offset_y > mask.cols) ?
00112 mask.cols : (window_height/2 + max_loc.y + offset_y);
00113 for(int i=start_row; i<end_row; ++i)
00114 for(int j=start_col; j<end_col; ++j)
00115 mask.at<uchar>(i,j) = 255;
00116
00117 if(mask_pub.getNumSubscribers() > 0)
00118 {
00119 cv_bridge::CvImage mask_msg;
00120 mask_msg.header = msg->header;
00121 mask_msg.encoding = sensor_msgs::image_encodings::MONO8;
00122 mask_msg.image = mask;
00123 mask_pub.publish(mask_msg.toImageMsg());
00124 }
00125
00126 if(image_pub.getNumSubscribers() > 0)
00127 {
00128 cv::Mat masked;
00129 image_rect.copyTo(masked, mask);
00130 cv_bridge::CvImage masked_msg;
00131 masked_msg.header = msg->header;
00132 masked_msg.encoding = sensor_msgs::image_encodings::BGR8;
00133 masked_msg.image = masked;
00134 image_pub.publish(*masked_msg.toImageMsg());
00135 }
00136
00137
00138 if(debug_pub.getNumSubscribers() > 0)
00139 {
00140 cv::rectangle(dct_d, cv::Rect(start_col, start_row, window_width, window_height), cv::Scalar(255,0,0), 2);
00141 cv::circle(dct_d, cv::Point(max_loc.x + offset_x, max_loc.y + offset_y), 5, cv::Scalar(255,0,0), 1);
00142 cv_bridge::CvImage debug_msg;
00143 debug_msg.header = msg->header;
00144 debug_msg.encoding = sensor_msgs::image_encodings::TYPE_8UC3;
00145 debug_msg.image = dct_d;
00146 debug_pub.publish(*debug_msg.toImageMsg());
00147 }
00148
00149 ROS_INFO("imageCb runtime: %f ms",
00150 1000*(cv::getTickCount() - ticksBefore)/cv::getTickFrequency());
00151 }
00152 }
00153
00154 void reconf_callback(pal_vision_segmentation::TemplateSegmentConfig &config, uint32_t level)
00155 {
00156 window_width = config.window_width;
00157 window_height = config.window_height;
00158 coeffs_to_cancel = config.coeffs_to_cancel;
00159
00160 pal_vision_util::dctNormalization(raw_template, target_template, coeffs_to_cancel);
00161 }
00162
00163 int main(int argc, char *argv[] )
00164 {
00165 ros::init(argc, argv, "template_segmentation");
00166 ros::NodeHandle nh("template_segmentation");
00167 nh.param<int>("window_width", window_width, 10);
00168 nh.param<int>("window_height", window_height, 10);
00169 nh.param<int>("coeffs_to_cancel", coeffs_to_cancel, 2);
00170
00171 if(argc < 2)
00172 {
00173 ROS_ERROR("Template segmentation needs a template to match. Please provide it as a command line argument.");
00174 return -1;
00175 }
00176
00177 std::string template_path(argv[1]);
00178 ROS_INFO("%s", template_path.c_str());
00179 raw_template = cv::imread(template_path);
00180 pal_vision_util::dctNormalization(raw_template, target_template, coeffs_to_cancel);
00181
00182 offset_x = (target_template.rows-1)/2;
00183 offset_y = (target_template.cols-1)/2;
00184
00185 image_transport::ImageTransport it(nh);
00186 image_transport::Subscriber rect_sub = it.subscribe("/image", 1, &imageCb);
00187 image_pub = it.advertise("image_masked", 1);
00188 mask_pub = it.advertise("mask", 1);
00189 debug_pub = it.advertise("debug",1);
00190
00191 dynamic_reconfigure::Server<pal_vision_segmentation::TemplateSegmentConfig> server;
00192 dynamic_reconfigure::Server<pal_vision_segmentation::TemplateSegmentConfig>::CallbackType f;
00193 f = boost::bind(&reconf_callback, _1, _2);
00194 server.setCallback(f);
00195
00196 ros::spin();
00197 }
00198