lk_flow_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/2.4/samples/cpp/lk_demo.cpp
40 #include <ros/ros.h>
41 #include "opencv_apps/nodelet.h"
44 #include <cv_bridge/cv_bridge.h>
46 
47 #include <opencv2/highgui/highgui.hpp>
48 #include <opencv2/imgproc/imgproc.hpp>
49 #include <opencv2/video/tracking.hpp>
50 
51 #include <dynamic_reconfigure/server.h>
52 #include "std_srvs/Empty.h"
53 #include "opencv_apps/LKFlowConfig.h"
54 #include "opencv_apps/FlowArrayStamped.h"
55 
56 namespace opencv_apps
57 {
59 {
67 
69 
70  typedef opencv_apps::LKFlowConfig Config;
71  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
72  Config config_;
74 
78 
79  std::string window_name_;
80  static bool need_config_update_;
81 
82  int MAX_COUNT;
83  bool needToInit;
84  bool nightMode;
85  cv::Point2f point;
87  cv::Mat gray, prevGray;
88  std::vector<cv::Point2f> points[2];
89 
93  float harris_k_;
94 
95  void reconfigureCallback(Config& new_config, uint32_t level)
96  {
97  config_ = new_config;
98  quality_level_ = config_.quality_level;
99  min_distance_ = config_.min_distance;
100  block_size_ = config_.block_size;
101  harris_k_ = config_.harris_k;
102  }
103 
104  const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame)
105  {
106  if (frame.empty())
107  return image_frame;
108  return frame;
109  }
110 
111  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
112  {
113  doWork(msg, cam_info->header.frame_id);
114  }
115 
116  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
117  {
118  doWork(msg, msg->header.frame_id);
119  }
120 #if 0
121  static void onMouse( int event, int x, int y, int /*flags*/, void* /*param*/ )
122  {
123  if( event == CV_EVENT_LBUTTONDOWN )
124  {
125  point = Point2f((float)x, (float)y);
126  addRemovePt = true;
127  }
128  }
129 #endif
130  static void trackbarCallback(int /*unused*/, void* /*unused*/)
131  {
132  need_config_update_ = true;
133  }
134 
135  void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg)
136  {
137  // Work on the image.
138  try
139  {
140  // Convert the image into something opencv can handle.
141  cv::Mat image = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
142 
143  // Messages
144  opencv_apps::FlowArrayStamped flows_msg;
145  flows_msg.header = msg->header;
146 
147  if (debug_view_)
148  {
150  cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
151 
152  cv::createTrackbar("Min Distance", window_name_, &min_distance_, 100, trackbarCallback);
153  cv::createTrackbar("Block Size", window_name_, &block_size_, 100, trackbarCallback);
154 
155  // cv::setMouseCallback( window_name_, onMouse, 0 );
156  if (need_config_update_)
157  {
158  reconfigure_server_->updateConfig(config_);
159  config_.quality_level = quality_level_;
160  config_.min_distance = min_distance_;
161  config_.block_size = block_size_;
162  config_.harris_k = harris_k_;
163  need_config_update_ = false;
164  }
165  }
166 
167  // Do the work
168  cv::TermCriteria termcrit(cv::TermCriteria::MAX_ITER + cv::TermCriteria::EPS, 20, 0.03);
169  cv::Size sub_pix_win_size(10, 10), win_size(31, 31);
170 
171  if (image.channels() > 1)
172  {
173  cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
174  }
175  else
176  {
177  image.copyTo(gray);
178  }
179 
180  if (nightMode)
181  image = cv::Scalar::all(0);
182 
183  if (needToInit)
184  {
185  // automatic initialization
186  cv::goodFeaturesToTrack(gray, points[1], MAX_COUNT, quality_level_, min_distance_, cv::Mat(), block_size_,
187  false, harris_k_);
188  cv::cornerSubPix(gray, points[1], sub_pix_win_size, cv::Size(-1, -1), termcrit);
189  addRemovePt = false;
190  }
191  else if (!points[0].empty())
192  {
193  std::vector<uchar> status;
194  std::vector<float> err;
195  if (prevGray.empty())
196  gray.copyTo(prevGray);
197  cv::calcOpticalFlowPyrLK(prevGray, gray, points[0], points[1], status, err, win_size, 3, termcrit, 0, 0.001);
198  size_t i, k;
199  for (i = k = 0; i < points[1].size(); i++)
200  {
201  if (addRemovePt)
202  {
203  if (cv::norm(point - points[1][i]) <= 5)
204  {
205  addRemovePt = false;
206  continue;
207  }
208  }
209 
210  if (!status[i])
211  continue;
212 
213  points[1][k++] = points[1][i];
214  cv::circle(image, points[1][i], 3, cv::Scalar(0, 255, 0), -1, 8);
215  cv::line(image, points[1][i], points[0][i], cv::Scalar(0, 255, 0), 1, 8, 0);
216 
217  opencv_apps::Flow flow_msg;
218  opencv_apps::Point2D point_msg;
219  opencv_apps::Point2D velocity_msg;
220  point_msg.x = points[1][i].x;
221  point_msg.y = points[1][i].y;
222  velocity_msg.x = points[1][i].x - points[0][i].x;
223  velocity_msg.y = points[1][i].y - points[0][i].y;
224  flow_msg.point = point_msg;
225  flow_msg.velocity = velocity_msg;
226  flows_msg.flow.push_back(flow_msg);
227  }
228  points[1].resize(k);
229  }
230 
231  if (addRemovePt && points[1].size() < (size_t)MAX_COUNT)
232  {
233  std::vector<cv::Point2f> tmp;
234  tmp.push_back(point);
235  cv::cornerSubPix(gray, tmp, win_size, cv::Size(-1, -1), termcrit);
236  points[1].push_back(tmp[0]);
237  addRemovePt = false;
238  }
239 
240  needToInit = false;
241  if (debug_view_)
242  {
243  cv::imshow(window_name_, image);
244 
245  char c = (char)cv::waitKey(1);
246  // if( c == 27 )
247  // break;
248  switch (c)
249  {
250  case 'r':
251  needToInit = true;
252  break;
253  case 'c':
254  points[0].clear();
255  points[1].clear();
256  break;
257  case 'n':
258  nightMode = !nightMode;
259  break;
260  }
261  }
262  std::swap(points[1], points[0]);
263  cv::swap(prevGray, gray);
264 
265  // Publish the image.
266  sensor_msgs::Image::Ptr out_img = cv_bridge::CvImage(msg->header, msg->encoding, image).toImageMsg();
267  img_pub_.publish(out_img);
268  msg_pub_.publish(flows_msg);
269  }
270  catch (cv::Exception& e)
271  {
272  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
273  }
274 
275  prev_stamp_ = msg->header.stamp;
276  }
277 
278  bool initializePointsCb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
279  {
280  needToInit = true;
281  return true;
282  }
283 
284  bool deletePointsCb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
285  {
286  points[0].clear();
287  points[1].clear();
288  return true;
289  }
290 
291  bool toggleNightModeCb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
292  {
293  nightMode = !nightMode;
294  return true;
295  }
296 
297  void subscribe() // NOLINT(modernize-use-override)
298  {
299  NODELET_DEBUG("Subscribing to image topic.");
300  if (config_.use_camera_info)
301  cam_sub_ = it_->subscribeCamera("image", queue_size_, &LKFlowNodelet::imageCallbackWithInfo, this);
302  else
303  img_sub_ = it_->subscribe("image", queue_size_, &LKFlowNodelet::imageCallback, this);
304  }
305 
306  void unsubscribe() // NOLINT(modernize-use-override)
307  {
308  NODELET_DEBUG("Unsubscribing from image topic.");
309  img_sub_.shutdown();
310  cam_sub_.shutdown();
311  }
312 
313 public:
314  virtual void onInit() // NOLINT(modernize-use-override)
315  {
316  Nodelet::onInit();
318 
319  pnh_->param("queue_size", queue_size_, 3);
320  pnh_->param("debug_view", debug_view_, false);
321  if (debug_view_)
322  {
323  always_subscribe_ = true;
324  }
325  prev_stamp_ = ros::Time(0, 0);
326 
327  window_name_ = "LK Demo";
328  MAX_COUNT = 500;
329  needToInit = true;
330  nightMode = false;
331  addRemovePt = false;
332 
333  reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
334  dynamic_reconfigure::Server<Config>::CallbackType f =
335  boost::bind(&LKFlowNodelet::reconfigureCallback, this, _1, _2);
336  reconfigure_server_->setCallback(f);
337 
338  img_pub_ = advertiseImage(*pnh_, "image", 1);
339  msg_pub_ = advertise<opencv_apps::FlowArrayStamped>(*pnh_, "flows", 1);
340  initialize_points_service_ = pnh_->advertiseService("initialize_points", &LKFlowNodelet::initializePointsCb, this);
341  delete_points_service_ = pnh_->advertiseService("delete_points", &LKFlowNodelet::deletePointsCb, this);
342  toggle_night_mode_service_ = pnh_->advertiseService("toggle_night_mode", &LKFlowNodelet::toggleNightModeCb, this);
343 
344  NODELET_INFO("Hot keys: ");
345  NODELET_INFO("\tESC - quit the program");
346  NODELET_INFO("\tr - auto-initialize tracking");
347  NODELET_INFO("\tc - delete all the points");
348  NODELET_INFO("\tn - switch the \"night\" mode on/off");
349  // NODELET_INFO("To add/remove a feature point click it");
350 
352  }
353 };
355 } // namespace opencv_apps
356 
357 namespace lk_flow
358 {
360 {
361 public:
362  virtual void onInit() // NOLINT(modernize-use-override)
363  {
364  ROS_WARN("DeprecationWarning: Nodelet lk_flow/lk_flow is deprecated, "
365  "and renamed to opencv_apps/lk_flow.");
367  }
368 };
369 } // namespace lk_flow
370 
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
boost::shared_ptr< ReconfigureServer > reconfigure_server_
#define NODELET_ERROR(...)
std::vector< cv::Point2f > points[2]
void publish(const boost::shared_ptr< M > &message) const
f
void reconfigureCallback(Config &new_config, uint32_t level)
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Definition: nodelet.h:70
Demo code to calculate moments.
Definition: nodelet.h:48
ros::ServiceServer initialize_points_service_
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
Definition: nodelet.h:250
void doWork(const sensor_msgs::ImageConstPtr &msg, const std::string &input_frame_from_msg)
opencv_apps::LKFlowConfig Config
#define ROS_WARN(...)
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
Definition: nodelet.h:273
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
void publish(const sensor_msgs::Image &message) const
PLUGINLIB_EXPORT_CLASS(opencv_apps::LKFlowNodelet, nodelet::Nodelet)
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
Definition: nodelet.h:245
bool deletePointsCb(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
bool toggleNightModeCb(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
boost::shared_ptr< image_transport::ImageTransport > it_
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
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
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
image_transport::CameraSubscriber cam_sub_
#define NODELET_INFO(...)
ros::ServiceServer toggle_night_mode_service_
ros::ServiceServer delete_points_service_
image_transport::Publisher img_pub_
dynamic_reconfigure::Server< Config > ReconfigureServer
image_transport::Subscriber img_sub_
bool initializePointsCb(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
#define NODELET_DEBUG(...)
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
sensor_msgs::ImagePtr toImageMsg() const
static void trackbarCallback(int, void *)


opencv_apps
Author(s): Kei Okada
autogenerated on Wed Apr 24 2019 03:00:17