Skip to content

Commit 9cbfdfc

Browse files
7675t7675t
committed
Add match_method dynamic parameter
1 parent 24e5680 commit 9cbfdfc

File tree

2 files changed

+89
-28
lines changed

2 files changed

+89
-28
lines changed

‎cfg/MatchTemplate.cfg‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,16 @@ PACKAGE='match_template'
3737
from dynamic_reconfigure.parameter_generator_catkin import *
3838

3939
gen = ParameterGenerator()
40+
41+
method_enum = gen.enum([ gen.const("TM_SQDIFF", int_t, 0, "TM_SQDIFF"),
42+
gen.const("TM_SQDIFF_NORMED", int_t, 1, "TM_SQDIFF_NORMED"),
43+
gen.const("TM_CCORR", int_t, 2, "TM_CCORR"),
44+
gen.const("TM_CCORR_NORMED", int_t, 3, "TM_CCORR_NORMED"),
45+
gen.const("TM_CCOEFF", int_t, 4, "TM_CCOEFF"),
46+
gen.const("TM_CCOEFF_NORMED", int_t, 5, "TM_CCOEFF_NORMED")],
47+
"Template matching method")
48+
49+
gen.add("match_method", int_t, 0, "Template matching method", 5, 0, 5, edit_method=method_enum)
4050
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)
4151

4252
exit(gen.generate(PACKAGE, "match_template", "MatchTemplate"))

‎src/nodelet/match_template_nodelet.cpp‎

Lines changed: 79 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,7 @@
5252
#include <dynamic_reconfigure/server.h>
5353
#include "opencv_apps/MatchTemplateConfig.h"
5454
#include "opencv_apps/RectArrayStamped.h"
55+
#include "geometry_msgs/PointStamped.h"
5556

5657
namespace match_template
5758
{
@@ -61,23 +62,24 @@ namespace match_template
6162
image_transport::Publisher matched_img_pub_;
6263
image_transport::Subscriber img_sub_;
6364
image_transport::CameraSubscriber cam_sub_;
65+
image_transport::Subscriber template_sub_;
6466
ros::Publisher msg_pub_;
6567

6668
boost::shared_ptr < image_transport::ImageTransport > it_;
67-
69+
6870
typedef match_template::MatchTemplateConfig Config;
6971
typedef dynamic_reconfigure::Server < Config > ReconfigureServer;
7072
Config config_;
71-
boost::shared_ptr < ReconfigureServer > reconfigure_server_;
73+
boost::shared_ptr < ReconfigureServer > reconfigure_server_;
7274

7375
bool debug_view_;
74-
int match_method_;
76+
// int match_method_;
7577
bool use_mask_;
7678

77-
ros::Time prev_stamp_;
79+
ros::Time prev_stamp_;
7880

79-
cv::Mat templ_;
80-
cv::Mat mask_;
81+
cv::Mat templ_;
82+
cv::Mat mask_;
8183

8284
void reconfigureCallback (Config & new_config, uint32_t level)
8385
{
@@ -102,6 +104,11 @@ namespace match_template
102104
do_work (msg, msg->header.frame_id);
103105
}
104106

107+
void templateCallback (const sensor_msgs::ImageConstPtr & msg)
108+
{
109+
templ_ = cv_bridge::toCvCopy (msg, sensor_msgs::image_encodings::BGR8)->image;
110+
}
111+
105112
void do_work (const sensor_msgs::ImageConstPtr & msg, const std::string input_frame_from_msg)
106113
{
107114
// Work on the image.
@@ -121,23 +128,24 @@ namespace match_template
121128
//-- Show template
122129
if (debug_view_)
123130
{
131+
cv::imshow ("Image", frame);
124132
cv::imshow ("Template", templ_);
125133
int c = cv::waitKey (1);
126134
}
127135

128136
//! [match_template]
129137
/// Do the Matching and Normalize
130-
bool method_accepts_mask = (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_CCORR_NORMED);
138+
//bool method_accepts_mask = (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_CCORR_NORMED);
139+
bool method_accepts_mask = (config_.match_method == CV_TM_SQDIFF || config_.match_method == CV_TM_CCORR_NORMED);
131140
if (use_mask_ && method_accepts_mask)
132141
{
133-
matchTemplate (frame, templ_, result, match_method_, mask_);
142+
matchTemplate (frame, templ_, result, config_.match_method, mask_);
134143
}
135144
else
136145
{
137-
matchTemplate (frame, templ_, result, match_method_);
146+
matchTemplate (frame, templ_, result, config_.match_method);
138147
}
139148
//! [normalize]
140-
//normalize (result, result, 0, 255, cv::NORM_MINMAX, CV_8UC1, cv::Mat ());
141149
normalize (result, result, 0, 255, cv::NORM_MINMAX, CV_8UC1, cv::Mat ());
142150

143151
//! [best_match]
@@ -147,22 +155,52 @@ namespace match_template
147155
cv::Point minLoc;
148156
cv::Point maxLoc;
149157
cv::Point matchLoc;
150-
151-
cv::minMaxLoc (result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat ());
152-
//! [match_loc]
153-
/// For SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better
154-
if (match_method_ == CV_TM_SQDIFF || match_method_ == CV_TM_SQDIFF_NORMED || match_method_ == CV_TM_CCORR)
155-
{
156-
matchLoc = minLoc;
158+
bool is_top = true;
159+
int remove_margin_x = templ_.cols / 2;
160+
int remove_margin_y = templ_.rows / 2;
161+
162+
// find top 10 position from image
163+
for (int i=0; i<3; i++) {
164+
cv::minMaxLoc (result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat ());
165+
NODELET_INFO ("min_val, max_val = %f %f", double(minVal), double(maxVal));
166+
//! [match_loc]
167+
/// For SQDIFF and SQDIFF_NORMED, the best matches are lower values. For all the other methods, the higher the better
168+
if (config_.match_method == CV_TM_SQDIFF || config_.match_method == CV_TM_SQDIFF_NORMED || config_.match_method == CV_TM_CCORR)
169+
{
170+
matchLoc = minLoc;
171+
// remove matched region with some margin
172+
rectangle (result,
173+
cv::Point(matchLoc.x - remove_margin_x,
174+
matchLoc.y - remove_margin_y),
175+
cv::Point(matchLoc.x + remove_margin_x,
176+
matchLoc.y + remove_margin_y),
177+
255, -1, 8, 0);
178+
}
179+
else
180+
{
181+
matchLoc = maxLoc;
182+
// remove matched region
183+
rectangle (result,
184+
cv::Point(matchLoc.x - remove_margin_x,
185+
matchLoc.y - remove_margin_y),
186+
cv::Point(matchLoc.x + remove_margin_x,
187+
matchLoc.y + remove_margin_y),
188+
0, -1, 8, 0);
189+
}
190+
if (is_top)
191+
{
192+
rectangle (frame, matchLoc, cv::Point (matchLoc.x + templ_.cols,
193+
matchLoc.y + templ_.rows),
194+
cv::Scalar(0, 0, 255), 4, 8, 0);
195+
is_top = false;
196+
}
197+
else
198+
{
199+
rectangle (frame, matchLoc, cv::Point (matchLoc.x + templ_.cols,
200+
matchLoc.y + templ_.rows),
201+
cv::Scalar(0, 255, 0), 2, 8, 0);
202+
}
157203
}
158-
else
159-
{
160-
matchLoc = maxLoc;
161-
}
162-
rectangle (frame, matchLoc, cv::Point (matchLoc.x + templ_.cols, matchLoc.y + templ_.rows), cv::Scalar::all (0),
163-
2, 8, 0);
164-
rectangle (result, matchLoc, cv::Point (matchLoc.x + templ_.cols, matchLoc.y + templ_.rows),
165-
cv::Scalar::all (0), 2, 8, 0);
166204

167205
// Publish the image.
168206
sensor_msgs::Image::Ptr out_img =
@@ -171,6 +209,16 @@ namespace match_template
171209
cv_bridge::CvImage (msg->header, sensor_msgs::image_encodings::MONO8, result).toImageMsg ();
172210
img_pub_.publish (out_img);
173211
matched_img_pub_.publish (match_img);
212+
213+
// Publish the point on the image
214+
geometry_msgs::PointStamped point;
215+
point.header.stamp = ros::Time::now ();
216+
point.header.frame_id = input_frame_from_msg;
217+
point.point.x = matchLoc.x + templ_.cols / 2;
218+
point.point.y = matchLoc.y + templ_.rows / 2;
219+
point.point.z = 0;
220+
msg_pub_.publish (point);
221+
174222
}
175223
catch (cv::Exception & e)
176224
{
@@ -187,13 +235,17 @@ namespace match_template
187235
cam_sub_ = it_->subscribeCamera ("image", 3, &MatchTemplateNodelet::imageCallbackWithInfo, this);
188236
else
189237
img_sub_ = it_->subscribe ("image", 3, &MatchTemplateNodelet::imageCallback, this);
238+
239+
template_sub_ = it_->subscribe ("template", 3, &MatchTemplateNodelet::templateCallback, this);
240+
190241
}
191242

192243
void unsubscribe ()
193244
{
194245
NODELET_DEBUG ("Unsubscribing from image topic.");
195246
img_sub_.shutdown ();
196247
cam_sub_.shutdown ();
248+
template_sub_.shutdown ();
197249
}
198250

199251
public:
@@ -203,7 +255,7 @@ namespace match_template
203255
it_ = boost::shared_ptr < image_transport::ImageTransport > (new image_transport::ImageTransport (*nh_));
204256

205257
pnh_->param ("debug_view", debug_view_, false);
206-
pnh_->param ("match_method", match_method_, (int) CV_TM_SQDIFF);
258+
// pnh_->param ("match_method", match_method_, (int) CV_TM_SQDIFF);
207259
pnh_->param ("use_mask", use_mask_, false);
208260
std::string templ_file, mask_file;
209261
pnh_->param ("template_file", templ_file, std::string ("template.png"));
@@ -224,7 +276,6 @@ namespace match_template
224276
NODELET_ERROR ("Cannot open template file %s", templ_file.c_str ());
225277
exit (0);
226278
}
227-
//templ_ = imread(templ_file, cv::IMREAD_COLOR);
228279
templ_ = imread (templ_file, cv::IMREAD_COLOR);
229280

230281
prev_stamp_ = ros::Time (0, 0);
@@ -236,7 +287,7 @@ namespace match_template
236287

237288
img_pub_ = advertiseImage (*pnh_, "image", 1);
238289
matched_img_pub_ = advertiseImage (*pnh_, "matched_image", 1);
239-
msg_pub_ = advertise < opencv_apps::RectArrayStamped > (*pnh_, "matched_rectangle", 1);
290+
msg_pub_ = advertise < geometry_msgs::PointStamped > (*pnh_, "pixel_position", 1);
240291

241292
onInitPostProcess ();
242293
}

0 commit comments

Comments
 (0)