Skip to content

Commit

Permalink
Add matchTemplate nodelet
Browse files Browse the repository at this point in the history
  • Loading branch information
7675t committed Jun 29, 2017
1 parent 2406db3 commit 3f78088
Show file tree
Hide file tree
Showing 5 changed files with 319 additions and 0 deletions.
3 changes: 3 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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}
Expand Down
42 changes: 42 additions & 0 deletions cfg/MatchTemplate.cfg
Original file line number Diff line number Diff line change
@@ -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"))
18 changes: 18 additions & 0 deletions launch/match_template.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
<launch>
<arg name="node_name" default="match_template" />
<arg name="image" default="image" doc="The image topic. Should be remapped to the name of the real image topic." />
<arg name="use_camera_info" default="false" doc="Indicates that the camera_info topic should be subscribed to to get the default input_frame_id. Otherwise the frame from the image message will be used." />
<arg name="debug_view" default="false" doc="Specify whether the node displays a window to show edge image" />
<arg name="match_method" default="0" doc="Matching method. 0:CV_TM_SQDIFF 1:CV_TM_SQDIFF_NORMED 2:CV_TM_CCORR 3:CV_TM_CCORR_NORMED 4:CV_TM_CCOEFF 5:CV_TM_CCOEFF_NORMED" />

<!-- match_template_nodelet.cpp -->
<node name="$(arg node_name)" pkg="opencv_apps" type="match_template" output="screen">
<remap from="image" to="$(arg image)" />
<param name="use_camera_info" value="$(arg use_camera_info)" />
<param name="debug_view" value="$(arg debug_view)" />
<param name="match_method" value="$(arg match_method)" />
<param name="use_mask" value="false" />
<param name="template_file" value="$(find opencv_apps)/template.png" />
<param name="mask_file" value="$(find opencv_apps)/mask.png" />
</node>
</launch>
4 changes: 4 additions & 0 deletions nodelet_plugins.xml
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,10 @@
<description>Nodelet to calculate Lukas-Kanade optical flow</description>
</class>

<class name="match_template/match_template" type="match_template::MatchTemplateNodelet" base_class_type="nodelet::Nodelet">
<description>Nodelet to find the template on the image</description>
</class>

<class name="people_detect/people_detect" type="people_detect::PeopleDetectNodelet" base_class_type="nodelet::Nodelet">
<description>Nodelet to demonstrate the use of the HoG descriptor</description>
</class>
Expand Down
252 changes: 252 additions & 0 deletions src/nodelet/match_template_nodelet.cpp
Original file line number Diff line number Diff line change
@@ -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 <ros/ros.h>
#include "opencv_apps/nodelet.h"
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>

#include <dynamic_reconfigure/server.h>
#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/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS (match_template::MatchTemplateNodelet, nodelet::Nodelet);

0 comments on commit 3f78088

Please sign in to comment.