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


stereo_image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Tue Sep 19 2017 02:56:25