From b0e571a70da9b26a48ac4c2ca32b364515f97594 Mon Sep 17 00:00:00 2001 From: Ryosuke Tajima Date: Tue, 27 Jun 2017 17:36:51 +0900 Subject: [PATCH 01/15] Add matchTemplate nodelet --- CMakeLists.txt | 3 + cfg/MatchTemplate.cfg | 42 +++++ launch/match_template.launch | 18 ++ nodelet_plugins.xml | 4 + src/nodelet/match_template_nodelet.cpp | 252 +++++++++++++++++++++++++ 5 files changed, 319 insertions(+) create mode 100755 cfg/MatchTemplate.cfg create mode 100644 launch/match_template.launch create mode 100644 src/nodelet/match_template_nodelet.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fb39261f..2f5115e2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,7 @@ generate_dynamic_reconfigure_options( cfg/CamShift.cfg cfg/FBackFlow.cfg cfg/LKFlow.cfg + cfg/MatchTemplate.cfg cfg/PeopleDetect.cfg cfg/PhaseCorr.cfg cfg/SegmentObjects.cfg @@ -330,6 +331,8 @@ 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 new file mode 100755 index 00000000..37c7d565 --- /dev/null +++ b/cfg/MatchTemplate.cfg @@ -0,0 +1,42 @@ +#! /usr/bin/env python +# 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 Kei Okada 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. + + +PACKAGE='match_template' + +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) + +exit(gen.generate(PACKAGE, "match_template", "MatchTemplate")) diff --git a/launch/match_template.launch b/launch/match_template.launch new file mode 100644 index 00000000..de1aa44b --- /dev/null +++ b/launch/match_template.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 1ab76dac..3bd95d94 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -67,6 +67,10 @@ Nodelet to calculate Lukas-Kanade optical flow + + Nodelet to find the template on the image + + Nodelet to demonstrate the use of the HoG descriptor diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp new file mode 100644 index 00000000..858af0b5 --- /dev/null +++ b/src/nodelet/match_template_nodelet.cpp @@ -0,0 +1,252 @@ +/********************************************************************* +* 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 "opencv_apps/MatchTemplateConfig.h" +#include "opencv_apps/RectArrayStamped.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_; + 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_; + + bool debug_view_; + int match_method_; + bool use_mask_; + + ros::Time prev_stamp_; + + cv::Mat templ_; + cv::Mat mask_; + + void reconfigureCallback (Config & new_config, uint32_t level) + { + config_ = new_config; + } + + const std::string & frameWithDefault (const std::string & frame, const std::string & image_frame) + { + if (frame.empty ()) + return image_frame; + return frame; + } + + void imageCallbackWithInfo (const sensor_msgs::ImageConstPtr & msg, + const sensor_msgs::CameraInfoConstPtr & 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 do_work (const sensor_msgs::ImageConstPtr & msg, const std::string input_frame_from_msg) + { + // 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 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_) + { + 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 (use_mask_ && method_accepts_mask) + { + matchTemplate (frame, templ_, result, match_method_, mask_); + } + else + { + 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; + cv::Point minLoc; + cv::Point maxLoc; + cv::Point matchLoc; + + cv::minMaxLoc (result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat ()); + //! [match_loc] + /// For SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better + if (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_SQDIFF_NORMED || match_method_ == CV_TM_CCORR) + { + matchLoc = minLoc; + } + else + { + matchLoc = maxLoc; + } + 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), + cv::Scalar::all (0), 2, 8, 0); + + // 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); + } + catch (cv::Exception & e) + { + NODELET_ERROR ("Image processing error: %s %s %s %i", e.err.c_str (), e.func.c_str (), e.file.c_str (), e.line); + } + + prev_stamp_ = msg->header.stamp; + } + + void subscribe () + { + NODELET_INFO ("Subscribing to image topic."); + if (config_.use_camera_info) + cam_sub_ = it_->subscribeCamera ("image", 3, &MatchTemplateNodelet::imageCallbackWithInfo, this); + else + img_sub_ = it_->subscribe ("image", 3, &MatchTemplateNodelet::imageCallback, this); + } + + void unsubscribe () + { + NODELET_DEBUG ("Unsubscribing from image topic."); + img_sub_.shutdown (); + cam_sub_.shutdown (); + } + + public: + virtual void onInit () + { + Nodelet::onInit (); + it_ = boost::shared_ptr < image_transport::ImageTransport > (new image_transport::ImageTransport (*nh_)); + + 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")); + + NODELET_INFO ("template_file: %s", templ_file.c_str ()); + + 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_ = imread(templ_file, cv::IMREAD_COLOR); + templ_ = imread (templ_file, cv::IMREAD_COLOR); + + 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_); + 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 < opencv_apps::RectArrayStamped > (*pnh_, "matched_rectangle", 1); + + onInitPostProcess (); + } + }; +} + +#include +PLUGINLIB_EXPORT_CLASS (match_template::MatchTemplateNodelet, nodelet::Nodelet); From 3f78088f5393b67660ea891b4d29feaa8ff45243 Mon Sep 17 00:00:00 2001 From: Ryosuke Tajima Date: Tue, 27 Jun 2017 17:36:51 +0900 Subject: [PATCH 02/15] Add matchTemplate nodelet --- CMakeLists.txt | 3 + cfg/MatchTemplate.cfg | 42 +++++ launch/match_template.launch | 18 ++ nodelet_plugins.xml | 4 + src/nodelet/match_template_nodelet.cpp | 252 +++++++++++++++++++++++++ 5 files changed, 319 insertions(+) create mode 100755 cfg/MatchTemplate.cfg create mode 100644 launch/match_template.launch create mode 100644 src/nodelet/match_template_nodelet.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fb39261f..2f5115e2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,7 @@ generate_dynamic_reconfigure_options( cfg/CamShift.cfg cfg/FBackFlow.cfg cfg/LKFlow.cfg + cfg/MatchTemplate.cfg cfg/PeopleDetect.cfg cfg/PhaseCorr.cfg cfg/SegmentObjects.cfg @@ -330,6 +331,8 @@ 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 new file mode 100755 index 00000000..37c7d565 --- /dev/null +++ b/cfg/MatchTemplate.cfg @@ -0,0 +1,42 @@ +#! /usr/bin/env python +# 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 Kei Okada 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. + + +PACKAGE='match_template' + +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) + +exit(gen.generate(PACKAGE, "match_template", "MatchTemplate")) diff --git a/launch/match_template.launch b/launch/match_template.launch new file mode 100644 index 00000000..de1aa44b --- /dev/null +++ b/launch/match_template.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index 1ab76dac..3bd95d94 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -67,6 +67,10 @@ Nodelet to calculate Lukas-Kanade optical flow + + Nodelet to find the template on the image + + Nodelet to demonstrate the use of the HoG descriptor diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp new file mode 100644 index 00000000..858af0b5 --- /dev/null +++ b/src/nodelet/match_template_nodelet.cpp @@ -0,0 +1,252 @@ +/********************************************************************* +* 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 "opencv_apps/MatchTemplateConfig.h" +#include "opencv_apps/RectArrayStamped.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_; + 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_; + + bool debug_view_; + int match_method_; + bool use_mask_; + + ros::Time prev_stamp_; + + cv::Mat templ_; + cv::Mat mask_; + + void reconfigureCallback (Config & new_config, uint32_t level) + { + config_ = new_config; + } + + const std::string & frameWithDefault (const std::string & frame, const std::string & image_frame) + { + if (frame.empty ()) + return image_frame; + return frame; + } + + void imageCallbackWithInfo (const sensor_msgs::ImageConstPtr & msg, + const sensor_msgs::CameraInfoConstPtr & 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 do_work (const sensor_msgs::ImageConstPtr & msg, const std::string input_frame_from_msg) + { + // 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 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_) + { + 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 (use_mask_ && method_accepts_mask) + { + matchTemplate (frame, templ_, result, match_method_, mask_); + } + else + { + 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; + cv::Point minLoc; + cv::Point maxLoc; + cv::Point matchLoc; + + cv::minMaxLoc (result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat ()); + //! [match_loc] + /// For SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better + if (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_SQDIFF_NORMED || match_method_ == CV_TM_CCORR) + { + matchLoc = minLoc; + } + else + { + matchLoc = maxLoc; + } + 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), + cv::Scalar::all (0), 2, 8, 0); + + // 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); + } + catch (cv::Exception & e) + { + NODELET_ERROR ("Image processing error: %s %s %s %i", e.err.c_str (), e.func.c_str (), e.file.c_str (), e.line); + } + + prev_stamp_ = msg->header.stamp; + } + + void subscribe () + { + NODELET_INFO ("Subscribing to image topic."); + if (config_.use_camera_info) + cam_sub_ = it_->subscribeCamera ("image", 3, &MatchTemplateNodelet::imageCallbackWithInfo, this); + else + img_sub_ = it_->subscribe ("image", 3, &MatchTemplateNodelet::imageCallback, this); + } + + void unsubscribe () + { + NODELET_DEBUG ("Unsubscribing from image topic."); + img_sub_.shutdown (); + cam_sub_.shutdown (); + } + + public: + virtual void onInit () + { + Nodelet::onInit (); + it_ = boost::shared_ptr < image_transport::ImageTransport > (new image_transport::ImageTransport (*nh_)); + + 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")); + + NODELET_INFO ("template_file: %s", templ_file.c_str ()); + + 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_ = imread(templ_file, cv::IMREAD_COLOR); + templ_ = imread (templ_file, cv::IMREAD_COLOR); + + 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_); + 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 < opencv_apps::RectArrayStamped > (*pnh_, "matched_rectangle", 1); + + onInitPostProcess (); + } + }; +} + +#include +PLUGINLIB_EXPORT_CLASS (match_template::MatchTemplateNodelet, nodelet::Nodelet); From 24e568096bb81411dd13b79168e3b14fb4d27d06 Mon Sep 17 00:00:00 2001 From: Ryosuke Tajima Date: Wed, 28 Jun 2017 15:49:22 +0900 Subject: [PATCH 03/15] Remove imview at onInit --- src/nodelet/match_template_nodelet.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp index 858af0b5..354f2083 100644 --- a/src/nodelet/match_template_nodelet.cpp +++ b/src/nodelet/match_template_nodelet.cpp @@ -227,11 +227,6 @@ namespace match_template //templ_ = imread(templ_file, cv::IMREAD_COLOR); templ_ = imread (templ_file, cv::IMREAD_COLOR); - 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 79f8ecee6cdfd12b768404ed9bded1141b72a103 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 11 Jul 2017 16:09:36 +0900 Subject: [PATCH 04/15] 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 05/15] 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 06/15] 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 07/15] 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 08/15] 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 09/15] 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 10/15] 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 11/15] 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 12/15] 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 9cbfdfc3719399289308faacfa9d19509ba72b88 Mon Sep 17 00:00:00 2001 From: Ryosuke Tajima Date: Fri, 14 Jul 2017 14:28:38 +0900 Subject: [PATCH 13/15] Add match_method dynamic parameter --- cfg/MatchTemplate.cfg | 10 +++ src/nodelet/match_template_nodelet.cpp | 107 ++++++++++++++++++------- 2 files changed, 89 insertions(+), 28 deletions(-) diff --git a/cfg/MatchTemplate.cfg b/cfg/MatchTemplate.cfg index 37c7d565..8eec3478 100755 --- a/cfg/MatchTemplate.cfg +++ b/cfg/MatchTemplate.cfg @@ -37,6 +37,16 @@ PACKAGE='match_template' from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() + +method_enum = gen.enum([ gen.const("TM_SQDIFF", int_t, 0, "TM_SQDIFF"), + gen.const("TM_SQDIFF_NORMED", int_t, 1, "TM_SQDIFF_NORMED"), + gen.const("TM_CCORR", int_t, 2, "TM_CCORR"), + gen.const("TM_CCORR_NORMED", int_t, 3, "TM_CCORR_NORMED"), + gen.const("TM_CCOEFF", int_t, 4, "TM_CCOEFF"), + gen.const("TM_CCOEFF_NORMED", int_t, 5, "TM_CCOEFF_NORMED")], + "Template matching method") + +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) exit(gen.generate(PACKAGE, "match_template", "MatchTemplate")) diff --git a/src/nodelet/match_template_nodelet.cpp b/src/nodelet/match_template_nodelet.cpp index 354f2083..38d8044a 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 "geometry_msgs/PointStamped.h" namespace match_template { @@ -61,23 +62,24 @@ namespace match_template 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_; + boost::shared_ptr < ReconfigureServer > reconfigure_server_; bool debug_view_; - int match_method_; + // int match_method_; bool use_mask_; - ros::Time prev_stamp_; + ros::Time prev_stamp_; - cv::Mat templ_; - cv::Mat mask_; + cv::Mat templ_; + cv::Mat mask_; void reconfigureCallback (Config & new_config, uint32_t level) { @@ -102,6 +104,11 @@ namespace match_template 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) { // Work on the image. @@ -121,23 +128,24 @@ namespace match_template //-- Show template if (debug_view_) { + cv::imshow ("Image", frame); 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); + //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) { - matchTemplate (frame, templ_, result, match_method_, mask_); + matchTemplate (frame, templ_, result, config_.match_method, mask_); } else { - matchTemplate (frame, templ_, result, match_method_); + matchTemplate (frame, templ_, result, config_.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] @@ -147,22 +155,52 @@ namespace match_template cv::Point minLoc; cv::Point maxLoc; cv::Point matchLoc; - - cv::minMaxLoc (result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat ()); - //! [match_loc] - /// For SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better - if (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_SQDIFF_NORMED || match_method_ == CV_TM_CCORR) - { - matchLoc = minLoc; + 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; - } - 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), - cv::Scalar::all (0), 2, 8, 0); // Publish the image. sensor_msgs::Image::Ptr out_img = @@ -171,6 +209,16 @@ 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 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); + } catch (cv::Exception & e) { @@ -187,6 +235,9 @@ namespace match_template cam_sub_ = it_->subscribeCamera ("image", 3, &MatchTemplateNodelet::imageCallbackWithInfo, this); else img_sub_ = it_->subscribe ("image", 3, &MatchTemplateNodelet::imageCallback, this); + + template_sub_ = it_->subscribe ("template", 3, &MatchTemplateNodelet::templateCallback, this); + } void unsubscribe () @@ -194,6 +245,7 @@ namespace match_template NODELET_DEBUG ("Unsubscribing from image topic."); img_sub_.shutdown (); cam_sub_.shutdown (); + template_sub_.shutdown (); } public: @@ -203,7 +255,7 @@ namespace match_template it_ = boost::shared_ptr < image_transport::ImageTransport > (new image_transport::ImageTransport (*nh_)); pnh_->param ("debug_view", debug_view_, false); - pnh_->param ("match_method", match_method_, (int) CV_TM_SQDIFF); + // 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")); @@ -224,7 +276,6 @@ namespace match_template NODELET_ERROR ("Cannot open template file %s", templ_file.c_str ()); exit (0); } - //templ_ = imread(templ_file, cv::IMREAD_COLOR); templ_ = imread (templ_file, cv::IMREAD_COLOR); prev_stamp_ = ros::Time (0, 0); @@ -236,7 +287,7 @@ 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); + msg_pub_ = advertise < geometry_msgs::PointStamped > (*pnh_, "pixel_position", 1); onInitPostProcess (); } From 2e5f4fecc66353e0d4631826e064b7a37da73b9e Mon Sep 17 00:00:00 2001 From: Ryosuke Tajima Date: Tue, 18 Jul 2017 12:19:11 +0900 Subject: [PATCH 14/15] Add rostest for match_template - Test using face_detector_withface_test_diamondback.bag - Dynamic reconfigure for match_threshold - Chnage to match only one in the scene image - Change to use camera_info to publish gaze_vector --- CMakeLists.txt | 2 +- cfg/MatchTemplate.cfg | 1 + launch/match_template.launch | 9 +- src/nodelet/match_template_nodelet.cpp | 522 +++++++++++++------------ test/CMakeLists.txt | 2 + test/template.png | Bin 0 -> 5087 bytes test/test-match_template.test | 45 +++ 7 files changed, 331 insertions(+), 250 deletions(-) create mode 100644 test/template.png create mode 100644 test/test-match_template.test 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 0000000000000000000000000000000000000000..f7d22cacd5223709b85ce94c4f00e773a55e3949 GIT binary patch literal 5087 zcmV<56Cmt~P)XhM zkLx$cjO2r4rX_c|T<&EEU;*^Cx~j4=Gs2G^vhW`!9;-5|BHaDBA3q-Um*4)cfBwgR z`llaWfbrpFSZ@E}yUTz0pTB6|{-QN8FGGf8Cc^>DKr#SH#QCZDE3-flnqL<&eJJDm z|M&Y3|NQ&+6`;0O_os*5>EYvF^Zrx5xB*_jyM5g?`Qub}7BLD?k{@A7ToQpY=U>DG zauoAkP#}cD5(>^+t3qIyn^p69LoPGbBNNmm+yj`Dz{6~COJ;)u49?Q>$EU}aV?-7} zn5|u-*JrelLi5}7J1zIxNgL@tfVfiJRW5^v;5PJGfNm6@WpCh1o8Poqmw+v{RMmy1 zA>uh`4DiNP%~xg%vz^<<09%-1cEFEm<-_CS7-1EFWIC9w+%Jp>Il#j~5AS!U^ZkEcfB!e{fMgfBQQ5~hsjPxy0#uQkPDXEJ zV>Eyf7UV;T=@f8wnJyE#@!oBXRDt_8e*bXX{pNYs)Of75tw7z?(nY+~=!$kVmI1_3 zwIltJbd&0D^X*Lmx~;sZtTjZ`r=WF!?5e1fmWf@WrL`|aYkPNn)q1yTw>jrKh9Y7tGU z9)Nf9N^kVfi!>w-4sh*MaS>|7(bO7XtAcm34t#sRBlhMucVXmbu1 z4S>Z!94GS8$1T7ZbSXEhWhSS^1lZBy)_jv}aBEBjfGc#_^X1V#RZqqCXP3pFhVg#v zpU0-Y*=&C9rYWzR{X>>fVPwo14y}Isz@{mri%`S{0 zI#C_Cxoy_}<4s-WdRKKFx))$K*aP;)?6d5(_-ue~O%L*Ku=`c_?JC;_kf9=NRhQBU zd`@@@)yO3}{FHOo0%^50x6GnRb=|51(lYs-+YxI}DBOV>O?~rbJieaZ)v_<;9e`fo z=dMTRr`4XTedmDpp5Bp83 z(tupeeqwg%6w%b!e+DXF%3tcVsi)6n*wo=3AhOk7N{Xp^S8W7nUs>}9Z*RQax7>Er z4zP=sX)3F+w#PU`Z4FRzWHiQ7HiWwXYq1|A>}ido>2$`e-(Mn~5K4`KHtl2BdE-eJJs^=zZP4s{6kJEN->j<$9CiWqtF@_07!&XsV))lT9e}(4{rsh+yQA~MeNkt? zQnq;2s#(SL@#Dks3RR{gu@{@t%9VDlX7tv?h$=*V%>5&YX<08VaHreb}t(3y=Ftg%cVN~Lxch)H@K z+DgZ1x~ix32%HY}G!{F`+A51E0Qre@D67q3`QbR;9VeZDbf{J|95vZE?DBCp0Zop! zVQiTe)%wXi0dAQouxgzczxERCoYj}1p~NWwa`>ItMO;nw&qKKzfkLVMs2DjYq?iO0 zS32#>vmK9dP>pE>25LD*j1kIpI}|TKRRy#&Jr$Zs(Euu8)R-zKu`){r*d$y07*Tep zqC^gW(fqY@oueP>qD*Z8${|Wbl!%BJRjmP1ZPHTe6*f#$tq3S#n(!mOv08E&4Ty@0 zSV^nUp^_?qSEdM1D;ux?uxuvFtRu=4#{iV!&V84gpJRWX>R&4$4@y8aNkqf~SX?jH zt6ERBjH5?k2%sd&U}}-vnv}BxslhwqIm%_cl&jF|yD8pQ%u;tf@^0?Ox6 zr<1x0QZ|wSRFO^3;wjVy=cJ>QWkXP~wOR8uhVqXQ_B&~+F*gGz@lEJe)X&wAwcP|@ zBZ)F?t35S#ytefK=rmM|I4Bm$v*j6OV+~b-Janp*8O<<>4Z4D(&1J&PJa99zIGIfV zq&cX!A?jDyhkcItQtP*&Wld86eO4beo`z;@mT#|{i#<@Ds44ms{myKnHWkoR>=3lr zB)Xzz=JQeu_wWLZnHykcWJ53+YsfQ|6OCn_LSwB>02<7GV!F2S+>lo3bqCZx4m0jwSKNkplWtPSq~VxWj*@&QZd zptg#)*d6wab@<$~<7CMyrD|2eMj%dOxM%)a^-^MPL{1UkO1GvV=XSB2643Nggb&5H zvW>^l0Y4?%Gz8@^cpY@AKv*e`kJ*>WCZCyx88+YxY->IwmjU6#VALd+#uOz3p{Cl8 z5M{e2dZu!mX-?%%++p8kGP+7YEEK2eUTjnxd|EpYCyzxten_^fwLRf;;-R>5D-#Ls$5nrd=kd@3MH53&s;VV+L-Z36W6>~19T*B0_EjVIVrFg= z*jI%&j8;6!cBo^P>S8j13<_+ZozD}Pc|Q%)KEfkBVw7HKdaJ{I=^v+|)S=W`B!>Z! zv|hwA$1>N^r$w%9r*5vIZn&8n<^z`9YC?vL1Ov2ifkX|fVPPH>sz|}9Y7C8mf%M~p zeZN1Aes)>7bf@7@v7hQmQynT$t74K8cFg0xJG?ppb?~xjPCr=qE9JANoJj>FHF;2-LAwmkQ)z znx4yFUV!p&D7l=DrPNcdC53610V$vL@2?@7y)bvrd;o* zYF~po$?jDDtq#xAz-cHGP^i>MnT4Hne2(V>Ano-~aw<(`={wFts=rEdBvw>lL3}sk~(RNozB9gGI26hQxbz1W;sTPe7NgJ zLS^Pd;z9a1=~%0cbw+rdGzwgVRql~++bEB|PGy<$k z-pp|#*6<+q(4P6cRcuaCBP^x%oGnA-!}K!t=UE)Zbc6n@#RtVpt;9Jp(%9|n@`uO8 z-`e7S08Up!|DVTlb9{NfPg5x7c#&nZ;+UI~R|m|E!Dx()=yi^vTurhtOU?-m2E>B; zV}$+m{8BAc1cak|v)_@6Y+1DnpoVHc-KVM_tlbS=KkEv^c;7!pzrQ;Ts?4(+1c_`( z?u~nPz-8W<7xNxc%-B<@LXn8kV(qkNDrZifENW5A#b}hilm8d&v#clVEOM8mhl8G@ zHV;Gg2`Ia9VjNzk!3&^SW0%RzM)QPcJ7X-FjaX-P;q22aFVP?rsCsA+qX0THlr@GF zRs@j59rhoj1-6kM=N4>&TBF4C5Q|iE0ZL4z#N@SxnK;)>jBK==#{hFLE$B?uRQ#-U zLX4&Ot4KTUO4LvwUnt+~B|V^p9pQoO3~y2r0s?PF5m-I#NUlDvTT;k!LwOPAfl?5dJ(QL3$uy^k|ddhcX*Rnn6)@KSTapm zGVjc*xy)tWIt>b{D^5~h)z^qpOU-&R!C8N3v3vZFQu?&Rz?zmgU`OO_1_@V$#y=5qgVc`by9YBZ&!H$vuFil1uiufC45=R>Y0@&r(wM zzSFYNvXP~5Cz)kKHlGlZHRi}1nxyQ?j=3#8x2fHvyiD%NJ*6h4;@-MeXi_bzMFI>n zBVl8h%w%cV<}PLJrKCB|mBqcSKlwL5Z$5tCUVLf*ZxWSIge+zA;y|oZ)KbQ%B?+HP zPt#hHyi4BI)I>@t0m(cqJv~|;Dec$$B1n$OYz$pQG!DenL?UY)x{os>pTsTeZ_}H* zH?PpeFW&?Fu?rvSek|9t#oCJm)FzeVvD_WX%2Yj80-A|t+jV)-UaoVu@Z18P8<)~j zX_NHq;Q*e|D3eIipcn&)MMRJ}gi@U?6iM;L{EN4L^XA=flRk97hi~-ZU;6y>V;-CC zvT1Q(=xqG-a=6?bUZBNHfamaw#n!uRx4zykuGTHkY;v(}e++s)Nw2c%W!)3LiS-!}Fo&3s5s4X>oy5F?rl&?+R) zws&)Eu6Y^0IQP!=!eW_U11vwRFO{yY{rX$agQq+J%VFI7{X^G0tozoNtuJ=~k2|d0 zteT7MYICu=xM%>gj9L3oQlGv3TJtJSG9^xMjWsic4NGJ;;-0U~fVdU6Qk6GQ99Uj8 z+n?I)TYK%9i!^JilQTZ}s%ZON<3Cy3Gk|r8w%cAWwrPTC?*?hcr_*;mn?0lQAz7=NDIM3h`I ziD*qjnqfcBgmOG7EF#Xe%)ZFJU|S?40f*ovz98&A%ik#x>4)iJ|hFl}A)5MqLC{~rM)^`SIfVKM*!002ovPDHLkV1hEx B-|PSY literal 0 HcmV?d00001 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 787f8f2e23f45487515117f898271a9814d2c3ba Mon Sep 17 00:00:00 2001 From: Ryosuke Tajima Date: Thu, 20 Jul 2017 12:09:44 +0900 Subject: [PATCH 15/15] Remove default template and mask file name --- launch/match_template.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/match_template.launch b/launch/match_template.launch index bd2cc50b..03244974 100644 --- a/launch/match_template.launch +++ b/launch/match_template.launch @@ -4,8 +4,8 @@ - - + +