From 79f8ecee6cdfd12b768404ed9bded1141b72a103 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 11 Jul 2017 16:09:36 +0900 Subject: [PATCH 1/9] matchTemplate: check opencv version, matchTemplate with mask is only suppreted >= 3.2 --- src/nodelet/match_template_nodelet.cpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp index 858af0b5..e00f5050 100644 --- a/src/nodelet/match_template_nodelet.cpp +++ b/src/nodelet/match_template_nodelet.cpp @@ -72,12 +72,16 @@ namespace match_template bool debug_view_; int match_method_; +#if (CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION >= 2) // mask is only supported since opencv 3.2 (https://github.com/opencv/opencv/pull/3554) bool use_mask_; +#endif ros::Time prev_stamp_; cv::Mat templ_; +#if (CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION >= 2) cv::Mat mask_; +#endif void reconfigureCallback (Config & new_config, uint32_t level) { @@ -128,11 +132,13 @@ namespace match_template //! [match_template] /// Do the Matching and Normalize bool method_accepts_mask = (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_CCORR_NORMED); +#if (CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION >= 2) if (use_mask_ && method_accepts_mask) { matchTemplate (frame, templ_, result, match_method_, mask_); } else +#endif { matchTemplate (frame, templ_, result, match_method_); } @@ -204,10 +210,15 @@ namespace match_template pnh_->param ("debug_view", debug_view_, false); pnh_->param ("match_method", match_method_, (int) CV_TM_SQDIFF); +#if (CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION >= 2) pnh_->param ("use_mask", use_mask_, false); - std::string templ_file, mask_file; +#endif + std::string templ_file; pnh_->param ("template_file", templ_file, std::string ("template.png")); +#if (CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION >= 2) + std::string mask_file; pnh_->param ("mask_file", mask_file, std::string ("mask.png")); +#endif NODELET_INFO ("template_file: %s", templ_file.c_str ()); @@ -215,10 +226,12 @@ namespace match_template { always_subscribe_ = true; } +#if (CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION >= 2) if (use_mask_) { mask_ = imread (mask_file, cv::IMREAD_COLOR); } +#endif if (templ_file.empty ()) { NODELET_ERROR ("Cannot open template file %s", templ_file.c_str ()); From 082e094661558142248b8732ceddcdf3d0901a3a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 11 Jul 2017 16:18:14 +0900 Subject: [PATCH 2/9] CMakeLists.txt move opencv_apps_add_nodlet location --- CMakeLists.txt | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2f5115e2..e1f8e197 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -159,7 +159,7 @@ opencv_apps_add_nodelet(discrete_fourier_transform discrete_fourier_transform/di # ./tutorial_code/Histograms_Matching/calcHist_Demo.cpp # ./tutorial_code/Histograms_Matching/compareHist_Demo.cpp # ./tutorial_code/Histograms_Matching/EqualizeHist_Demo.cpp - # ./tutorial_code/Histograms_Matching/MatchTemplate_Demo.cpp +opencv_apps_add_nodelet(match_template match_template/match_template src/nodelet/match_template_nodelet.cpp) # ./tutorial_code/Histograms_Matching/MatchTemplate_Demo.cpp # imagecodecs # ./tutorial_code/imgcodecs/GDAL_IO/gdal-image.cpp @@ -331,8 +331,6 @@ endif() # https://github.com/opencv/opencv/blob/2.4/samples/cpp/video_homography.cpp # https://github.com/opencv/opencv/blob/2.4/samples/cpp/videocapture_pvapi.cpp -opencv_apps_add_nodelet(match_template match_template/match_template src/nodelet/match_template_nodelet.cpp) - add_library(${PROJECT_NAME} SHARED src/nodelet/nodelet.cpp ${_opencv_apps_nodelet_cppfiles} From f38331d01eba265af82bdeeab16b6ea21245a21d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 11 Jul 2017 19:42:28 +0900 Subject: [PATCH 3/9] match_template_nodelet.cpp, show error when iread fails --- src/nodelet/match_template_nodelet.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp index e00f5050..c24f7298 100644 --- a/src/nodelet/match_template_nodelet.cpp +++ b/src/nodelet/match_template_nodelet.cpp @@ -232,13 +232,12 @@ namespace match_template mask_ = imread (mask_file, cv::IMREAD_COLOR); } #endif - if (templ_file.empty ()) + templ_ = imread (templ_file, cv::IMREAD_COLOR); + if (!templ_.data) { - NODELET_ERROR ("Cannot open template file %s", templ_file.c_str ()); - exit (0); + NODELET_ERROR ("Cannot open template file (%s)", templ_file.c_str ()); + exit (-1); } - //templ_ = imread(templ_file, cv::IMREAD_COLOR); - templ_ = imread (templ_file, cv::IMREAD_COLOR); if (debug_view_) { From e45deba0059ab13fddc4ef529c1dabcf7527904f Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 11 Jul 2017 19:43:30 +0900 Subject: [PATCH 4/9] show template and result --- src/nodelet/match_template_nodelet.cpp | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp index c24f7298..43f70b10 100644 --- a/src/nodelet/match_template_nodelet.cpp +++ b/src/nodelet/match_template_nodelet.cpp @@ -122,13 +122,6 @@ namespace match_template int result_rows = frame.rows - templ_.rows + 1; cv::Mat result (result_rows, result_cols, CV_32FC1); - //-- Show template - if (debug_view_) - { - cv::imshow ("Template", templ_); - int c = cv::waitKey (1); - } - //! [match_template] /// Do the Matching and Normalize bool method_accepts_mask = (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_CCORR_NORMED); @@ -170,6 +163,15 @@ namespace match_template rectangle (result, matchLoc, cv::Point (matchLoc.x + templ_.cols, matchLoc.y + templ_.rows), cv::Scalar::all (0), 2, 8, 0); + //-- Show template and result + if (debug_view_) + { + cv::imshow ("Template", templ_); + cv::imshow ("Soure Image", frame); + cv::imshow ("Result window", result); + int c = cv::waitKey (1); + } + // Publish the image. sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage (msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg (); @@ -239,11 +241,6 @@ namespace match_template exit (-1); } - if (debug_view_) - { - cv::imshow ("Match Template", templ_); - int c = cv::waitKey (1); - } prev_stamp_ = ros::Time (0, 0); reconfigure_server_ = boost::make_shared < dynamic_reconfigure::Server < Config > >(*pnh_); From 8205cad9a724b821c4002cd827c51ce4495a2b8a Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 11 Jul 2017 19:47:45 +0900 Subject: [PATCH 5/9] match_template_nodelet.cpp: fix show rectanlge on result image --- src/nodelet/match_template_nodelet.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp index 43f70b10..69a5e742 100644 --- a/src/nodelet/match_template_nodelet.cpp +++ b/src/nodelet/match_template_nodelet.cpp @@ -160,7 +160,7 @@ namespace match_template } rectangle (frame, matchLoc, cv::Point (matchLoc.x + templ_.cols, matchLoc.y + templ_.rows), cv::Scalar::all (0), 2, 8, 0); - rectangle (result, matchLoc, cv::Point (matchLoc.x + templ_.cols, matchLoc.y + templ_.rows), + rectangle (result, cv::Point (matchLoc.x - templ_.cols/2, matchLoc.y - templ_.rows/2), cv::Point (matchLoc.x + templ_.cols/2, matchLoc.y + templ_.rows/2), cv::Scalar::all (0), 2, 8, 0); //-- Show template and result From 35bcd7760fa0c23c00a47ab916979bf8a1a04fca Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 11 Jul 2017 20:24:52 +0900 Subject: [PATCH 6/9] match_template_nodelet.cpp: publish and display matched value --- src/nodelet/match_template_nodelet.cpp | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp index 69a5e742..0855b50b 100644 --- a/src/nodelet/match_template_nodelet.cpp +++ b/src/nodelet/match_template_nodelet.cpp @@ -52,6 +52,7 @@ #include #include "opencv_apps/MatchTemplateConfig.h" #include "opencv_apps/RectArrayStamped.h" +#include namespace match_template { @@ -61,6 +62,7 @@ namespace match_template image_transport::Publisher matched_img_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; + ros::Publisher matched_val_pub_; ros::Publisher msg_pub_; boost::shared_ptr < image_transport::ImageTransport > it_; @@ -135,14 +137,12 @@ namespace match_template { matchTemplate (frame, templ_, result, match_method_); } - //! [normalize] - //normalize (result, result, 0, 255, cv::NORM_MINMAX, CV_8UC1, cv::Mat ()); - normalize (result, result, 0, 255, cv::NORM_MINMAX, CV_8UC1, cv::Mat ()); //! [best_match] /// Localizing the best match with minMaxLoc double minVal; double maxVal; + double matchVal; cv::Point minLoc; cv::Point maxLoc; cv::Point matchLoc; @@ -153,13 +153,23 @@ namespace match_template if (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_SQDIFF_NORMED || match_method_ == CV_TM_CCORR) { matchLoc = minLoc; + matchVal = minVal; } else { matchLoc = maxLoc; + matchVal = maxVal; } + NODELET_DEBUG_STREAM(std::fixed << std::setw(12) << minVal << " " << maxVal << " min " << matchVal); + + //! [normalize] for visualization + normalize (result, result, 0, 255, cv::NORM_MINMAX, CV_8UC1, cv::Mat ()); + rectangle (frame, matchLoc, cv::Point (matchLoc.x + templ_.cols, matchLoc.y + templ_.rows), cv::Scalar::all (0), 2, 8, 0); + std::stringstream ss; + ss << std::fixed << std::setw(12) << std::setprecision(0) << matchVal; + cv::putText(frame, ss.str(), cv::Point(0,20), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(255.0, 255.0, 255.0), 1, CV_AA); rectangle (result, cv::Point (matchLoc.x - templ_.cols/2, matchLoc.y - templ_.rows/2), cv::Point (matchLoc.x + templ_.cols/2, matchLoc.y + templ_.rows/2), cv::Scalar::all (0), 2, 8, 0); @@ -179,6 +189,9 @@ namespace match_template cv_bridge::CvImage (msg->header, sensor_msgs::image_encodings::MONO8, result).toImageMsg (); img_pub_.publish (out_img); matched_img_pub_.publish (match_img); + std_msgs::Float64 matched_val_msg; + matched_val_msg.data = matchVal; + matched_val_pub_.publish (matched_val_msg); } catch (cv::Exception & e) { @@ -250,6 +263,7 @@ namespace match_template img_pub_ = advertiseImage (*pnh_, "image", 1); matched_img_pub_ = advertiseImage (*pnh_, "matched_image", 1); + matched_val_pub_ = advertise< std_msgs::Float64 > (*pnh_, "matched_value", 1); msg_pub_ = advertise < opencv_apps::RectArrayStamped > (*pnh_, "matched_rectangle", 1); onInitPostProcess (); From 3624f77491636e1e66dec4781b46b168e867d717 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 11 Jul 2017 20:35:42 +0900 Subject: [PATCH 7/9] match_template_nodelet.cpp: publish result rectangle --- src/nodelet/match_template_nodelet.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp index 0855b50b..a1ebb33e 100644 --- a/src/nodelet/match_template_nodelet.cpp +++ b/src/nodelet/match_template_nodelet.cpp @@ -63,7 +63,7 @@ namespace match_template image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; ros::Publisher matched_val_pub_; - ros::Publisher msg_pub_; + ros::Publisher matched_rect_pub_; boost::shared_ptr < image_transport::ImageTransport > it_; @@ -189,6 +189,18 @@ namespace match_template cv_bridge::CvImage (msg->header, sensor_msgs::image_encodings::MONO8, result).toImageMsg (); img_pub_.publish (out_img); matched_img_pub_.publish (match_img); + + // Publish the result. + opencv_apps::RectArrayStamped matched_rect_msg; + matched_rect_msg.header = msg->header; + opencv_apps::Rect rect_msg; + rect_msg.x = matchLoc.x; + rect_msg.y = matchLoc.y; + rect_msg.width = templ_.cols; + rect_msg.height = templ_.rows; + matched_rect_msg.rects.push_back(rect_msg); + matched_rect_pub_.publish (matched_rect_msg); + std_msgs::Float64 matched_val_msg; matched_val_msg.data = matchVal; matched_val_pub_.publish (matched_val_msg); @@ -264,7 +276,7 @@ namespace match_template img_pub_ = advertiseImage (*pnh_, "image", 1); matched_img_pub_ = advertiseImage (*pnh_, "matched_image", 1); matched_val_pub_ = advertise< std_msgs::Float64 > (*pnh_, "matched_value", 1); - msg_pub_ = advertise < opencv_apps::RectArrayStamped > (*pnh_, "matched_rectangle", 1); + matched_rect_pub_ = advertise < opencv_apps::RectArrayStamped > (*pnh_, "matched_rectangle", 1); onInitPostProcess (); } From 8682b4287c2a17351332befdd614a6a9be65fc67 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 11 Jul 2017 20:54:04 +0900 Subject: [PATCH 8/9] match_template_nodelet.cpp: enable to select mathing method from rqt_reconfigure --- cfg/MatchTemplate.cfg | 8 ++++++++ src/nodelet/match_template_nodelet.cpp | 4 ++++ 2 files changed, 12 insertions(+) diff --git a/cfg/MatchTemplate.cfg b/cfg/MatchTemplate.cfg index 37c7d565..17c95794 100755 --- a/cfg/MatchTemplate.cfg +++ b/cfg/MatchTemplate.cfg @@ -38,5 +38,13 @@ from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("use_camera_info", bool_t, 0, "Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used.", False) +match_type = gen.enum([gen.const("CV_TM_SQDIFF", int_t, 0, "Sum of Ssquared Difference"), + gen.const("CV_TM_SQDIFF_NORMED", int_t, 1, "Sum of Normalized Ssquared Difference"), + gen.const("CV_TM_CCORR", int_t, 2, "Correlation"), + gen.const("CV_TM_CCORR_NORMED", int_t, 3, "Normalized Correlation"), + gen.const("CV_TM_CCOEFF", int_t, 4, "Correlation of Coefficient"), + gen.const("CV_TM_CCOEFF_NORMED", int_t, 5, "Normalized Correlation of Coefficient"), + ], "An enum for Mathing Mehtods") +gen.add("match_method", int_t, 0, "Matching Methods", 0, 0, 5, edit_method=match_type) exit(gen.generate(PACKAGE, "match_template", "MatchTemplate")) diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp index a1ebb33e..fa345ce0 100644 --- a/src/nodelet/match_template_nodelet.cpp +++ b/src/nodelet/match_template_nodelet.cpp @@ -88,6 +88,10 @@ namespace match_template void reconfigureCallback (Config & new_config, uint32_t level) { config_ = new_config; + if ( match_method_ != config_.match_method ) { + match_method_ = config_.match_method; + NODELET_WARN_STREAM("Change Mathing Method to " << match_method_); + } } const std::string & frameWithDefault (const std::string & frame, const std::string & image_frame) From 905b3db3ca4cb2274b0eb1759bbe4288107dcbc5 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 11 Jul 2017 20:57:17 +0900 Subject: [PATCH 9/9] add test/test-match-template.test --- launch/match_template.launch | 6 ++++-- test/test-match_template.test | 28 ++++++++++++++++++++++++++++ 2 files changed, 32 insertions(+), 2 deletions(-) create mode 100644 test/test-match_template.test diff --git a/launch/match_template.launch b/launch/match_template.launch index de1aa44b..6eb71532 100644 --- a/launch/match_template.launch +++ b/launch/match_template.launch @@ -4,6 +4,8 @@ + + @@ -12,7 +14,7 @@ - - + + diff --git a/test/test-match_template.test b/test/test-match_template.test new file mode 100644 index 00000000..921f0b74 --- /dev/null +++ b/test/test-match_template.test @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + +