|
| 1 | +/********************************************************************* |
| 2 | +* Software License Agreement (BSD License) |
| 3 | +* |
| 4 | +* Copyright (c) 2017, Ryosuke Tajima. |
| 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 Ryosuke Tajima 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/opencv/opencv/raw/master/samples/cpp/tutorial_code/Histograms_Matching/MatchTemplate_Demo.cpp |
| 36 | +/** |
| 37 | + * @file MatchTemplate_Demo.cpp |
| 38 | + * @brief Sample code to use the function MatchTemplate |
| 39 | + * @author OpenCV team |
| 40 | + */ |
| 41 | + |
| 42 | +#include <ros/ros.h> |
| 43 | +#include "opencv_apps/nodelet.h" |
| 44 | +#include <image_transport/image_transport.h> |
| 45 | +#include <cv_bridge/cv_bridge.h> |
| 46 | +#include <sensor_msgs/image_encodings.h> |
| 47 | + |
| 48 | +#include <opencv2/highgui/highgui.hpp> |
| 49 | +#include <opencv2/imgproc/imgproc.hpp> |
| 50 | +#include <opencv2/imgproc/types_c.h> |
| 51 | + |
| 52 | +#include <dynamic_reconfigure/server.h> |
| 53 | +#include "opencv_apps/MatchTemplateConfig.h" |
| 54 | +#include "opencv_apps/RectArrayStamped.h" |
| 55 | + |
| 56 | +namespace match_template |
| 57 | +{ |
| 58 | + class MatchTemplateNodelet:public opencv_apps::Nodelet |
| 59 | + { |
| 60 | + image_transport::Publisher img_pub_; |
| 61 | + image_transport::Publisher matched_img_pub_; |
| 62 | + image_transport::Subscriber img_sub_; |
| 63 | + image_transport::CameraSubscriber cam_sub_; |
| 64 | + ros::Publisher msg_pub_; |
| 65 | + |
| 66 | + boost::shared_ptr < image_transport::ImageTransport > it_; |
| 67 | + |
| 68 | + typedef match_template::MatchTemplateConfig Config; |
| 69 | + typedef dynamic_reconfigure::Server < Config > ReconfigureServer; |
| 70 | + Config config_; |
| 71 | + boost::shared_ptr < ReconfigureServer > reconfigure_server_; |
| 72 | + |
| 73 | + bool debug_view_; |
| 74 | + int match_method_; |
| 75 | + bool use_mask_; |
| 76 | + |
| 77 | + ros::Time prev_stamp_; |
| 78 | + |
| 79 | + cv::Mat templ_; |
| 80 | + cv::Mat mask_; |
| 81 | + |
| 82 | + void reconfigureCallback (Config & new_config, uint32_t level) |
| 83 | + { |
| 84 | + config_ = new_config; |
| 85 | + } |
| 86 | + |
| 87 | + const std::string & frameWithDefault (const std::string & frame, const std::string & image_frame) |
| 88 | + { |
| 89 | + if (frame.empty ()) |
| 90 | + return image_frame; |
| 91 | + return frame; |
| 92 | + } |
| 93 | + |
| 94 | + void imageCallbackWithInfo (const sensor_msgs::ImageConstPtr & msg, |
| 95 | + const sensor_msgs::CameraInfoConstPtr & cam_info) |
| 96 | + { |
| 97 | + do_work (msg, cam_info->header.frame_id); |
| 98 | + } |
| 99 | + |
| 100 | + void imageCallback (const sensor_msgs::ImageConstPtr & msg) |
| 101 | + { |
| 102 | + do_work (msg, msg->header.frame_id); |
| 103 | + } |
| 104 | + |
| 105 | + void do_work (const sensor_msgs::ImageConstPtr & msg, const std::string input_frame_from_msg) |
| 106 | + { |
| 107 | + // Work on the image. |
| 108 | + try |
| 109 | + { |
| 110 | + // Convert the image into something opencv can handle. |
| 111 | + cv::Mat frame = cv_bridge::toCvShare (msg, sensor_msgs::image_encodings::BGR8)->image; |
| 112 | + // Messages |
| 113 | + opencv_apps::RectArrayStamped rects_msg; |
| 114 | + rects_msg.header = msg->header; |
| 115 | + |
| 116 | + /// Create the result matrix |
| 117 | + int result_cols = frame.cols - templ_.cols + 1; |
| 118 | + int result_rows = frame.rows - templ_.rows + 1; |
| 119 | + cv::Mat result (result_rows, result_cols, CV_32FC1); |
| 120 | + |
| 121 | + //-- Show template |
| 122 | + if (debug_view_) |
| 123 | + { |
| 124 | + cv::imshow ("Template", templ_); |
| 125 | + int c = cv::waitKey (1); |
| 126 | + } |
| 127 | + |
| 128 | + //! [match_template] |
| 129 | + /// Do the Matching and Normalize |
| 130 | + bool method_accepts_mask = (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_CCORR_NORMED); |
| 131 | + if (use_mask_ && method_accepts_mask) |
| 132 | + { |
| 133 | + matchTemplate (frame, templ_, result, match_method_, mask_); |
| 134 | + } |
| 135 | + else |
| 136 | + { |
| 137 | + matchTemplate (frame, templ_, result, match_method_); |
| 138 | + } |
| 139 | + //! [normalize] |
| 140 | + //normalize (result, result, 0, 255, cv::NORM_MINMAX, CV_8UC1, cv::Mat ()); |
| 141 | + normalize (result, result, 0, 255, cv::NORM_MINMAX, CV_8UC1, cv::Mat ()); |
| 142 | + |
| 143 | + //! [best_match] |
| 144 | + /// Localizing the best match with minMaxLoc |
| 145 | + double minVal; |
| 146 | + double maxVal; |
| 147 | + cv::Point minLoc; |
| 148 | + cv::Point maxLoc; |
| 149 | + cv::Point matchLoc; |
| 150 | + |
| 151 | + cv::minMaxLoc (result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat ()); |
| 152 | + //! [match_loc] |
| 153 | + /// For SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better |
| 154 | + if (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_SQDIFF_NORMED || match_method_ == CV_TM_CCORR) |
| 155 | + { |
| 156 | + matchLoc = minLoc; |
| 157 | + } |
| 158 | + else |
| 159 | + { |
| 160 | + matchLoc = maxLoc; |
| 161 | + } |
| 162 | + rectangle (frame, matchLoc, cv::Point (matchLoc.x + templ_.cols, matchLoc.y + templ_.rows), cv::Scalar::all (0), |
| 163 | + 2, 8, 0); |
| 164 | + rectangle (result, matchLoc, cv::Point (matchLoc.x + templ_.cols, matchLoc.y + templ_.rows), |
| 165 | + cv::Scalar::all (0), 2, 8, 0); |
| 166 | + |
| 167 | + // Publish the image. |
| 168 | + sensor_msgs::Image::Ptr out_img = |
| 169 | + cv_bridge::CvImage (msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg (); |
| 170 | + sensor_msgs::Image::Ptr match_img = |
| 171 | + cv_bridge::CvImage (msg->header, sensor_msgs::image_encodings::MONO8, result).toImageMsg (); |
| 172 | + img_pub_.publish (out_img); |
| 173 | + matched_img_pub_.publish (match_img); |
| 174 | + } |
| 175 | + catch (cv::Exception & e) |
| 176 | + { |
| 177 | + NODELET_ERROR ("Image processing error: %s %s %s %i", e.err.c_str (), e.func.c_str (), e.file.c_str (), e.line); |
| 178 | + } |
| 179 | + |
| 180 | + prev_stamp_ = msg->header.stamp; |
| 181 | + } |
| 182 | + |
| 183 | + void subscribe () |
| 184 | + { |
| 185 | + NODELET_INFO ("Subscribing to image topic."); |
| 186 | + if (config_.use_camera_info) |
| 187 | + cam_sub_ = it_->subscribeCamera ("image", 3, &MatchTemplateNodelet::imageCallbackWithInfo, this); |
| 188 | + else |
| 189 | + img_sub_ = it_->subscribe ("image", 3, &MatchTemplateNodelet::imageCallback, this); |
| 190 | + } |
| 191 | + |
| 192 | + void unsubscribe () |
| 193 | + { |
| 194 | + NODELET_DEBUG ("Unsubscribing from image topic."); |
| 195 | + img_sub_.shutdown (); |
| 196 | + cam_sub_.shutdown (); |
| 197 | + } |
| 198 | + |
| 199 | + public: |
| 200 | + virtual void onInit () |
| 201 | + { |
| 202 | + Nodelet::onInit (); |
| 203 | + it_ = boost::shared_ptr < image_transport::ImageTransport > (new image_transport::ImageTransport (*nh_)); |
| 204 | + |
| 205 | + pnh_->param ("debug_view", debug_view_, false); |
| 206 | + pnh_->param ("match_method", match_method_, (int) CV_TM_SQDIFF); |
| 207 | + pnh_->param ("use_mask", use_mask_, false); |
| 208 | + std::string templ_file, mask_file; |
| 209 | + pnh_->param ("template_file", templ_file, std::string ("template.png")); |
| 210 | + pnh_->param ("mask_file", mask_file, std::string ("mask.png")); |
| 211 | + |
| 212 | + NODELET_INFO ("template_file: %s", templ_file.c_str ()); |
| 213 | + |
| 214 | + if (debug_view_) |
| 215 | + { |
| 216 | + always_subscribe_ = true; |
| 217 | + } |
| 218 | + if (use_mask_) |
| 219 | + { |
| 220 | + mask_ = imread (mask_file, cv::IMREAD_COLOR); |
| 221 | + } |
| 222 | + if (templ_file.empty ()) |
| 223 | + { |
| 224 | + NODELET_ERROR ("Cannot open template file %s", templ_file.c_str ()); |
| 225 | + exit (0); |
| 226 | + } |
| 227 | + //templ_ = imread(templ_file, cv::IMREAD_COLOR); |
| 228 | + templ_ = imread (templ_file, cv::IMREAD_COLOR); |
| 229 | + |
| 230 | + if (debug_view_) |
| 231 | + { |
| 232 | + cv::imshow ("Match Template", templ_); |
| 233 | + int c = cv::waitKey (1); |
| 234 | + } |
| 235 | + prev_stamp_ = ros::Time (0, 0); |
| 236 | + |
| 237 | + reconfigure_server_ = boost::make_shared < dynamic_reconfigure::Server < Config > >(*pnh_); |
| 238 | + dynamic_reconfigure::Server < Config >::CallbackType f = |
| 239 | + boost::bind (&MatchTemplateNodelet::reconfigureCallback, this, _1, _2); |
| 240 | + reconfigure_server_->setCallback (f); |
| 241 | + |
| 242 | + img_pub_ = advertiseImage (*pnh_, "image", 1); |
| 243 | + matched_img_pub_ = advertiseImage (*pnh_, "matched_image", 1); |
| 244 | + msg_pub_ = advertise < opencv_apps::RectArrayStamped > (*pnh_, "matched_rectangle", 1); |
| 245 | + |
| 246 | + onInitPostProcess (); |
| 247 | + } |
| 248 | + }; |
| 249 | +} |
| 250 | + |
| 251 | +#include <pluginlib/class_list_macros.h> |
| 252 | +PLUGINLIB_EXPORT_CLASS (match_template::MatchTemplateNodelet, nodelet::Nodelet); |
0 commit comments