point_cloud.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/synchronizer.h>
00045 #include <message_filters/sync_policies/exact_time.h>
00046 #include <message_filters/sync_policies/approximate_time.h>
00047 #include <image_geometry/stereo_camera_model.h>
00048 
00049 #include <stereo_msgs/DisparityImage.h>
00050 #include <sensor_msgs/PointCloud.h>
00051 #include <sensor_msgs/image_encodings.h>
00052 
00053 namespace stereo_image_proc {
00054 
00055 using namespace sensor_msgs;
00056 using namespace stereo_msgs;
00057 using namespace message_filters::sync_policies;
00058 
00059 class PointCloudNodelet : public nodelet::Nodelet
00060 {
00061   boost::shared_ptr<image_transport::ImageTransport> it_;
00062 
00063   // Subscriptions
00064   image_transport::SubscriberFilter sub_l_image_;
00065   message_filters::Subscriber<CameraInfo> sub_l_info_, sub_r_info_;
00066   message_filters::Subscriber<DisparityImage> sub_disparity_;
00067   typedef ExactTime<Image, CameraInfo, CameraInfo, DisparityImage> ExactPolicy;
00068   typedef ApproximateTime<Image, CameraInfo, CameraInfo, DisparityImage> ApproximatePolicy;
00069   typedef message_filters::Synchronizer<ExactPolicy> ExactSync;
00070   typedef message_filters::Synchronizer<ApproximatePolicy> ApproximateSync;
00071   boost::shared_ptr<ExactSync> exact_sync_;
00072   boost::shared_ptr<ApproximateSync> approximate_sync_;
00073 
00074   // Publications
00075   boost::mutex connect_mutex_;
00076   ros::Publisher pub_points_;
00077 
00078   // Processing state (note: only safe because we're single-threaded!)
00079   image_geometry::StereoCameraModel model_;
00080   cv::Mat_<cv::Vec3f> points_mat_; // scratch buffer
00081 
00082   virtual void onInit();
00083 
00084   void connectCb();
00085 
00086   void imageCb(const ImageConstPtr& l_image_msg,
00087                const CameraInfoConstPtr& l_info_msg,
00088                const CameraInfoConstPtr& r_info_msg,
00089                const DisparityImageConstPtr& disp_msg);
00090 };
00091 
00092 void PointCloudNodelet::onInit()
00093 {
00094   ros::NodeHandle &nh = getNodeHandle();
00095   ros::NodeHandle &private_nh = getPrivateNodeHandle();
00096   it_.reset(new image_transport::ImageTransport(nh));
00097 
00098   // Synchronize inputs. Topic subscriptions happen on demand in the connection
00099   // callback. Optionally do approximate synchronization.
00100   int queue_size;
00101   private_nh.param("queue_size", queue_size, 5);
00102   bool approx;
00103   private_nh.param("approximate_sync", approx, false);
00104   if (approx)
00105   {
00106     approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size),
00107                                                  sub_l_image_, sub_l_info_,
00108                                                  sub_r_info_, sub_disparity_) );
00109     approximate_sync_->registerCallback(boost::bind(&PointCloudNodelet::imageCb,
00110                                                     this, _1, _2, _3, _4));
00111   }
00112   else
00113   {
00114     exact_sync_.reset( new ExactSync(ExactPolicy(queue_size),
00115                                      sub_l_image_, sub_l_info_,
00116                                      sub_r_info_, sub_disparity_) );
00117     exact_sync_->registerCallback(boost::bind(&PointCloudNodelet::imageCb,
00118                                               this, _1, _2, _3, _4));
00119   }
00120 
00121   // Monitor whether anyone is subscribed to the output
00122   ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudNodelet::connectCb, this);
00123   // Make sure we don't enter connectCb() between advertising and assigning to pub_points_
00124   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00125   pub_points_ = nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
00126 }
00127 
00128 // Handles (un)subscribing when clients (un)subscribe
00129 void PointCloudNodelet::connectCb()
00130 {
00131   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00132   if (pub_points_.getNumSubscribers() == 0)
00133   {
00134     sub_l_image_  .unsubscribe();
00135     sub_l_info_   .unsubscribe();
00136     sub_r_info_   .unsubscribe();
00137     sub_disparity_.unsubscribe();
00138   }
00139   else if (!sub_l_image_.getSubscriber())
00140   {
00141     ros::NodeHandle &nh = getNodeHandle();
00142     // Queue size 1 should be OK; the one that matters is the synchronizer queue size.
00143     image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00144     sub_l_image_  .subscribe(*it_, "left/image_rect_color", 1, hints);
00145     sub_l_info_   .subscribe(nh,   "left/camera_info", 1);
00146     sub_r_info_   .subscribe(nh,   "right/camera_info", 1);
00147     sub_disparity_.subscribe(nh,   "disparity", 1);
00148   }
00149 }
00150 
00151 inline bool isValidPoint(const cv::Vec3f& pt)
00152 {
00153   // Check both for disparities explicitly marked as invalid (where OpenCV maps pt.z to MISSING_Z)
00154   // and zero disparities (point mapped to infinity).
00155   return pt[2] != image_geometry::StereoCameraModel::MISSING_Z && !std::isinf(pt[2]);
00156 }
00157 
00158 void PointCloudNodelet::imageCb(const ImageConstPtr& l_image_msg,
00159                                 const CameraInfoConstPtr& l_info_msg,
00160                                 const CameraInfoConstPtr& r_info_msg,
00161                                 const DisparityImageConstPtr& disp_msg)
00162 {
00163   // Update the camera model
00164   model_.fromCameraInfo(l_info_msg, r_info_msg);
00165 
00166   // Calculate point cloud
00167   const Image& dimage = disp_msg->image;
00168   const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
00169   model_.projectDisparityImageTo3d(dmat, points_mat_, true);
00170   cv::Mat_<cv::Vec3f> mat = points_mat_;
00171 
00172   // Fill in new PointCloud message (1D dense layout - no invalid points)
00173   PointCloudPtr points_msg = boost::make_shared<PointCloud>();
00174   points_msg->header = disp_msg->header;
00175 
00176   points_msg->channels.resize(3);
00177   points_msg->channels[0].name = "rgb";
00178   points_msg->channels[0].values.resize(0);
00179   points_msg->channels[1].name = "u";
00180   points_msg->channels[1].values.resize(0);
00181   points_msg->channels[2].name = "v";
00182   points_msg->channels[2].values.resize(0);
00183 
00184   // NOTE: For historical reasons, (u,v) = (row, column), the opposite of the
00185   // usual convention. Good thing we're moving to sensor_msgs/PointCloud2.
00186   for (int32_t u = 0; u < mat.rows; ++u) {
00187     for (int32_t v = 0; v < mat.cols; ++v) {
00188       if (isValidPoint(mat(u,v))) {
00189         // x,y,z
00190         geometry_msgs::Point32 pt;
00191         pt.x = mat(u,v)[0];
00192         pt.y = mat(u,v)[1];
00193         pt.z = mat(u,v)[2];
00194         points_msg->points.push_back(pt);
00195         // u,v
00196         points_msg->channels[1].values.push_back(u);
00197         points_msg->channels[2].values.push_back(v);
00198       }
00199     }
00200   }
00201 
00202   // Fill in color
00203   namespace enc = sensor_msgs::image_encodings;
00204   const std::string& encoding = l_image_msg->encoding;
00205   points_msg->channels[0].values.reserve(points_msg->points.size());
00206   if (encoding == enc::MONO8) {
00207     const cv::Mat_<uint8_t> color(l_image_msg->height, l_image_msg->width,
00208                                   (uint8_t*)&l_image_msg->data[0],
00209                                   l_image_msg->step);
00210     for (int32_t u = 0; u < mat.rows; ++u) {
00211       for (int32_t v = 0; v < mat.cols; ++v) {
00212         if (isValidPoint(mat(u,v))) {
00213           uint8_t g = color(u,v);
00214           int32_t rgb = (g << 16) | (g << 8) | g;
00215           points_msg->channels[0].values.push_back(*(float*)(&rgb));
00216         }
00217       }
00218     }
00219   }
00220   else if (encoding == enc::RGB8) {
00221     const cv::Mat_<cv::Vec3b> color(l_image_msg->height, l_image_msg->width,
00222                                     (cv::Vec3b*)&l_image_msg->data[0],
00223                                     l_image_msg->step);
00224     for (int32_t u = 0; u < mat.rows; ++u) {
00225       for (int32_t v = 0; v < mat.cols; ++v) {
00226         if (isValidPoint(mat(u,v))) {
00227           const cv::Vec3b& rgb = color(u,v);
00228           int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2];
00229           points_msg->channels[0].values.push_back(*(float*)(&rgb_packed));
00230         }
00231       }
00232     }
00233   }
00234   else if (encoding == enc::BGR8) {
00235     const cv::Mat_<cv::Vec3b> color(l_image_msg->height, l_image_msg->width,
00236                                     (cv::Vec3b*)&l_image_msg->data[0],
00237                                     l_image_msg->step);
00238     for (int32_t u = 0; u < mat.rows; ++u) {
00239       for (int32_t v = 0; v < mat.cols; ++v) {
00240         if (isValidPoint(mat(u,v))) {
00241           const cv::Vec3b& bgr = color(u,v);
00242           int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0];
00243           points_msg->channels[0].values.push_back(*(float*)(&rgb_packed));
00244         }
00245       }
00246     }
00247   }
00248   else {
00249     NODELET_WARN_THROTTLE(30, "Could not fill color channel of the point cloud, "
00250                           "unsupported encoding '%s'", encoding.c_str());
00251   }
00252 
00253   pub_points_.publish(points_msg);
00254 }
00255 
00256 } // namespace stereo_image_proc
00257 
00258 // Register nodelet
00259 #include <pluginlib/class_list_macros.h>
00260 PLUGINLIB_EXPORT_CLASS(stereo_image_proc::PointCloudNodelet,nodelet::Nodelet)


stereo_image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Wed Aug 26 2015 11:57:43