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 inline 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::WND_PROP_AUTOSIZE : 0;
00376     cv::namedWindow("left", flags);
00377     cv::namedWindow("right", flags);
00378     cv::namedWindow("disparity", flags);
00379     cv::setMouseCallback("left",      &StereoView::mouseCb, this);
00380     cv::setMouseCallback("right",     &StereoView::mouseCb, this);
00381     cv::setMouseCallback("disparity", &StereoView::mouseCb, this);
00382 #if CV_MAJOR_VERSION == 2
00383 #ifdef HAVE_GTK
00384     g_signal_connect(GTK_WIDGET( cvGetWindowHandle("left") ),
00385                      "destroy", G_CALLBACK(destroy), NULL);
00386     g_signal_connect(GTK_WIDGET( cvGetWindowHandle("right") ),
00387                      "destroy", G_CALLBACK(destroy), NULL);
00388     g_signal_connect(GTK_WIDGET( cvGetWindowHandle("disparity") ),
00389                      "destroy", G_CALLBACK(destroy), NULL);
00390 #endif
00391     cvStartWindowThread();
00392 #endif
00393 
00394     // Resolve topic names
00395     ros::NodeHandle nh;
00396     std::string stereo_ns = nh.resolveName("stereo");
00397     std::string left_topic = ros::names::clean(stereo_ns + "/left/" + nh.resolveName("image"));
00398     std::string right_topic = ros::names::clean(stereo_ns + "/right/" + nh.resolveName("image"));
00399     std::string disparity_topic = ros::names::clean(stereo_ns + "/disparity");
00400     ROS_INFO("Subscribing to:\n\t* %s\n\t* %s\n\t* %s", left_topic.c_str(), right_topic.c_str(),
00401              disparity_topic.c_str());
00402 
00403     // Subscribe to three input topics.
00404     image_transport::ImageTransport it(nh);
00405     left_sub_.subscribe(it, left_topic, 1, transport);
00406     right_sub_.subscribe(it, right_topic, 1, transport);
00407     disparity_sub_.subscribe(nh, disparity_topic, 1);
00408 
00409     // Complain every 30s if the topics appear unsynchronized
00410     left_sub_.registerCallback(boost::bind(increment, &left_received_));
00411     right_sub_.registerCallback(boost::bind(increment, &right_received_));
00412     disparity_sub_.registerCallback(boost::bind(increment, &disp_received_));
00413     check_synced_timer_ = nh.createWallTimer(ros::WallDuration(15.0),
00414                                              boost::bind(&StereoView::checkInputsSynchronized, this));
00415 
00416     // Synchronize input topics. Optionally do approximate synchronization.
00417     local_nh.param("queue_size", queue_size_, 5);
00418     bool approx;
00419     local_nh.param("approximate_sync", approx, false);
00420     if (approx)
00421     {
00422       approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size_),
00423                                                    left_sub_, right_sub_, disparity_sub_) );
00424       approximate_sync_->registerCallback(boost::bind(&StereoView::imageCb, this, _1, _2, _3));
00425     }
00426     else
00427     {
00428       exact_sync_.reset( new ExactSync(ExactPolicy(queue_size_),
00429                                        left_sub_, right_sub_, disparity_sub_) );
00430       exact_sync_->registerCallback(boost::bind(&StereoView::imageCb, this, _1, _2, _3));
00431     }
00432   }
00433 
00434   ~StereoView()
00435   {
00436     cv::destroyAllWindows();
00437   }
00438 
00439   void imageCb(const ImageConstPtr& left, const ImageConstPtr& right,
00440                const DisparityImageConstPtr& disparity_msg)
00441   {
00442     ++all_received_; // For error checking
00443     
00444     image_mutex_.lock();
00445 
00446     // May want to view raw bayer data
00447     if (left->encoding.find("bayer") != std::string::npos)
00448       boost::const_pointer_cast<Image>(left)->encoding = "mono8";
00449     if (right->encoding.find("bayer") != std::string::npos)
00450       boost::const_pointer_cast<Image>(right)->encoding = "mono8";
00451 
00452     // Hang on to image data for sake of mouseCb
00453     last_left_msg_ = left;
00454     last_right_msg_ = right;
00455     try {
00456       last_left_image_ = cv_bridge::toCvShare(left, "bgr8")->image;
00457       last_right_image_ = cv_bridge::toCvShare(right, "bgr8")->image;
00458     }
00459     catch (cv_bridge::Exception& e) {
00460       ROS_ERROR("Unable to convert one of '%s' or '%s' to 'bgr8'",
00461                 left->encoding.c_str(), right->encoding.c_str());
00462     }
00463 
00464     // Colormap and display the disparity image
00465     float min_disparity = disparity_msg->min_disparity;
00466     float max_disparity = disparity_msg->max_disparity;
00467     float multiplier = 255.0f / (max_disparity - min_disparity);
00468 
00469     assert(disparity_msg->image.encoding == enc::TYPE_32FC1);
00470     const cv::Mat_<float> dmat(disparity_msg->image.height, disparity_msg->image.width,
00471                                (float*)&disparity_msg->image.data[0], disparity_msg->image.step);
00472     disparity_color_.create(disparity_msg->image.height, disparity_msg->image.width);
00473     
00474     for (int row = 0; row < disparity_color_.rows; ++row) {
00475       const float* d = dmat[row];
00476       for (int col = 0; col < disparity_color_.cols; ++col) {
00477         int index = (d[col] - min_disparity) * multiplier + 0.5;
00478         index = std::min(255, std::max(0, index));
00479         // Fill as BGR
00480         disparity_color_(row, col)[2] = colormap[3*index + 0];
00481         disparity_color_(row, col)[1] = colormap[3*index + 1];
00482         disparity_color_(row, col)[0] = colormap[3*index + 2];
00483       }
00484     }
00485 
00486     // Must release the mutex before calling cv::imshow, or can deadlock against
00487     // OpenCV's window mutex.
00488     image_mutex_.unlock();
00489     if (!last_left_image_.empty()) {
00490       cv::imshow("left", last_left_image_);
00491       cv::waitKey(1);
00492     }
00493     if (!last_right_image_.empty()) {
00494       cv::imshow("right", last_right_image_);
00495       cv::waitKey(1);
00496     }
00497     cv::imshow("disparity", disparity_color_);
00498     cv::waitKey(1);
00499   }
00500 
00501   void saveImage(const char* prefix, const cv::Mat& image)
00502   {
00503     if (!image.empty()) {
00504       std::string filename = (filename_format_ % prefix % save_count_).str();
00505       cv::imwrite(filename, image);
00506       ROS_INFO("Saved image %s", filename.c_str());
00507     } else {
00508       ROS_WARN("Couldn't save %s image, no data!", prefix);
00509     }
00510   }
00511   
00512   static void mouseCb(int event, int x, int y, int flags, void* param)
00513   {
00514     if (event == cv::EVENT_LBUTTONDOWN)
00515     {
00516       ROS_WARN_ONCE("Left-clicking no longer saves images. Right-click instead.");
00517       return;
00518     }
00519     if (event != cv::EVENT_RBUTTONDOWN)
00520       return;
00521     
00522     StereoView *sv = (StereoView*)param;
00523     boost::lock_guard<boost::mutex> guard(sv->image_mutex_);
00524 
00525     sv->saveImage("left",  sv->last_left_image_);
00526     sv->saveImage("right", sv->last_right_image_);
00527     sv->saveImage("disp",  sv->disparity_color_);
00528     sv->save_count_++;
00529   }
00530 
00531   void checkInputsSynchronized()
00532   {
00533     int threshold = 3 * all_received_;
00534     if (left_received_ >= threshold || right_received_ >= threshold || disp_received_ >= threshold) {
00535       ROS_WARN("[stereo_view] Low number of synchronized left/right/disparity triplets received.\n"
00536                "Left images received:      %d (topic '%s')\n"
00537                "Right images received:     %d (topic '%s')\n"
00538                "Disparity images received: %d (topic '%s')\n"
00539                "Synchronized triplets: %d\n"
00540                "Possible issues:\n"
00541                "\t* stereo_image_proc is not running.\n"
00542                "\t  Does `rosnode info %s` show any connections?\n"
00543                "\t* The cameras are not synchronized.\n"
00544                "\t  Try restarting stereo_view with parameter _approximate_sync:=True\n"
00545                "\t* The network is too slow. One or more images are dropped from each triplet.\n"
00546                "\t  Try restarting stereo_view, increasing parameter 'queue_size' (currently %d)",
00547                left_received_, left_sub_.getTopic().c_str(),
00548                right_received_, right_sub_.getTopic().c_str(),
00549                disp_received_, disparity_sub_.getTopic().c_str(),
00550                all_received_, ros::this_node::getName().c_str(), queue_size_);
00551     }
00552   }
00553 };
00554 
00555 int main(int argc, char **argv)
00556 {
00557   ros::init(argc, argv, "stereo_view", ros::init_options::AnonymousName);
00558   if (ros::names::remap("stereo") == "stereo") {
00559     ROS_WARN("'stereo' has not been remapped! Example command-line usage:\n"
00560              "\t$ rosrun image_view stereo_view stereo:=narrow_stereo image:=image_color");
00561   }
00562   if (ros::names::remap("image") == "/image_raw") {
00563     ROS_WARN("There is a delay between when the camera drivers publish the raw images and "
00564              "when stereo_image_proc publishes the computed point cloud. stereo_view "
00565              "may fail to synchronize these topics without a large queue_size.");
00566   }
00567 
00568   std::string transport = argc > 1 ? argv[1] : "raw";
00569   StereoView view(transport);
00570   
00571   ros::spin();
00572   return 0;
00573 }


image_view
Author(s): Patrick Mihelich
autogenerated on Tue Sep 19 2017 02:56:22