disparity.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 #include <boost/version.hpp>
00035 #if ((BOOST_VERSION / 100) % 1000) >= 53
00036 #include <boost/thread/lock_guard.hpp>
00037 #endif
00038 
00039 #include <ros/ros.h>
00040 #include <nodelet/nodelet.h>
00041 #include <image_transport/image_transport.h>
00042 #include <image_transport/subscriber_filter.h>
00043 #include <message_filters/subscriber.h>
00044 #include <message_filters/time_synchronizer.h>
00045 #include <sensor_msgs/image_encodings.h>
00046 #include <stereo_msgs/DisparityImage.h>
00047 #include "depth_traits.h"
00048 
00049 namespace depth_image_proc {
00050 
00051 namespace enc = sensor_msgs::image_encodings;
00052 
00053 class DisparityNodelet : public nodelet::Nodelet
00054 {
00055   boost::shared_ptr<image_transport::ImageTransport> left_it_;
00056   ros::NodeHandlePtr right_nh_;
00057   image_transport::SubscriberFilter sub_depth_image_;
00058   message_filters::Subscriber<sensor_msgs::CameraInfo> sub_info_;
00059   typedef message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo> Sync;
00060   boost::shared_ptr<Sync> sync_;
00061   
00062   boost::mutex connect_mutex_;
00063   ros::Publisher pub_disparity_;
00064   double min_range_;
00065   double max_range_;
00066   double delta_d_;
00067 
00068   virtual void onInit();
00069 
00070   void connectCb();
00071 
00072   void depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
00073                const sensor_msgs::CameraInfoConstPtr& info_msg);
00074 
00075   template<typename T>
00076   void convert(const sensor_msgs::ImageConstPtr& depth_msg,
00077                stereo_msgs::DisparityImagePtr& disp_msg);
00078 };
00079 
00080 void DisparityNodelet::onInit()
00081 {
00082   ros::NodeHandle &nh         = getNodeHandle();
00083   ros::NodeHandle &private_nh = getPrivateNodeHandle();
00084   ros::NodeHandle left_nh(nh, "left");
00085   left_it_.reset(new image_transport::ImageTransport(left_nh));
00086   right_nh_.reset( new ros::NodeHandle(nh, "right") );
00087 
00088   // Read parameters
00089   int queue_size;
00090   private_nh.param("queue_size", queue_size, 5);
00091   private_nh.param("min_range", min_range_, 0.0);
00092   private_nh.param("max_range", max_range_, std::numeric_limits<double>::infinity());
00093   private_nh.param("delta_d", delta_d_, 0.125);
00094 
00095   // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
00096   sync_.reset( new Sync(sub_depth_image_, sub_info_, queue_size) );
00097   sync_->registerCallback(boost::bind(&DisparityNodelet::depthCb, this, _1, _2));
00098 
00099   // Monitor whether anyone is subscribed to the output
00100   ros::SubscriberStatusCallback connect_cb = boost::bind(&DisparityNodelet::connectCb, this);
00101   // Make sure we don't enter connectCb() between advertising and assigning to pub_disparity_
00102   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00103   pub_disparity_ = left_nh.advertise<stereo_msgs::DisparityImage>("disparity", 1, connect_cb, connect_cb);
00104 }
00105 
00106 // Handles (un)subscribing when clients (un)subscribe
00107 void DisparityNodelet::connectCb()
00108 {
00109   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00110   if (pub_disparity_.getNumSubscribers() == 0)
00111   {
00112     sub_depth_image_.unsubscribe();
00113     sub_info_ .unsubscribe();
00114   }
00115   else if (!sub_depth_image_.getSubscriber())
00116   {
00117     image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00118     sub_depth_image_.subscribe(*left_it_, "image_rect", 1, hints);
00119     sub_info_.subscribe(*right_nh_, "camera_info", 1);
00120   }
00121 }
00122 
00123 void DisparityNodelet::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
00124                                const sensor_msgs::CameraInfoConstPtr& info_msg)
00125 {
00126   // Allocate new DisparityImage message
00127   stereo_msgs::DisparityImagePtr disp_msg( new stereo_msgs::DisparityImage );
00128   disp_msg->header         = depth_msg->header;
00129   disp_msg->image.header   = disp_msg->header;
00130   disp_msg->image.encoding = enc::TYPE_32FC1;
00131   disp_msg->image.height   = depth_msg->height;
00132   disp_msg->image.width    = depth_msg->width;
00133   disp_msg->image.step     = disp_msg->image.width * sizeof (float);
00134   disp_msg->image.data.resize( disp_msg->image.height * disp_msg->image.step, 0.0f );
00135   double fx = info_msg->P[0];
00136   disp_msg->T = -info_msg->P[3] / fx;
00137   disp_msg->f = fx;
00138   // Remaining fields depend on device characteristics, so rely on user input
00139   disp_msg->min_disparity = disp_msg->f * disp_msg->T / max_range_;
00140   disp_msg->max_disparity = disp_msg->f * disp_msg->T / min_range_;
00141   disp_msg->delta_d = delta_d_;
00142 
00143   if (depth_msg->encoding == enc::TYPE_16UC1)
00144   {
00145     convert<uint16_t>(depth_msg, disp_msg);
00146   }
00147   else if (depth_msg->encoding == enc::TYPE_32FC1)
00148   {
00149     convert<float>(depth_msg, disp_msg);
00150   }
00151   else
00152   {
00153     NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
00154     return;
00155   }
00156 
00157   pub_disparity_.publish(disp_msg);
00158 }
00159 
00160 template<typename T>
00161 void DisparityNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
00162                                stereo_msgs::DisparityImagePtr& disp_msg)
00163 {
00164   // For each depth Z, disparity d = fT / Z
00165   float unit_scaling = DepthTraits<T>::toMeters( T(1) );
00166   float constant = disp_msg->f * disp_msg->T / unit_scaling;
00167 
00168   const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
00169   int row_step = depth_msg->step / sizeof(T);
00170   float* disp_data = reinterpret_cast<float*>(&disp_msg->image.data[0]);
00171   for (int v = 0; v < (int)depth_msg->height; ++v)
00172   {
00173     for (int u = 0; u < (int)depth_msg->width; ++u)
00174     {
00175       T depth = depth_row[u];
00176       if (DepthTraits<T>::valid(depth))
00177         *disp_data = constant / depth;
00178       ++disp_data;
00179     }
00180 
00181     depth_row += row_step;
00182   }
00183 }
00184 
00185 } // namespace depth_image_proc
00186 
00187 // Register as nodelet
00188 #include <pluginlib/class_list_macros.h>
00189 PLUGINLIB_EXPORT_CLASS(depth_image_proc::DisparityNodelet,nodelet::Nodelet);


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Mon Oct 6 2014 00:46:03