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