disparity_view.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2009, Dejan Pangercic <pangercic@cs.tum.edu>
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "cv_bridge/CvBridge.h" // CvBridge converts ros image_msgs to OpenCV images.
00031 #include "opencv/cxcore.h" // OpenCV
00032 #include "opencv/cv.h" // OpenCV
00033 #include "opencv/highgui.h" // OpenCV's gui functions
00034 #include "ros/node_handle.h"
00035 #include "sensor_msgs/Image.h"
00036 //#include "topic_synchronizer/topic_synchronizer.h" // Synchronizes multiple messages so they arrive at the same time.
00037 //#include <boost/thread.hpp>
00038 
00039 class DisparityViewer
00040 {
00041 private:
00042   // Images in ROS
00043   sensor_msgs::ImageConstPtr left_image_ptr_;
00044   sensor_msgs::ImageConstPtr disparity_image_ptr_;
00045   
00046   // Converter from ROS images to OpenCV images
00047   sensor_msgs::CvBridge left_bridge_;
00048   sensor_msgs::CvBridge disparity_bridge_;
00049   
00050   // Node handle and subscribers
00051   ros::NodeHandle nh_;
00052   ros::Subscriber left_image_sub_;
00053   ros::Subscriber disparity_image_sub_;
00054   //TopicSynchronizer sync_; // Synchronizes the images
00055   
00056   // Image mutices
00057   //boost::mutex left_image_mutex_, disparity_image_mutex_;
00058   
00059 public:
00060   DisparityViewer(const ros::NodeHandle& node_handle)
00061     : nh_(node_handle)
00062   {
00063     // Create OpenCV gui windows for the images.
00064     cvNamedWindow("Left");
00065     cvNamedWindow("Disparity");
00066     
00067     // Initialize the image synchronization
00068     //sync_(&DisparityViewer::imageCallback, this);
00069     // Subscribe to the images
00070     left_image_sub_ = nh_.subscribe("/stereo/left/image", 1, &DisparityViewer::leftImageCallback,this );
00071     disparity_image_sub_ = nh_.subscribe("/stereo/disparity", 1, &DisparityViewer::disparityImageCallback,this );
00072   }
00073   
00074   ~DisparityViewer()
00075   {
00076     // Destroy all of the OpenCV windows
00077     cvDestroyWindow("Left");
00078     cvDestroyWindow("Disparity");
00079   }
00080   
00081   // Left image callback. Keep a pointer to the left image.
00082   void leftImageCallback(const sensor_msgs::ImageConstPtr& image_ptr)
00083   {
00084     IplImage *left_iplimage;
00085     //boost::mutex::scoped_lock lock(left_image_mutex_);
00086     left_image_ptr_ =  image_ptr;
00087     if (left_bridge_.fromImage(*left_image_ptr_)) {
00088       ROS_INFO("left image\n");
00089       left_iplimage = left_bridge_.toIpl();
00090     }
00091     else {
00092       ROS_INFO("no left image\n");
00093       return;
00094     }
00095 
00096     // Use OpenCV's HighGUI to display the images.
00097     cvShowImage("Left", left_iplimage);
00098 
00099     cvWaitKey(2);
00100   }
00101   
00102   // Disparity image callback. Keep a pointer to the disparity image.
00103   void disparityImageCallback(const sensor_msgs::ImageConstPtr& image_ptr)
00104   {
00105     IplImage *disparity_iplimage;
00106     //boost::mutex::scoped_lock lock(disparity_image_mutex_);
00107     disparity_image_ptr_ = image_ptr;
00108 
00109     if (disparity_bridge_.fromImage(*disparity_image_ptr_)) {
00110       ROS_INFO("disp image\n");
00111       disparity_iplimage = disparity_bridge_.toIpl();
00112     }
00113     else {
00114       ROS_INFO("no disp image\n");
00115       return;
00116     }
00117     
00118     // Hack: convert the 16-bit disparity image to an 8-bit image so OpenCV can display it.
00119     IplImage *disparity_out = cvCreateImage(cvGetSize(disparity_iplimage), IPL_DEPTH_8U, 1);
00120     cvCvtScale(disparity_iplimage, disparity_out, 4.0/16.0);
00121     cvShowImage("Disparity", disparity_out);
00122     //cvShowImage("Disparity", disparity_iplimage);
00123     //
00124     cvReleaseImage(&disparity_out);
00125 
00126     cvWaitKey(4);
00127   }
00128   
00129   // Synchronized image callback. Displays the images.
00130   void imageCallback()
00131   {
00132     IplImage *left_iplimage, *disparity_iplimage;
00133 
00134     //boost::mutex::scoped_lock lock(left_image_mutex_);
00135     //boost::mutex::scoped_lock lock(disparity_image_mutex_);
00136     
00137     // Use CvBridge to get OpenCV versions of the images
00138     if (left_bridge_.fromImage(*left_image_ptr_,"bgr") && disparity_bridge_.fromImage(*disparity_image_ptr_)) {
00139       left_iplimage = left_bridge_.toIpl();
00140       disparity_iplimage = disparity_bridge_.toIpl();
00141     }
00142     else {
00143       return;
00144     }
00145     
00146     // Use OpenCV's HighGUI to display the images.
00147     cvShowImage("Left", left_iplimage);
00148     
00149     // Hack: convert the 16-bit disparity image to an 8-bit image so OpenCV can display it.
00150     IplImage *disparity_out = cvCreateImage(cvGetSize(disparity_iplimage), IPL_DEPTH_8U, 1);
00151     cvCvtScale(disparity_iplimage, disparity_out, 4.0/16.0);
00152     cvShowImage("Disparity", disparity_iplimage);
00153     
00154     cvWaitKey(2);
00155     
00156     cvReleaseImage(&disparity_out);
00157   }
00158   
00159 };  // end class
00160 
00161 int main(int argc, char **argv)
00162 {
00163   //sleep(10000);
00164 
00165   ros::init(argc, argv, "disparity_viewer");
00166   ros::NodeHandle n;
00167   DisparityViewer viewer(n);
00168   ros::spin();
00169   return 0;
00170 }
00171 
00172 


projected_light
Author(s): Dejan Pangercic, Andreas Leha
autogenerated on Mon Oct 6 2014 09:24:44