phase_corr_nodelet.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2014, Kei Okada.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Kei Okada nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 // https://github.com/Itseez/opencv/blob/master/samples/cpp/phase_corr.cpp
36 
37 #include <ros/ros.h>
38 #include "opencv_apps/nodelet.h"
40 #include <cv_bridge/cv_bridge.h>
42 
43 #include <opencv2/highgui/highgui.hpp>
44 #include <opencv2/imgproc/imgproc.hpp>
45 
46 #include <dynamic_reconfigure/server.h>
47 #include "opencv_apps/PhaseCorrConfig.h"
48 #include "opencv_apps/Point2DStamped.h"
49 
50 namespace opencv_apps
51 {
53 {
58 
60 
61  typedef opencv_apps::PhaseCorrConfig Config;
62  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
63  Config config_;
65 
69 
70  cv::Mat curr, prev, curr64f, prev64f, hann;
71 
72  std::string window_name_;
73  static bool need_config_update_;
74 
75  void reconfigureCallback(Config& new_config, uint32_t level)
76  {
77  config_ = new_config;
78  }
79 
80  const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame)
81  {
82  if (frame.empty())
83  return image_frame;
84  return frame;
85  }
86 
87  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
88  {
89  doWork(msg, cam_info->header.frame_id);
90  }
91 
92  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
93  {
94  doWork(msg, msg->header.frame_id);
95  }
96 
97  static void trackbarCallback(int /*unused*/, void* /*unused*/)
98  {
99  need_config_update_ = true;
100  }
101 
102  void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg)
103  {
104  // Work on the image.
105  try
106  {
107  // Convert the image into something opencv can handle.
108  cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
109 
110  // Messages
111  opencv_apps::Point2DStamped shift_msg;
112  shift_msg.header = msg->header;
113 
114  // Do the work
115  if (frame.channels() > 1)
116  {
117  cv::cvtColor(frame, curr, cv::COLOR_BGR2GRAY);
118  }
119  else
120  {
121  curr = frame;
122  }
123 
124  if (debug_view_)
125  {
126  cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
127  if (need_config_update_)
128  {
129  reconfigure_server_->updateConfig(config_);
130  need_config_update_ = false;
131  }
132  }
133 
134  if (prev.empty())
135  {
136  prev = curr.clone();
137  cv::createHanningWindow(hann, curr.size(), CV_64F);
138  }
139 
140  prev.convertTo(prev64f, CV_64F);
141  curr.convertTo(curr64f, CV_64F);
142 
143  cv::Point2d shift = cv::phaseCorrelate(prev64f, curr64f, hann);
144  double radius = cv::sqrt(shift.x * shift.x + shift.y * shift.y);
145 
146  if (radius > 0)
147  {
148  // draw a circle and line indicating the shift direction...
149  cv::Point center(curr.cols >> 1, curr.rows >> 1);
150 #ifndef CV_VERSION_EPOCH
151  cv::circle(frame, center, (int)(radius * 5), cv::Scalar(0, 255, 0), 3, cv::LINE_AA);
152  cv::line(frame, center, cv::Point(center.x + (int)(shift.x * 5), center.y + (int)(shift.y * 5)),
153  cv::Scalar(0, 255, 0), 3, cv::LINE_AA);
154 #else
155  cv::circle(frame, center, (int)(radius * 5), cv::Scalar(0, 255, 0), 3, CV_AA);
156  cv::line(frame, center, cv::Point(center.x + (int)(shift.x * 5), center.y + (int)(shift.y * 5)),
157  cv::Scalar(0, 255, 0), 3, CV_AA);
158 #endif
159 
160  //
161  shift_msg.point.x = shift.x;
162  shift_msg.point.y = shift.y;
163  }
164 
165  //-- Show what you got
166  if (debug_view_)
167  {
168  cv::imshow(window_name_, frame);
169  int c = cv::waitKey(1);
170  }
171 
172  prev = curr.clone();
173 
174  // Publish the image.
175  sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, frame).toImageMsg();
176  img_pub_.publish(out_img);
177  msg_pub_.publish(shift_msg);
178  }
179  catch (cv::Exception& e)
180  {
181  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
182  }
183 
184  prev_stamp_ = msg->header.stamp;
185  }
186 
187  void subscribe() // NOLINT(modernize-use-override)
188  {
189  NODELET_DEBUG("Subscribing to image topic.");
190  if (config_.use_camera_info)
191  cam_sub_ = it_->subscribeCamera("image", queue_size_, &PhaseCorrNodelet::imageCallbackWithInfo, this);
192  else
193  img_sub_ = it_->subscribe("image", queue_size_, &PhaseCorrNodelet::imageCallback, this);
194  }
195 
196  void unsubscribe() // NOLINT(modernize-use-override)
197  {
198  NODELET_DEBUG("Unsubscribing from image topic.");
199  img_sub_.shutdown();
200  cam_sub_.shutdown();
201  }
202 
203 public:
204  virtual void onInit() // NOLINT(modernize-use-override)
205  {
206  Nodelet::onInit();
208 
209  pnh_->param("queue_size", queue_size_, 3);
210  pnh_->param("debug_view", debug_view_, false);
211  if (debug_view_)
212  {
213  always_subscribe_ = true;
214  }
215  prev_stamp_ = ros::Time(0, 0);
216 
217  window_name_ = "phase shift";
218 
219  reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
220  dynamic_reconfigure::Server<Config>::CallbackType f =
221  boost::bind(&PhaseCorrNodelet::reconfigureCallback, this, _1, _2);
222  reconfigure_server_->setCallback(f);
223 
224  img_pub_ = advertiseImage(*pnh_, "image", 1);
225  msg_pub_ = advertise<opencv_apps::Point2DStamped>(*pnh_, "shift", 1);
226 
228  }
229 };
231 } // namespace opencv_apps
232 
233 namespace phase_corr
234 {
236 {
237 public:
238  virtual void onInit() // NOLINT(modernize-use-override)
239  {
240  ROS_WARN("DeprecationWarning: Nodelet phase_corr/phase_corr is deprecated, "
241  "and renamed to opencv_apps/phase_corr.");
243  }
244 };
245 } // namespace phase_corr
246 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
static void trackbarCallback(int, void *)
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
f
opencv_apps::PhaseCorrConfig Config
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Definition: nodelet.h:70
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
Demo code to calculate moments.
Definition: nodelet.h:48
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
Definition: nodelet.h:250
image_transport::CameraSubscriber cam_sub_
#define ROS_WARN(...)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
void doWork(const sensor_msgs::ImageConstPtr &msg, const std::string &input_frame_from_msg)
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
Definition: nodelet.h:273
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
virtual void onInitPostProcess()
Post processing of initialization of nodelet. You need to call this method in order to use always_sub...
Definition: nodelet.cpp:57
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
Definition: nodelet.cpp:40
boost::shared_ptr< image_transport::ImageTransport > it_
void publish(const sensor_msgs::Image &message) const
image_transport::Subscriber img_sub_
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
Definition: nodelet.h:245
boost::shared_ptr< ReconfigureServer > reconfigure_server_
image_transport::Publisher img_pub_
void reconfigureCallback(Config &new_config, uint32_t level)
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
image_transport::Publisher advertiseImage(ros::NodeHandle &nh, const std::string &topic, int queue_size)
Advertise an image topic and watch the publisher. Publishers which are created by this method...
Definition: nodelet.h:180
PLUGINLIB_EXPORT_CLASS(opencv_apps::PhaseCorrNodelet, nodelet::Nodelet)
dynamic_reconfigure::Server< Config > ReconfigureServer
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const


opencv_apps
Author(s): Kei Okada
autogenerated on Sat Aug 22 2020 03:35:08