stereo_view.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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 #include <opencv2/highgui/highgui.hpp>
36 
37 #include <ros/ros.h>
38 #include <sensor_msgs/Image.h>
40 #include <stereo_msgs/DisparityImage.h>
41 #include <cv_bridge/cv_bridge.h>
42 
48 
49 #include <boost/thread.hpp>
50 #include <boost/format.hpp>
51 
52 #ifdef HAVE_GTK
53 #include <gtk/gtk.h>
54 
55 // Platform-specific workaround for #3026: image_view doesn't close when
56 // closing image window. On platforms using GTK+ we connect this to the
57 // window's "destroy" event so that image_view exits.
58 static void destroy(GtkWidget *widget, gpointer data)
59 {
60  ros::shutdown();
61 }
62 #endif
63 
65 
66 // colormap for disparities, RGB
67 static unsigned char colormap[768] =
68  { 150, 150, 150,
69  107, 0, 12,
70  106, 0, 18,
71  105, 0, 24,
72  103, 0, 30,
73  102, 0, 36,
74  101, 0, 42,
75  99, 0, 48,
76  98, 0, 54,
77  97, 0, 60,
78  96, 0, 66,
79  94, 0, 72,
80  93, 0, 78,
81  92, 0, 84,
82  91, 0, 90,
83  89, 0, 96,
84  88, 0, 102,
85  87, 0, 108,
86  85, 0, 114,
87  84, 0, 120,
88  83, 0, 126,
89  82, 0, 131,
90  80, 0, 137,
91  79, 0, 143,
92  78, 0, 149,
93  77, 0, 155,
94  75, 0, 161,
95  74, 0, 167,
96  73, 0, 173,
97  71, 0, 179,
98  70, 0, 185,
99  69, 0, 191,
100  68, 0, 197,
101  66, 0, 203,
102  65, 0, 209,
103  64, 0, 215,
104  62, 0, 221,
105  61, 0, 227,
106  60, 0, 233,
107  59, 0, 239,
108  57, 0, 245,
109  56, 0, 251,
110  55, 0, 255,
111  54, 0, 255,
112  52, 0, 255,
113  51, 0, 255,
114  50, 0, 255,
115  48, 0, 255,
116  47, 0, 255,
117  46, 0, 255,
118  45, 0, 255,
119  43, 0, 255,
120  42, 0, 255,
121  41, 0, 255,
122  40, 0, 255,
123  38, 0, 255,
124  37, 0, 255,
125  36, 0, 255,
126  34, 0, 255,
127  33, 0, 255,
128  32, 0, 255,
129  31, 0, 255,
130  29, 0, 255,
131  28, 0, 255,
132  27, 0, 255,
133  26, 0, 255,
134  24, 0, 255,
135  23, 0, 255,
136  22, 0, 255,
137  20, 0, 255,
138  19, 0, 255,
139  18, 0, 255,
140  17, 0, 255,
141  15, 0, 255,
142  14, 0, 255,
143  13, 0, 255,
144  11, 0, 255,
145  10, 0, 255,
146  9, 0, 255,
147  8, 0, 255,
148  6, 0, 255,
149  5, 0, 255,
150  4, 0, 255,
151  3, 0, 255,
152  1, 0, 255,
153  0, 4, 255,
154  0, 10, 255,
155  0, 16, 255,
156  0, 22, 255,
157  0, 28, 255,
158  0, 34, 255,
159  0, 40, 255,
160  0, 46, 255,
161  0, 52, 255,
162  0, 58, 255,
163  0, 64, 255,
164  0, 70, 255,
165  0, 76, 255,
166  0, 82, 255,
167  0, 88, 255,
168  0, 94, 255,
169  0, 100, 255,
170  0, 106, 255,
171  0, 112, 255,
172  0, 118, 255,
173  0, 124, 255,
174  0, 129, 255,
175  0, 135, 255,
176  0, 141, 255,
177  0, 147, 255,
178  0, 153, 255,
179  0, 159, 255,
180  0, 165, 255,
181  0, 171, 255,
182  0, 177, 255,
183  0, 183, 255,
184  0, 189, 255,
185  0, 195, 255,
186  0, 201, 255,
187  0, 207, 255,
188  0, 213, 255,
189  0, 219, 255,
190  0, 225, 255,
191  0, 231, 255,
192  0, 237, 255,
193  0, 243, 255,
194  0, 249, 255,
195  0, 255, 255,
196  0, 255, 249,
197  0, 255, 243,
198  0, 255, 237,
199  0, 255, 231,
200  0, 255, 225,
201  0, 255, 219,
202  0, 255, 213,
203  0, 255, 207,
204  0, 255, 201,
205  0, 255, 195,
206  0, 255, 189,
207  0, 255, 183,
208  0, 255, 177,
209  0, 255, 171,
210  0, 255, 165,
211  0, 255, 159,
212  0, 255, 153,
213  0, 255, 147,
214  0, 255, 141,
215  0, 255, 135,
216  0, 255, 129,
217  0, 255, 124,
218  0, 255, 118,
219  0, 255, 112,
220  0, 255, 106,
221  0, 255, 100,
222  0, 255, 94,
223  0, 255, 88,
224  0, 255, 82,
225  0, 255, 76,
226  0, 255, 70,
227  0, 255, 64,
228  0, 255, 58,
229  0, 255, 52,
230  0, 255, 46,
231  0, 255, 40,
232  0, 255, 34,
233  0, 255, 28,
234  0, 255, 22,
235  0, 255, 16,
236  0, 255, 10,
237  0, 255, 4,
238  2, 255, 0,
239  8, 255, 0,
240  14, 255, 0,
241  20, 255, 0,
242  26, 255, 0,
243  32, 255, 0,
244  38, 255, 0,
245  44, 255, 0,
246  50, 255, 0,
247  56, 255, 0,
248  62, 255, 0,
249  68, 255, 0,
250  74, 255, 0,
251  80, 255, 0,
252  86, 255, 0,
253  92, 255, 0,
254  98, 255, 0,
255  104, 255, 0,
256  110, 255, 0,
257  116, 255, 0,
258  122, 255, 0,
259  128, 255, 0,
260  133, 255, 0,
261  139, 255, 0,
262  145, 255, 0,
263  151, 255, 0,
264  157, 255, 0,
265  163, 255, 0,
266  169, 255, 0,
267  175, 255, 0,
268  181, 255, 0,
269  187, 255, 0,
270  193, 255, 0,
271  199, 255, 0,
272  205, 255, 0,
273  211, 255, 0,
274  217, 255, 0,
275  223, 255, 0,
276  229, 255, 0,
277  235, 255, 0,
278  241, 255, 0,
279  247, 255, 0,
280  253, 255, 0,
281  255, 251, 0,
282  255, 245, 0,
283  255, 239, 0,
284  255, 233, 0,
285  255, 227, 0,
286  255, 221, 0,
287  255, 215, 0,
288  255, 209, 0,
289  255, 203, 0,
290  255, 197, 0,
291  255, 191, 0,
292  255, 185, 0,
293  255, 179, 0,
294  255, 173, 0,
295  255, 167, 0,
296  255, 161, 0,
297  255, 155, 0,
298  255, 149, 0,
299  255, 143, 0,
300  255, 137, 0,
301  255, 131, 0,
302  255, 126, 0,
303  255, 120, 0,
304  255, 114, 0,
305  255, 108, 0,
306  255, 102, 0,
307  255, 96, 0,
308  255, 90, 0,
309  255, 84, 0,
310  255, 78, 0,
311  255, 72, 0,
312  255, 66, 0,
313  255, 60, 0,
314  255, 54, 0,
315  255, 48, 0,
316  255, 42, 0,
317  255, 36, 0,
318  255, 30, 0,
319  255, 24, 0,
320  255, 18, 0,
321  255, 12, 0,
322  255, 6, 0,
323  255, 0, 0,
324  };
325 
326 inline void increment(int* value)
327 {
328  ++(*value);
329 }
330 
331 using namespace sensor_msgs;
332 using namespace stereo_msgs;
333 using namespace message_filters::sync_policies;
334 
335 // Note: StereoView is NOT nodelet-based, as it synchronizes the three streams.
337 {
338 private:
348 
349  ImageConstPtr last_left_msg_, last_right_msg_;
350  cv::Mat last_left_image_, last_right_image_;
351  cv::Mat_<cv::Vec3b> disparity_color_;
352  boost::mutex image_mutex_;
353 
354  boost::format filename_format_;
356 
358  int left_received_, right_received_, disp_received_, all_received_;
359 
360 public:
361  StereoView(const std::string& transport)
362  : filename_format_(""), save_count_(0),
363  left_received_(0), right_received_(0), disp_received_(0), all_received_(0)
364  {
365  // Read local parameters
366  ros::NodeHandle local_nh("~");
367  bool autosize;
368  local_nh.param("autosize", autosize, true);
369 
370  std::string format_string;
371  local_nh.param("filename_format", format_string, std::string("%s%04i.jpg"));
372  filename_format_.parse(format_string);
373 
374  // Do GUI window setup
375  int flags = autosize ? cv::WND_PROP_AUTOSIZE : 0;
376  cv::namedWindow("left", flags);
377  cv::namedWindow("right", flags);
378  cv::namedWindow("disparity", flags);
379  cv::setMouseCallback("left", &StereoView::mouseCb, this);
380  cv::setMouseCallback("right", &StereoView::mouseCb, this);
381  cv::setMouseCallback("disparity", &StereoView::mouseCb, this);
382 #if CV_MAJOR_VERSION == 2
383 #ifdef HAVE_GTK
384  g_signal_connect(GTK_WIDGET( cvGetWindowHandle("left") ),
385  "destroy", G_CALLBACK(destroy), NULL);
386  g_signal_connect(GTK_WIDGET( cvGetWindowHandle("right") ),
387  "destroy", G_CALLBACK(destroy), NULL);
388  g_signal_connect(GTK_WIDGET( cvGetWindowHandle("disparity") ),
389  "destroy", G_CALLBACK(destroy), NULL);
390 #endif
391  cvStartWindowThread();
392 #endif
393 
394  // Resolve topic names
395  ros::NodeHandle nh;
396  std::string stereo_ns = nh.resolveName("stereo");
397  std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
398  std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image"));
399  std::string disparity_topic = ros::names::clean(stereo_ns + "/disparity");
400  ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s", left_topic.c_str(), right_topic.c_str(),
401  disparity_topic.c_str());
402 
403  // Subscribe to three input topics.
405  left_sub_.subscribe(it, left_topic, 1, transport);
406  right_sub_.subscribe(it, right_topic, 1, transport);
407  disparity_sub_.subscribe(nh, disparity_topic, 1);
408 
409  // Complain every 30s if the topics appear unsynchronized
410  left_sub_.registerCallback(boost::bind(increment, &left_received_));
411  right_sub_.registerCallback(boost::bind(increment, &right_received_));
412  disparity_sub_.registerCallback(boost::bind(increment, &disp_received_));
413  check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
414  boost::bind(&StereoView::checkInputsSynchronized, this));
415 
416  // Synchronize input topics. Optionally do approximate synchronization.
417  local_nh.param("queue_size", queue_size_, 5);
418  bool approx;
419  local_nh.param("approximate_sync", approx, false);
420  if (approx)
421  {
422  approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size_),
423  left_sub_, right_sub_, disparity_sub_) );
424  approximate_sync_->registerCallback(boost::bind(&StereoView::imageCb, this, _1, _2, _3));
425  }
426  else
427  {
428  exact_sync_.reset( new ExactSync(ExactPolicy(queue_size_),
429  left_sub_, right_sub_, disparity_sub_) );
430  exact_sync_->registerCallback(boost::bind(&StereoView::imageCb, this, _1, _2, _3));
431  }
432  }
433 
435  {
436  cv::destroyAllWindows();
437  }
438 
439  void imageCb(const ImageConstPtr& left, const ImageConstPtr& right,
440  const DisparityImageConstPtr& disparity_msg)
441  {
442  ++all_received_; // For error checking
443 
444  image_mutex_.lock();
445 
446  // May want to view raw bayer data
447  if (left->encoding.find("bayer") != std::string::npos)
448  boost::const_pointer_cast<Image>(left)->encoding = "mono8";
449  if (right->encoding.find("bayer") != std::string::npos)
450  boost::const_pointer_cast<Image>(right)->encoding = "mono8";
451 
452  // Hang on to image data for sake of mouseCb
453  last_left_msg_ = left;
454  last_right_msg_ = right;
455  try {
456  last_left_image_ = cv_bridge::toCvShare(left, "bgr8")->image;
457  last_right_image_ = cv_bridge::toCvShare(right, "bgr8")->image;
458  }
459  catch (cv_bridge::Exception& e) {
460  ROS_ERROR("Unable to convert one of '%s' or '%s' to 'bgr8'",
461  left->encoding.c_str(), right->encoding.c_str());
462  }
463 
464  // Colormap and display the disparity image
465  float min_disparity = disparity_msg->min_disparity;
466  float max_disparity = disparity_msg->max_disparity;
467  float multiplier = 255.0f / (max_disparity - min_disparity);
468 
469  assert(disparity_msg->image.encoding == enc::TYPE_32FC1);
470  const cv::Mat_<float> dmat(disparity_msg->image.height, disparity_msg->image.width,
471  (float*)&disparity_msg->image.data[0], disparity_msg->image.step);
472  disparity_color_.create(disparity_msg->image.height, disparity_msg->image.width);
473 
474  for (int row = 0; row < disparity_color_.rows; ++row) {
475  const float* d = dmat[row];
476  for (int col = 0; col < disparity_color_.cols; ++col) {
477  int index = (d[col] - min_disparity) * multiplier + 0.5;
478  index = std::min(255, std::max(0, index));
479  // Fill as BGR
480  disparity_color_(row, col)[2] = colormap[3*index + 0];
481  disparity_color_(row, col)[1] = colormap[3*index + 1];
482  disparity_color_(row, col)[0] = colormap[3*index + 2];
483  }
484  }
485 
486  // Must release the mutex before calling cv::imshow, or can deadlock against
487  // OpenCV's window mutex.
488  image_mutex_.unlock();
489  if (!last_left_image_.empty()) {
490  cv::imshow("left", last_left_image_);
491  cv::waitKey(1);
492  }
493  if (!last_right_image_.empty()) {
494  cv::imshow("right", last_right_image_);
495  cv::waitKey(1);
496  }
497  cv::imshow("disparity", disparity_color_);
498  cv::waitKey(1);
499  }
500 
501  void saveImage(const char* prefix, const cv::Mat& image)
502  {
503  if (!image.empty()) {
504  std::string filename = (filename_format_ % prefix % save_count_).str();
505  cv::imwrite(filename, image);
506  ROS_INFO("Saved image %s", filename.c_str());
507  } else {
508  ROS_WARN("Couldn't save %s image, no data!", prefix);
509  }
510  }
511 
512  static void mouseCb(int event, int x, int y, int flags, void* param)
513  {
514  if (event == cv::EVENT_LBUTTONDOWN)
515  {
516  ROS_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
517  return;
518  }
519  if (event != cv::EVENT_RBUTTONDOWN)
520  return;
521 
522  StereoView *sv = (StereoView*)param;
523  boost::lock_guard<boost::mutex> guard(sv->image_mutex_);
524 
525  sv->saveImage("left", sv->last_left_image_);
526  sv->saveImage("right", sv->last_right_image_);
527  sv->saveImage("disp", sv->disparity_color_);
528  sv->save_count_++;
529  }
530 
532  {
533  int threshold = 3 * all_received_;
534  if (left_received_ >= threshold || right_received_ >= threshold || disp_received_ >= threshold) {
535  ROS_WARN("[stereo_view] Low number of synchronized left/right/disparity triplets received.\n"
536  "Left images received: %d (topic '%s')\n"
537  "Right images received: %d (topic '%s')\n"
538  "Disparity images received: %d (topic '%s')\n"
539  "Synchronized triplets: %d\n"
540  "Possible issues:\n"
541  "\t* stereo_image_proc is not running.\n"
542  "\t Does `rosnode info %s` show any connections?\n"
543  "\t* The cameras are not synchronized.\n"
544  "\t Try restarting stereo_view with parameter _approximate_sync:=True\n"
545  "\t* The network is too slow. One or more images are dropped from each triplet.\n"
546  "\t Try restarting stereo_view, increasing parameter 'queue_size' (currently %d)",
547  left_received_, left_sub_.getTopic().c_str(),
548  right_received_, right_sub_.getTopic().c_str(),
549  disp_received_, disparity_sub_.getTopic().c_str(),
550  all_received_, ros::this_node::getName().c_str(), queue_size_);
551  }
552  }
553 };
554 
555 int main(int argc, char **argv)
556 {
557  ros::init(argc, argv, "stereo_view", ros::init_options::AnonymousName);
558  if (ros::names::remap("stereo") == "stereo") {
559  ROS_WARN("'stereo' has not been remapped! Example command-line usage:\n"
560  "\t$ rosrun image_view stereo_view stereo:=narrow_stereo image:=image_color");
561  }
562  if (ros::names::remap("image") == "/image_raw") {
563  ROS_WARN("There is a delay between when the camera drivers publish the raw images and "
564  "when stereo_image_proc publishes the computed point cloud. stereo_view "
565  "may fail to synchronize these topics without a large queue_size.");
566  }
567 
568  std::string transport = argc > 1 ? argv[1] : "raw";
569  StereoView view(transport);
570 
571  ros::spin();
572  return 0;
573 }
message_filters::Synchronizer< ExactPolicy > ExactSync
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
message_filters::Synchronizer< ApproximatePolicy > ApproximateSync
std::string getTopic() const
std::string filename
ros::WallTimer check_synced_timer_
boost::mutex image_mutex_
image_transport::SubscriberFilter right_sub_
std::string resolveName(const std::string &name, bool remap=true) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
void imageCb(const ImageConstPtr &left, const ImageConstPtr &right, const DisparityImageConstPtr &disparity_msg)
ExactTime< Image, Image, DisparityImage > ExactPolicy
ROSCPP_DECL std::string clean(const std::string &name)
message_filters::Subscriber< DisparityImage > disparity_sub_
cv::Mat last_left_image_
static unsigned char colormap[768]
Definition: stereo_view.cpp:67
#define ROS_WARN(...)
ApproximateTime< Image, Image, DisparityImage > ApproximatePolicy
cv::Mat_< cv::Vec3b > disparity_color_
ROSCPP_DECL void spin(Spinner &spinner)
ROSCPP_DECL std::string remap(const std::string &name)
boost::shared_ptr< ExactSync > exact_sync_
cv::Mat last_right_image_
#define ROS_WARN_ONCE(...)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
int right_received_
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
StereoView(const std::string &transport)
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
boost::shared_ptr< ApproximateSync > approximate_sync_
void saveImage(const char *prefix, const cv::Mat &image)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
static void mouseCb(int event, int x, int y, int flags, void *param)
ROSCPP_DECL void shutdown()
boost::format filename_format_
int main(int argc, char **argv)
#define ROS_ERROR(...)
std::string getTopic() const
void increment(int *value)
ImageConstPtr last_right_msg_
Connection registerCallback(const C &callback)
void checkInputsSynchronized()


image_view
Author(s): Patrick Mihelich
autogenerated on Fri May 3 2019 02:59:57