Merge pull request #1 from k-okada/add_matching_template

update matching template

Author: 7675t

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}

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"))

launch/match_template.launch

@@ -4,6 +4,8 @@
   <arg name="use_camera_info" default="false" doc="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." />
   <arg name="debug_view" default="false" doc="Specify whether the node displays a window to show edge image" />
   <arg name="match_method" default="0" doc="Matching method. 0:CV_TM_SQDIFF 1:CV_TM_SQDIFF_NORMED 2:CV_TM_CCORR 3:CV_TM_CCORR_NORMED 4:CV_TM_CCOEFF 5:CV_TM_CCOEFF_NORMED" />
+  <arg name="template_file" default="$(find opencv_apps)/template.png" doc="filename for template image"/>
+  <arg name="mask_file" default="$(find opencv_apps)/mask.png" doc="filename for mask image"/>
 
   <!-- match_template_nodelet.cpp -->
   <node name="$(arg node_name)" pkg="opencv_apps" type="match_template" output="screen">
@@ -12,7 +14,7 @@
     <param name="debug_view" value="$(arg debug_view)" />
     <param name="match_method" value="$(arg match_method)" />
     <param name="use_mask" value="false" />
-    <param name="template_file" value="$(find opencv_apps)/template.png" />
-    <param name="mask_file" value="$(find opencv_apps)/mask.png" />
+    <param name="template_file" value="$(arg template_file)" />
+    <param name="mask_file" value="$(arg mask_file)" />
   </node>
 </launch>

src/nodelet/match_template_nodelet.cpp

@@ -52,6 +52,7 @@
 #include <dynamic_reconfigure/server.h>
 #include "opencv_apps/MatchTemplateConfig.h"
 #include "opencv_apps/RectArrayStamped.h"
+#include <std_msgs/Float64.h>
 
 namespace match_template
 {
@@ -61,7 +62,8 @@ namespace match_template
     image_transport::Publisher matched_img_pub_;
     image_transport::Subscriber img_sub_;
     image_transport::CameraSubscriber cam_sub_;
-    ros::Publisher msg_pub_;
+    ros::Publisher matched_val_pub_;
+    ros::Publisher matched_rect_pub_;
 
     boost::shared_ptr < image_transport::ImageTransport > it_;
 
@@ -72,16 +74,24 @@ 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)
     {
       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)
@@ -118,32 +128,25 @@ 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);
+#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_);
         }
-        //! [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;
@@ -154,23 +157,57 @@ 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);
-        rectangle (result, matchLoc, cv::Point (matchLoc.x + templ_.cols, matchLoc.y + templ_.rows),
+        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);
 
+        //-- 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 ();
         sensor_msgs::Image::Ptr match_img =
           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);
       }
       catch (cv::Exception & e)
       {
@@ -204,34 +241,35 @@ 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 ());
 
       if (debug_view_)
       {
         always_subscribe_ = true;
       }
+#if (CV_MAJOR_VERSION >= 3 && CV_MINOR_VERSION >= 2)
       if (use_mask_)
       {
         mask_ = imread (mask_file, cv::IMREAD_COLOR);
       }
-      if (templ_file.empty ())
-      {
-        NODELET_ERROR ("Cannot open template file %s", templ_file.c_str ());
-        exit (0);
-      }
-      //templ_ = imread(templ_file, cv::IMREAD_COLOR);
+#endif
       templ_ = imread (templ_file, cv::IMREAD_COLOR);
-
-      if (debug_view_)
+      if (!templ_.data)
       {
-        cv::imshow ("Match Template", templ_);
-        int c = cv::waitKey (1);
+        NODELET_ERROR ("Cannot open template file (%s)", templ_file.c_str ());
+        exit (-1);
       }
+
       prev_stamp_ = ros::Time (0, 0);
 
       reconfigure_server_ = boost::make_shared < dynamic_reconfigure::Server < Config > >(*pnh_);
@@ -241,7 +279,8 @@ namespace match_template
 
       img_pub_ = advertiseImage (*pnh_, "image", 1);
       matched_img_pub_ = advertiseImage (*pnh_, "matched_image", 1);
-      msg_pub_ = advertise < opencv_apps::RectArrayStamped > (*pnh_, "matched_rectangle", 1);
+      matched_val_pub_ = advertise< std_msgs::Float64 > (*pnh_, "matched_value", 1);
+      matched_rect_pub_ = advertise < opencv_apps::RectArrayStamped > (*pnh_, "matched_rectangle", 1);
 
       onInitPostProcess ();
     }

test/test-match_template.test

@@ -0,0 +1,28 @@
+<launch>
+  <arg name="gui" default="true" />
+  <node name="play_face_bag" pkg="rosbag" type="play" args="-l $(find opencv_apps)/test/face_detector_withface_test_diamondback.bag" />
+
+  <group ns="wide_stereo/left" >
+    <node name="image_proc" pkg="image_proc" type="image_proc" />
+    <node name="image_view" pkg="image_view" type="image_view" args="image:=image_rect_color" if="$(arg gui)" />
+
+    <!-- convex_hull.cpp -->
+    <include file="$(find opencv_apps)/launch/match_template.launch" >
+      <arg name="debug_view" value="$(arg gui)" />
+      <arg name="image" value="image_rect" />
+      <arg name="template_file" value="$(find opencv_apps)/test/template.png" />
+      <arg name="mask_file" value="" />
+    </include>
+
+    <!-- Test Codes -->
+    <node name="match_template_saver" pkg="image_view" type="image_saver" args="image:=match_template/image" >
+      <param name="filename_format" value="$(find opencv_apps)/test/match_template.png"/>
+    </node>
+    <param name="match_template_test/topic" value="match_template/matched_rectangle" />    <!-- opencv_apps/ContorArrayStamped -->
+    <test test-name="match_template_test" pkg="rostest" type="hztest" name="match_template_test" >
+      <param name="hz" value="20" />
+      <param name="hzerror" value="15" />
+      <param name="test_duration" value="5.0" /> 
+    </test>
+  </group>
+</launch>