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}
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/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/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp
index 858af0b5..fa345ce0 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,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,16 +157,35 @@ 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 ();
@@ -171,6 +193,21 @@ 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);
}
catch (cv::Exception & e)
{
@@ -204,10 +241,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,23 +257,19 @@ 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);
}
- 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 ();
}
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 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+