stereo_view.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 #include <opencv2/highgui/highgui.hpp>
00036 
00037 #include <ros/ros.h>
00038 #include <sensor_msgs/Image.h>
00039 #include <sensor_msgs/image_encodings.h>
00040 #include <stereo_msgs/DisparityImage.h>
00041 #include <cv_bridge/cv_bridge.h>
00042 
00043 #include <message_filters/subscriber.h>
00044 #include <message_filters/synchronizer.h>
00045 #include <message_filters/sync_policies/exact_time.h>
00046 #include <message_filters/sync_policies/approximate_time.h>
00047 #include <image_transport/subscriber_filter.h>
00048 
00049 #include <boost/thread.hpp>
00050 #include <boost/format.hpp>
00051 
00052 #ifdef HAVE_GTK
00053 #include <gtk/gtk.h>
00054 
00055 // Platform-specific workaround for #3026: image_view doesn't close when
00056 // closing image window. On platforms using GTK+ we connect this to the
00057 // window's "destroy" event so that image_view exits.
00058 static void destroy(GtkWidget *widget, gpointer data)
00059 {
00060   ros::shutdown();
00061 }
00062 #endif
00063 
00064 namespace enc = sensor_msgs::image_encodings;
00065 
00066 // colormap for disparities, RGB
00067 static unsigned char colormap[768] = 
00068   { 150, 150, 150,
00069     107, 0, 12,
00070     106, 0, 18,
00071     105, 0, 24,
00072     103, 0, 30,
00073     102, 0, 36,
00074     101, 0, 42,
00075     99, 0, 48,
00076     98, 0, 54,
00077     97, 0, 60,
00078     96, 0, 66,
00079     94, 0, 72,
00080     93, 0, 78,
00081     92, 0, 84,
00082     91, 0, 90,
00083     89, 0, 96,
00084     88, 0, 102,
00085     87, 0, 108,
00086     85, 0, 114,
00087     84, 0, 120,
00088     83, 0, 126,
00089     82, 0, 131,
00090     80, 0, 137,
00091     79, 0, 143,
00092     78, 0, 149,
00093     77, 0, 155,
00094     75, 0, 161,
00095     74, 0, 167,
00096     73, 0, 173,
00097     71, 0, 179,
00098     70, 0, 185,
00099     69, 0, 191,
00100     68, 0, 197,
00101     66, 0, 203,
00102     65, 0, 209,
00103     64, 0, 215,
00104     62, 0, 221,
00105     61, 0, 227,
00106     60, 0, 233,
00107     59, 0, 239,
00108     57, 0, 245,
00109     56, 0, 251,
00110     55, 0, 255,
00111     54, 0, 255,
00112     52, 0, 255,
00113     51, 0, 255,
00114     50, 0, 255,
00115     48, 0, 255,
00116     47, 0, 255,
00117     46, 0, 255,
00118     45, 0, 255,
00119     43, 0, 255,
00120     42, 0, 255,
00121     41, 0, 255,
00122     40, 0, 255,
00123     38, 0, 255,
00124     37, 0, 255,
00125     36, 0, 255,
00126     34, 0, 255,
00127     33, 0, 255,
00128     32, 0, 255,
00129     31, 0, 255,
00130     29, 0, 255,
00131     28, 0, 255,
00132     27, 0, 255,
00133     26, 0, 255,
00134     24, 0, 255,
00135     23, 0, 255,
00136     22, 0, 255,
00137     20, 0, 255,
00138     19, 0, 255,
00139     18, 0, 255,
00140     17, 0, 255,
00141     15, 0, 255,
00142     14, 0, 255,
00143     13, 0, 255,
00144     11, 0, 255,
00145     10, 0, 255,
00146     9, 0, 255,
00147     8, 0, 255,
00148     6, 0, 255,
00149     5, 0, 255,
00150     4, 0, 255,
00151     3, 0, 255,
00152     1, 0, 255,
00153     0, 4, 255,
00154     0, 10, 255,
00155     0, 16, 255,
00156     0, 22, 255,
00157     0, 28, 255,
00158     0, 34, 255,
00159     0, 40, 255,
00160     0, 46, 255,
00161     0, 52, 255,
00162     0, 58, 255,
00163     0, 64, 255,
00164     0, 70, 255,
00165     0, 76, 255,
00166     0, 82, 255,
00167     0, 88, 255,
00168     0, 94, 255,
00169     0, 100, 255,
00170     0, 106, 255,
00171     0, 112, 255,
00172     0, 118, 255,
00173     0, 124, 255,
00174     0, 129, 255,
00175     0, 135, 255,
00176     0, 141, 255,
00177     0, 147, 255,
00178     0, 153, 255,
00179     0, 159, 255,
00180     0, 165, 255,
00181     0, 171, 255,
00182     0, 177, 255,
00183     0, 183, 255,
00184     0, 189, 255,
00185     0, 195, 255,
00186     0, 201, 255,
00187     0, 207, 255,
00188     0, 213, 255,
00189     0, 219, 255,
00190     0, 225, 255,
00191     0, 231, 255,
00192     0, 237, 255,
00193     0, 243, 255,
00194     0, 249, 255,
00195     0, 255, 255,
00196     0, 255, 249,
00197     0, 255, 243,
00198     0, 255, 237,
00199     0, 255, 231,
00200     0, 255, 225,
00201     0, 255, 219,
00202     0, 255, 213,
00203     0, 255, 207,
00204     0, 255, 201,
00205     0, 255, 195,
00206     0, 255, 189,
00207     0, 255, 183,
00208     0, 255, 177,
00209     0, 255, 171,
00210     0, 255, 165,
00211     0, 255, 159,
00212     0, 255, 153,
00213     0, 255, 147,
00214     0, 255, 141,
00215     0, 255, 135,
00216     0, 255, 129,
00217     0, 255, 124,
00218     0, 255, 118,
00219     0, 255, 112,
00220     0, 255, 106,
00221     0, 255, 100,
00222     0, 255, 94,
00223     0, 255, 88,
00224     0, 255, 82,
00225     0, 255, 76,
00226     0, 255, 70,
00227     0, 255, 64,
00228     0, 255, 58,
00229     0, 255, 52,
00230     0, 255, 46,
00231     0, 255, 40,
00232     0, 255, 34,
00233     0, 255, 28,
00234     0, 255, 22,
00235     0, 255, 16,
00236     0, 255, 10,
00237     0, 255, 4,
00238     2, 255, 0,
00239     8, 255, 0,
00240     14, 255, 0,
00241     20, 255, 0,
00242     26, 255, 0,
00243     32, 255, 0,
00244     38, 255, 0,
00245     44, 255, 0,
00246     50, 255, 0,
00247     56, 255, 0,
00248     62, 255, 0,
00249     68, 255, 0,
00250     74, 255, 0,
00251     80, 255, 0,
00252     86, 255, 0,
00253     92, 255, 0,
00254     98, 255, 0,
00255     104, 255, 0,
00256     110, 255, 0,
00257     116, 255, 0,
00258     122, 255, 0,
00259     128, 255, 0,
00260     133, 255, 0,
00261     139, 255, 0,
00262     145, 255, 0,
00263     151, 255, 0,
00264     157, 255, 0,
00265     163, 255, 0,
00266     169, 255, 0,
00267     175, 255, 0,
00268     181, 255, 0,
00269     187, 255, 0,
00270     193, 255, 0,
00271     199, 255, 0,
00272     205, 255, 0,
00273     211, 255, 0,
00274     217, 255, 0,
00275     223, 255, 0,
00276     229, 255, 0,
00277     235, 255, 0,
00278     241, 255, 0,
00279     247, 255, 0,
00280     253, 255, 0,
00281     255, 251, 0,
00282     255, 245, 0,
00283     255, 239, 0,
00284     255, 233, 0,
00285     255, 227, 0,
00286     255, 221, 0,
00287     255, 215, 0,
00288     255, 209, 0,
00289     255, 203, 0,
00290     255, 197, 0,
00291     255, 191, 0,
00292     255, 185, 0,
00293     255, 179, 0,
00294     255, 173, 0,
00295     255, 167, 0,
00296     255, 161, 0,
00297     255, 155, 0,
00298     255, 149, 0,
00299     255, 143, 0,
00300     255, 137, 0,
00301     255, 131, 0,
00302     255, 126, 0,
00303     255, 120, 0,
00304     255, 114, 0,
00305     255, 108, 0,
00306     255, 102, 0,
00307     255, 96, 0,
00308     255, 90, 0,
00309     255, 84, 0,
00310     255, 78, 0,
00311     255, 72, 0,
00312     255, 66, 0,
00313     255, 60, 0,
00314     255, 54, 0,
00315     255, 48, 0,
00316     255, 42, 0,
00317     255, 36, 0,
00318     255, 30, 0,
00319     255, 24, 0,
00320     255, 18, 0,
00321     255, 12, 0,
00322     255,  6, 0,
00323     255,  0, 0,
00324   };
00325 
00326 void increment(int* value)
00327 {
00328   ++(*value);
00329 }
00330 
00331 using namespace sensor_msgs;
00332 using namespace stereo_msgs;
00333 using namespace message_filters::sync_policies;
00334 
00335 // Note: StereoView is NOT nodelet-based, as it synchronizes the three streams.
00336 class StereoView
00337 {
00338 private:
00339   image_transport::SubscriberFilter left_sub_, right_sub_;
00340   message_filters::Subscriber<DisparityImage> disparity_sub_;
00341   typedef ExactTime<Image, Image, DisparityImage> ExactPolicy;
00342   typedef ApproximateTime<Image, Image, DisparityImage> ApproximatePolicy;
00343   typedef message_filters::Synchronizer<ExactPolicy> ExactSync;
00344   typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;
00345   boost::shared_ptr<ExactSync> exact_sync_;
00346   boost::shared_ptr<ApproximateSync> approximate_sync_;
00347   int queue_size_;
00348   
00349   ImageConstPtr last_left_msg_, last_right_msg_;
00350   cv::Mat last_left_image_, last_right_image_;
00351   cv::Mat_<cv::Vec3b> disparity_color_;
00352   boost::mutex image_mutex_;
00353   
00354   boost::format filename_format_;
00355   int save_count_;
00356 
00357   ros::WallTimer check_synced_timer_;
00358   int left_received_, right_received_, disp_received_, all_received_;
00359 
00360 public:
00361   StereoView(const std::string& transport)
00362     : filename_format_(""), save_count_(0),
00363       left_received_(0), right_received_(0), disp_received_(0), all_received_(0)
00364   {
00365     // Read local parameters
00366     ros::NodeHandle local_nh("~");
00367     bool autosize;
00368     local_nh.param("autosize", autosize, true);
00369     
00370     std::string format_string;
00371     local_nh.param("filename_format", format_string, std::string("%s%04i.jpg"));
00372     filename_format_.parse(format_string);
00373 
00374     // Do GUI window setup
00375     int flags = autosize ? CV_WINDOW_AUTOSIZE : 0;
00376     cv::namedWindow("left", flags);
00377     cv::namedWindow("right", flags);
00378     cv::namedWindow("disparity", flags);
00379     cvSetMouseCallback("left",      &StereoView::mouseCb, this);
00380     cvSetMouseCallback("right",     &StereoView::mouseCb, this);
00381     cvSetMouseCallback("disparity", &StereoView::mouseCb, this);
00382 #ifdef HAVE_GTK
00383     g_signal_connect(GTK_WIDGET( cvGetWindowHandle("left") ),
00384                      "destroy", G_CALLBACK(destroy), NULL);
00385     g_signal_connect(GTK_WIDGET( cvGetWindowHandle("right") ),
00386                      "destroy", G_CALLBACK(destroy), NULL);
00387     g_signal_connect(GTK_WIDGET( cvGetWindowHandle("disparity") ),
00388                      "destroy", G_CALLBACK(destroy), NULL);
00389 #endif
00390     cvStartWindowThread();
00391 
00392     // Resolve topic names
00393     ros::NodeHandle nh;
00394     std::string stereo_ns = nh.resolveName("stereo");
00395     std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
00396     std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image"));
00397     std::string disparity_topic = ros::names::clean(stereo_ns + "/disparity");
00398     ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s", left_topic.c_str(), right_topic.c_str(),
00399              disparity_topic.c_str());
00400 
00401     // Subscribe to three input topics.
00402     image_transport::ImageTransport it(nh);
00403     left_sub_.subscribe(it, left_topic, 1, transport);
00404     right_sub_.subscribe(it, right_topic, 1, transport);
00405     disparity_sub_.subscribe(nh, disparity_topic, 1);
00406 
00407     // Complain every 30s if the topics appear unsynchronized
00408     left_sub_.registerCallback(boost::bind(increment, &left_received_));
00409     right_sub_.registerCallback(boost::bind(increment, &right_received_));
00410     disparity_sub_.registerCallback(boost::bind(increment, &disp_received_));
00411     check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
00412                                              boost::bind(&StereoView::checkInputsSynchronized, this));
00413 
00414     // Synchronize input topics. Optionally do approximate synchronization.
00415     local_nh.param("queue_size", queue_size_, 5);
00416     bool approx;
00417     local_nh.param("approximate_sync", approx, false);
00418     if (approx)
00419     {
00420       approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size_),
00421                                                    left_sub_, right_sub_, disparity_sub_) );
00422       approximate_sync_->registerCallback(boost::bind(&StereoView::imageCb, this, _1, _2, _3));
00423     }
00424     else
00425     {
00426       exact_sync_.reset( new ExactSync(ExactPolicy(queue_size_),
00427                                        left_sub_, right_sub_, disparity_sub_) );
00428       exact_sync_->registerCallback(boost::bind(&StereoView::imageCb, this, _1, _2, _3));
00429     }
00430   }
00431 
00432   ~StereoView()
00433   {
00434     cvDestroyAllWindows();
00435   }
00436 
00437   void imageCb(const ImageConstPtr& left, const ImageConstPtr& right,
00438                const DisparityImageConstPtr& disparity_msg)
00439   {
00440     ++all_received_; // For error checking
00441     
00442     image_mutex_.lock();
00443 
00444     // May want to view raw bayer data
00445     if (left->encoding.find("bayer") != std::string::npos)
00446       boost::const_pointer_cast<Image>(left)->encoding = "mono8";
00447     if (right->encoding.find("bayer") != std::string::npos)
00448       boost::const_pointer_cast<Image>(right)->encoding = "mono8";
00449 
00450     // Hang on to image data for sake of mouseCb
00451     last_left_msg_ = left;
00452     last_right_msg_ = right;
00453     try {
00454       last_left_image_ = cv_bridge::toCvShare(left, "bgr8")->image;
00455       last_right_image_ = cv_bridge::toCvShare(right, "bgr8")->image;
00456     }
00457     catch (cv_bridge::Exception& e) {
00458       ROS_ERROR("Unable to convert one of '%s' or '%s' to 'bgr8'",
00459                 left->encoding.c_str(), right->encoding.c_str());
00460     }
00461 
00462     // Colormap and display the disparity image
00463     float min_disparity = disparity_msg->min_disparity;
00464     float max_disparity = disparity_msg->max_disparity;
00465     float multiplier = 255.0f / (max_disparity - min_disparity);
00466 
00467     assert(disparity_msg->image.encoding == enc::TYPE_32FC1);
00468     const cv::Mat_<float> dmat(disparity_msg->image.height, disparity_msg->image.width,
00469                                (float*)&disparity_msg->image.data[0], disparity_msg->image.step);
00470     disparity_color_.create(disparity_msg->image.height, disparity_msg->image.width);
00471     
00472     for (int row = 0; row < disparity_color_.rows; ++row) {
00473       const float* d = dmat[row];
00474       for (int col = 0; col < disparity_color_.cols; ++col) {
00475         int index = (d[col] - min_disparity) * multiplier + 0.5;
00476         index = std::min(255, std::max(0, index));
00477         // Fill as BGR
00478         disparity_color_(row, col)[2] = colormap[3*index + 0];
00479         disparity_color_(row, col)[1] = colormap[3*index + 1];
00480         disparity_color_(row, col)[0] = colormap[3*index + 2];
00481       }
00482     }
00483 
00484     // Must release the mutex before calling cv::imshow, or can deadlock against
00485     // OpenCV's window mutex.
00486     image_mutex_.unlock();
00487     if (!last_left_image_.empty())
00488       cv::imshow("left", last_left_image_);
00489     if (!last_right_image_.empty())
00490       cv::imshow("right", last_right_image_);
00491     cv::imshow("disparity", disparity_color_);
00492   }
00493 
00494   void saveImage(const char* prefix, const cv::Mat& image)
00495   {
00496     if (!image.empty()) {
00497       std::string filename = (filename_format_ % prefix % save_count_).str();
00498       cv::imwrite(filename, image);
00499       ROS_INFO("Saved image %s", filename.c_str());
00500     } else {
00501       ROS_WARN("Couldn't save %s image, no data!", prefix);
00502     }
00503   }
00504   
00505   static void mouseCb(int event, int x, int y, int flags, void* param)
00506   {
00507     if (event == CV_EVENT_LBUTTONDOWN)
00508     {
00509       ROS_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
00510       return;
00511     }
00512     if (event != CV_EVENT_RBUTTONDOWN)
00513       return;
00514     
00515     StereoView *sv = (StereoView*)param;
00516     boost::lock_guard<boost::mutex> guard(sv->image_mutex_);
00517 
00518     sv->saveImage("left",  sv->last_left_image_);
00519     sv->saveImage("right", sv->last_right_image_);
00520     sv->saveImage("disp",  sv->disparity_color_);
00521     sv->save_count_++;
00522   }
00523 
00524   void checkInputsSynchronized()
00525   {
00526     int threshold = 3 * all_received_;
00527     if (left_received_ >= threshold || right_received_ >= threshold || disp_received_ >= threshold) {
00528       ROS_WARN("[stereo_view] Low number of synchronized left/right/disparity triplets received.\n"
00529                "Left images received:      %d (topic '%s')\n"
00530                "Right images received:     %d (topic '%s')\n"
00531                "Disparity images received: %d (topic '%s')\n"
00532                "Synchronized triplets: %d\n"
00533                "Possible issues:\n"
00534                "\t* stereo_image_proc is not running.\n"
00535                "\t  Does `rosnode info %s` show any connections?\n"
00536                "\t* The cameras are not synchronized.\n"
00537                "\t  Try restarting stereo_view with parameter _approximate_sync:=True\n"
00538                "\t* The network is too slow. One or more images are dropped from each triplet.\n"
00539                "\t  Try restarting stereo_view, increasing parameter 'queue_size' (currently %d)",
00540                left_received_, left_sub_.getTopic().c_str(),
00541                right_received_, right_sub_.getTopic().c_str(),
00542                disp_received_, disparity_sub_.getTopic().c_str(),
00543                all_received_, ros::this_node::getName().c_str(), queue_size_);
00544     }
00545   }
00546 };
00547 
00548 int main(int argc, char **argv)
00549 {
00550   ros::init(argc, argv, "stereo_view", ros::init_options::AnonymousName);
00551   if (ros::names::remap("stereo") == "stereo") {
00552     ROS_WARN("'stereo' has not been remapped! Example command-line usage:\n"
00553              "\t$ rosrun image_view stereo_view stereo:=narrow_stereo image:=image_color");
00554   }
00555   if (ros::names::remap("image") == "/image_raw") {
00556     ROS_WARN("There is a delay between when the camera drivers publish the raw images and "
00557              "when stereo_image_proc publishes the computed point cloud. stereo_view "
00558              "may fail to synchronize these topics without a large queue_size.");
00559   }
00560 
00561   std::string transport = argc > 1 ? argv[1] : "raw";
00562   StereoView view(transport);
00563   
00564   ros::spin();
00565   return 0;
00566 }


image_view
Author(s): Patrick Mihelich
autogenerated on Mon Oct 6 2014 00:45:31