diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2f5115e2..de6cbf74 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,7 +1,7 @@
cmake_minimum_required(VERSION 2.8.3)
project(opencv_apps)
-find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure message_generation image_transport nodelet roscpp sensor_msgs)
+find_package(catkin REQUIRED COMPONENTS cv_bridge dynamic_reconfigure message_generation image_transport image_geometry nodelet roscpp sensor_msgs)
find_package(OpenCV REQUIRED)
message(STATUS "OpenCV VERSION: ${OpenCV_VERSION}")
diff --git a/cfg/MatchTemplate.cfg b/cfg/MatchTemplate.cfg
index 8eec3478..4dc2e861 100755
--- a/cfg/MatchTemplate.cfg
+++ b/cfg/MatchTemplate.cfg
@@ -48,5 +48,6 @@ method_enum = gen.enum([ gen.const("TM_SQDIFF", int_t, 0, "TM_SQDIFF"),
gen.add("match_method", int_t, 0, "Template matching method", 5, 0, 5, edit_method=method_enum)
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)
+gen.add("match_threshold", double_t, 0, "Template matching threshold", 0.0, 0.0, 1.0)
exit(gen.generate(PACKAGE, "match_template", "MatchTemplate"))
diff --git a/launch/match_template.launch b/launch/match_template.launch
index de1aa44b..a68ee82e 100644
--- a/launch/match_template.launch
+++ b/launch/match_template.launch
@@ -2,8 +2,9 @@
-
-
+
+
+
@@ -12,7 +13,7 @@
-
-
+
+
diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp
index 38d8044a..a5c6e866 100644
--- a/src/nodelet/match_template_nodelet.cpp
+++ b/src/nodelet/match_template_nodelet.cpp
@@ -1,297 +1,329 @@
/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2017, Ryosuke Tajima.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Ryosuke Tajima nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2017, Ryosuke Tajima.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of the Ryosuke Tajima nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
// https://github.com/opencv/opencv/raw/master/samples/cpp/tutorial_code/Histograms_Matching/MatchTemplate_Demo.cpp
-/**
- * @file MatchTemplate_Demo.cpp
- * @brief Sample code to use the function MatchTemplate
- * @author OpenCV team
- */
-
-#include
-#include "opencv_apps/nodelet.h"
-#include
-#include
-#include
-
#include
#include
#include
-#include
+#include
+#include
+#include
+#include
+#include "dynamic_reconfigure/server.h"
+#include "opencv_apps/nodelet.h"
#include "opencv_apps/MatchTemplateConfig.h"
#include "opencv_apps/RectArrayStamped.h"
+#include "sensor_msgs/image_encodings.h"
#include "geometry_msgs/PointStamped.h"
+#include "geometry_msgs/Vector3Stamped.h"
namespace match_template
{
- class MatchTemplateNodelet:public opencv_apps::Nodelet
- {
- image_transport::Publisher img_pub_;
- image_transport::Publisher matched_img_pub_;
- image_transport::Subscriber img_sub_;
- image_transport::CameraSubscriber cam_sub_;
- image_transport::Subscriber template_sub_;
- ros::Publisher msg_pub_;
-
- boost::shared_ptr < image_transport::ImageTransport > it_;
-
- typedef match_template::MatchTemplateConfig Config;
- typedef dynamic_reconfigure::Server < Config > ReconfigureServer;
- Config config_;
- boost::shared_ptr < ReconfigureServer > reconfigure_server_;
+class MatchTemplateNodelet:public opencv_apps::Nodelet
+{
+ image_transport::Subscriber scene_img_sub_, templ_img_sub_;
+ image_transport::Publisher scene_img_pub_, templ_img_pub_, score_img_pub_;
+ image_transport::CameraSubscriber scene_cam_sub_;
+ ros::Publisher msg_pub_, gaze_vector_pub_;
- bool debug_view_;
- // int match_method_;
- bool use_mask_;
+ boost::shared_ptr < image_transport::ImageTransport > it_;
- ros::Time prev_stamp_;
+ typedef match_template::MatchTemplateConfig Config;
+ typedef dynamic_reconfigure::Server < Config > ReconfigureServer;
+ Config config_;
+ boost::shared_ptr < ReconfigureServer > reconfigure_server_;
- cv::Mat templ_;
- cv::Mat mask_;
+ image_geometry::PinholeCameraModel model_;
- void reconfigureCallback (Config & new_config, uint32_t level)
- {
- config_ = new_config;
- }
+ bool debug_view_;
+ ros::Time prev_stamp_;
+ cv::Mat templ_;
- const std::string & frameWithDefault (const std::string & frame, const std::string & image_frame)
- {
- if (frame.empty ())
- return image_frame;
- return frame;
- }
+ std::string scene_window_name_, templ_window_name_;
+ static bool mouse_update_;
+ static int mouse_event_;
+ static int mouse_x_;
+ static int mouse_y_;
+ static bool mouse_selecting_;
+ cv::Rect mouse_rect_;
- void imageCallbackWithInfo (const sensor_msgs::ImageConstPtr & msg,
- const sensor_msgs::CameraInfoConstPtr & cam_info)
- {
- do_work (msg, cam_info->header.frame_id);
- }
+ void reconfigureCallback (Config & new_config, uint32_t level)
+ {
+ config_ = new_config;
+ }
+ void imageCallbackWithInfo (const sensor_msgs::ImageConstPtr & msg,
+ const sensor_msgs::CameraInfoConstPtr & cam_info)
+ {
+ model_.fromCameraInfo(cam_info);
+ do_work (msg, cam_info->header.frame_id);
+ }
- void imageCallback (const sensor_msgs::ImageConstPtr & msg)
- {
- do_work (msg, msg->header.frame_id);
- }
+ void imageCallback (const sensor_msgs::ImageConstPtr & msg)
+ {
+ do_work (msg, msg->header.frame_id);
+ }
- void templateCallback (const sensor_msgs::ImageConstPtr & msg)
- {
- templ_ = cv_bridge::toCvCopy (msg, sensor_msgs::image_encodings::BGR8)->image;
- }
-
- void do_work (const sensor_msgs::ImageConstPtr & msg, const std::string input_frame_from_msg)
+ void templateCallback (const sensor_msgs::ImageConstPtr & msg)
+ {
+ templ_ = cv_bridge::toCvCopy (msg, sensor_msgs::image_encodings::BGR8)->image;
+ }
+
+ static void onMouse( int event, int x, int y, int, void* )
+ {
+ mouse_update_ = true;
+ mouse_event_ = event;
+ mouse_x_ = x;
+ mouse_y_ = y;
+ }
+
+ void do_work (const sensor_msgs::ImageConstPtr & msg, const std::string input_frame_from_msg)
+ {
+ // Work on the image.
+ try
{
- // Work on the image.
- try
+ // Convert the image into something opencv can handle.
+ cv::Mat frame = cv_bridge::toCvShare (msg, sensor_msgs::image_encodings::BGR8)->image;
+ // Messages
+ opencv_apps::RectArrayStamped rects_msg;
+ rects_msg.header = msg->header;
+
+ /// Create the score matrix
+ int score_cols = frame.cols - templ_.cols + 1;
+ int score_rows = frame.rows - templ_.rows + 1;
+ cv::Mat score (score_rows, score_cols, CV_32FC1);
+
+ /// Debug View
+ if (debug_view_)
+ {
+ cv::namedWindow( scene_window_name_, cv::WINDOW_AUTOSIZE );
+ cv::namedWindow( templ_window_name_, cv::WINDOW_AUTOSIZE );
+ cv::setMouseCallback( scene_window_name_, onMouse, 0 );
+ //cv::imshow (scene_window_name_, frame);
+ cv::imshow (templ_window_name_, templ_);
+ int c = cv::waitKey (1);
+ }
+ if (mouse_update_ )
{
- // Convert the image into something opencv can handle.
- cv::Mat frame = cv_bridge::toCvShare (msg, sensor_msgs::image_encodings::BGR8)->image;
- // Messages
- opencv_apps::RectArrayStamped rects_msg;
- rects_msg.header = msg->header;
-
- /// Create the result matrix
- int result_cols = frame.cols - templ_.cols + 1;
- int result_rows = frame.rows - templ_.rows + 1;
- cv::Mat result (result_rows, result_cols, CV_32FC1);
-
- //-- Show template
- if (debug_view_)
+ if( mouse_selecting_ )
{
- cv::imshow ("Image", frame);
- cv::imshow ("Template", templ_);
- int c = cv::waitKey (1);
+ mouse_rect_.x = MIN(mouse_x_, mouse_rect_.x);
+ mouse_rect_.y = MIN(mouse_y_, mouse_rect_.y);
+ mouse_rect_.width = std::abs(mouse_x_ - mouse_rect_.x);
+ mouse_rect_.height = std::abs(mouse_y_ - mouse_rect_.y);
+ mouse_rect_ &= cv::Rect(0, 0, frame.cols, frame.rows);
}
-
- //! [match_template]
- /// Do the Matching and Normalize
- //bool method_accepts_mask = (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_CCORR_NORMED);
- bool method_accepts_mask = (config_.match_method == CV_TM_SQDIFF || config_.match_method == CV_TM_CCORR_NORMED);
- if (use_mask_ && method_accepts_mask)
+ switch( mouse_event_ )
{
- matchTemplate (frame, templ_, result, config_.match_method, mask_);
+ case cv::EVENT_LBUTTONDOWN:
+ mouse_rect_ = cv::Rect(mouse_x_, mouse_y_, 0, 0);
+ mouse_selecting_ = true;
+ break;
+ case cv::EVENT_LBUTTONUP:
+ mouse_selecting_ = false;
+ if( mouse_rect_.width > 0 && mouse_rect_.height > 0 )
+ {
+ // publish template image
+ sensor_msgs::Image::Ptr templ_img_msg = cv_bridge::CvImage (msg->header, sensor_msgs::image_encodings::BGR8, frame(mouse_rect_)).toImageMsg ();
+ templ_img_pub_.publish (templ_img_msg);
+ //templ_ = frame(mouse_rect_).clone();
+ }
+ break;
}
- else
+ mouse_update_ = false;
+ }
+ // Match template
+ matchTemplate (frame, templ_, score, config_.match_method);
+ /// Localizing the best match with minMaxLoc
+ int remove_margin_x = templ_.cols / 2;
+ int remove_margin_y = templ_.rows / 2;
+
+ bool template_found = true;
+ double minVal, maxVal;
+ cv::Point minLoc, maxLoc, matchLoc;
+
+ // Find top score position from image
+ cv::minMaxLoc (score, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat());
+ /// In some methods, lower score is better.
+ if (config_.match_method == CV_TM_SQDIFF || config_.match_method == CV_TM_SQDIFF_NORMED || config_.match_method == CV_TM_CCORR)
+ {
+ matchLoc = minLoc;
+ // Check threshold value
+ if (minVal > config_.match_threshold)
{
- matchTemplate (frame, templ_, result, config_.match_method);
+ template_found = false;
}
- //! [normalize]
- normalize (result, result, 0, 255, cv::NORM_MINMAX, CV_8UC1, cv::Mat ());
-
- //! [best_match]
- /// Localizing the best match with minMaxLoc
- double minVal;
- double maxVal;
- cv::Point minLoc;
- cv::Point maxLoc;
- cv::Point matchLoc;
- bool is_top = true;
- int remove_margin_x = templ_.cols / 2;
- int remove_margin_y = templ_.rows / 2;
-
- // find top 10 position from image
- for (int i=0; i<3; i++) {
- cv::minMaxLoc (result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat ());
- NODELET_INFO ("min_val, max_val = %f %f", double(minVal), double(maxVal));
- //! [match_loc]
- /// For SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better
- if (config_.match_method == CV_TM_SQDIFF || config_.match_method == CV_TM_SQDIFF_NORMED || config_.match_method == CV_TM_CCORR)
- {
- matchLoc = minLoc;
- // remove matched region with some margin
- rectangle (result,
- cv::Point(matchLoc.x - remove_margin_x,
- matchLoc.y - remove_margin_y),
- cv::Point(matchLoc.x + remove_margin_x,
- matchLoc.y + remove_margin_y),
- 255, -1, 8, 0);
- }
- else
- {
- matchLoc = maxLoc;
- // remove matched region
- rectangle (result,
- cv::Point(matchLoc.x - remove_margin_x,
- matchLoc.y - remove_margin_y),
- cv::Point(matchLoc.x + remove_margin_x,
- matchLoc.y + remove_margin_y),
- 0, -1, 8, 0);
- }
- if (is_top)
- {
- rectangle (frame, matchLoc, cv::Point (matchLoc.x + templ_.cols,
- matchLoc.y + templ_.rows),
- cv::Scalar(0, 0, 255), 4, 8, 0);
- is_top = false;
- }
- else
- {
- rectangle (frame, matchLoc, cv::Point (matchLoc.x + templ_.cols,
- matchLoc.y + templ_.rows),
- cv::Scalar(0, 255, 0), 2, 8, 0);
- }
+ }
+ else
+ {
+ matchLoc = maxLoc;
+ // check threshold value
+ if (maxVal < config_.match_threshold)
+ {
+ template_found = false;
}
+ }
+ if (template_found)
+ {
+ rectangle (frame, matchLoc,
+ cv::Point (matchLoc.x + templ_.cols,
+ matchLoc.y + templ_.rows), cv::Scalar (0, 0, 255), 4, 8, 0);
+ }
+ // Publish the rectangle drawn image
+ sensor_msgs::Image::Ptr match_img =
+ cv_bridge::CvImage (msg->header, sensor_msgs::image_encodings::BGR8, frame).toImageMsg ();
+ scene_img_pub_.publish (match_img);
- // 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 point on the image
- geometry_msgs::PointStamped point;
- point.header.stamp = ros::Time::now ();
- point.header.frame_id = input_frame_from_msg;
- point.point.x = matchLoc.x + templ_.cols / 2;
- point.point.y = matchLoc.y + templ_.rows / 2;
- point.point.z = 0;
- msg_pub_.publish (point);
+ // Publish the monochrome score image
+ normalize (score, score, 0, 255, cv::NORM_MINMAX, CV_8UC1, cv::Mat ());
+ sensor_msgs::Image::Ptr score_img =
+ cv_bridge::CvImage (msg->header, sensor_msgs::image_encodings::MONO8, score).toImageMsg ();
+ score_img_pub_.publish (score_img);
- }
- catch (cv::Exception & e)
+ if (debug_view_)
{
- NODELET_ERROR ("Image processing error: %s %s %s %i", e.err.c_str (), e.func.c_str (), e.file.c_str (), e.line);
+ if (mouse_selecting_)
+ {
+ cv::Mat roi(frame, mouse_rect_);
+ bitwise_not(roi, roi);
+ }
+ cv::imshow (scene_window_name_, frame);
}
- prev_stamp_ = msg->header.stamp;
- }
+ // Publish the point on the image
+ geometry_msgs::PointStamped point;
+ point.header.stamp = ros::Time::now ();
+ point.header.frame_id = input_frame_from_msg;
+ point.point.x = matchLoc.x + templ_.cols / 2;
+ point.point.y = matchLoc.y + templ_.rows / 2;
+ point.point.z = 0;
- void subscribe ()
- {
- NODELET_INFO ("Subscribing to image topic.");
+ msg_pub_.publish (point);
+
+ // Publish the gaze vector
if (config_.use_camera_info)
- cam_sub_ = it_->subscribeCamera ("image", 3, &MatchTemplateNodelet::imageCallbackWithInfo, this);
- else
- img_sub_ = it_->subscribe ("image", 3, &MatchTemplateNodelet::imageCallback, this);
+ {
+ cv::Point center;
+ center.x = matchLoc.x + templ_.cols / 2;
+ center.y = matchLoc.y + templ_.rows / 2;
+ cv::Point3d ray = model_.projectPixelTo3dRay (center);
+ geometry_msgs::Vector3Stamped gaze_vector;
+ gaze_vector.header.stamp = ros::Time::now ();
+ gaze_vector.header.frame_id = input_frame_from_msg;
+ gaze_vector.vector.x = ray.x;
+ gaze_vector.vector.y = ray.y;
+ gaze_vector.vector.z = ray.z;
+ gaze_vector_pub_.publish (gaze_vector);
+ }
- template_sub_ = it_->subscribe ("template", 3, &MatchTemplateNodelet::templateCallback, this);
-
}
-
- void unsubscribe ()
+ catch (cv::Exception & e)
{
- NODELET_DEBUG ("Unsubscribing from image topic.");
- img_sub_.shutdown ();
- cam_sub_.shutdown ();
- template_sub_.shutdown ();
+ NODELET_ERROR ("Image processing error: %s %s %s %i", e.err.c_str (), e.func.c_str (), e.file.c_str (), e.line);
}
- public:
- virtual void onInit ()
+ prev_stamp_ = msg->header.stamp;
+ }
+
+ void subscribe ()
+ {
+ if (config_.use_camera_info)
{
- Nodelet::onInit ();
- it_ = boost::shared_ptr < image_transport::ImageTransport > (new image_transport::ImageTransport (*nh_));
+ scene_cam_sub_ = it_->subscribeCamera ("image", 3, &MatchTemplateNodelet::imageCallbackWithInfo, this);
+ }
+ else
+ {
+ scene_img_sub_ = it_->subscribe ("image", 3, &MatchTemplateNodelet::imageCallback, this);
+ }
+ templ_img_sub_ = it_->subscribe ("template", 3, &MatchTemplateNodelet::templateCallback, this);
+ }
- pnh_->param ("debug_view", debug_view_, false);
- // pnh_->param ("match_method", match_method_, (int) CV_TM_SQDIFF);
- pnh_->param ("use_mask", use_mask_, false);
- std::string templ_file, mask_file;
- pnh_->param ("template_file", templ_file, std::string ("template.png"));
- pnh_->param ("mask_file", mask_file, std::string ("mask.png"));
+ void unsubscribe ()
+ {
+ NODELET_DEBUG ("Unsubscribing from image topic.");
+ scene_img_sub_.shutdown ();
+ scene_cam_sub_.shutdown ();
+ templ_img_sub_.shutdown ();
+ }
+
+public:
+ virtual void onInit ()
+ {
+ Nodelet::onInit ();
+ it_ = boost::shared_ptr < image_transport::ImageTransport > (new image_transport::ImageTransport (*nh_));
- NODELET_INFO ("template_file: %s", templ_file.c_str ());
+ pnh_->param ("debug_view", debug_view_, true);
- if (debug_view_)
- {
- always_subscribe_ = true;
- }
- 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_ = cv::Mat(1,1, CV_8UC3);
+ std::string templ_file;
+ pnh_->param ("template_file", templ_file, std::string (""));
+ if (!templ_file.empty())
+ {
+ NODELET_INFO ("template_file: %s\n", templ_file.c_str());
templ_ = imread (templ_file, cv::IMREAD_COLOR);
+ }
- prev_stamp_ = ros::Time (0, 0);
-
- reconfigure_server_ = boost::make_shared < dynamic_reconfigure::Server < Config > >(*pnh_);
- dynamic_reconfigure::Server < Config >::CallbackType f =
- boost::bind (&MatchTemplateNodelet::reconfigureCallback, this, _1, _2);
- reconfigure_server_->setCallback (f);
-
- img_pub_ = advertiseImage (*pnh_, "image", 1);
- matched_img_pub_ = advertiseImage (*pnh_, "matched_image", 1);
- msg_pub_ = advertise < geometry_msgs::PointStamped > (*pnh_, "pixel_position", 1);
-
- onInitPostProcess ();
+ if (debug_view_)
+ {
+ always_subscribe_ = true;
}
- };
+
+ scene_window_name_ = "Scene Image";
+ templ_window_name_ = "Template Image";
+
+ prev_stamp_ = ros::Time (0, 0);
+
+ reconfigure_server_ = boost::make_shared < dynamic_reconfigure::Server < Config > >(*pnh_);
+ dynamic_reconfigure::Server < Config >::CallbackType f =
+ boost::bind (&MatchTemplateNodelet::reconfigureCallback, this, _1, _2);
+ reconfigure_server_->setCallback (f);
+
+ scene_img_pub_ = advertiseImage (*pnh_, "image", 1);
+ templ_img_pub_ = advertiseImage (*nh_, "template", 1);
+ score_img_pub_ = advertiseImage (*pnh_, "score_image", 1);
+ msg_pub_ = advertise < geometry_msgs::PointStamped > (*pnh_, "pixel_position", 1);
+ gaze_vector_pub_ = advertise < geometry_msgs::Vector3Stamped > (*pnh_, "gaze_vector", 1);
+
+ onInitPostProcess ();
+ }
+};
+// bool MatchTemplateNodelet::need_config_update_ = false;
+bool MatchTemplateNodelet::mouse_update_ = false;
+int MatchTemplateNodelet::mouse_event_ = 0;
+int MatchTemplateNodelet::mouse_x_ = 0;
+int MatchTemplateNodelet::mouse_y_ = 0;
+bool MatchTemplateNodelet::mouse_selecting_ = false;
}
#include
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 8d0e7b23..8f5b9866 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -69,6 +69,8 @@ add_rostest(test-watershed_segmentation.test ARGS gui:=false)
add_rostest(test-simple_example.test ARGS gui:=false)
add_rostest(test-simple_compressed_example.test ARGS gui:=false)
+add_rostest(test-match_template.test ARGS gui:=false)
+
if(roslaunch_VERSION VERSION_LESS "1.11.1")
message(WARNING "roslaunch_add_file check fails with unsupported doc attributes ${roslaunch_VERSION}")
else()
diff --git a/test/template.png b/test/template.png
new file mode 100644
index 00000000..f7d22cac
Binary files /dev/null and b/test/template.png differ
diff --git a/test/test-match_template.test b/test/test-match_template.test
new file mode 100644
index 00000000..252aa069
--- /dev/null
+++ b/test/test-match_template.test
@@ -0,0 +1,45 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+