point_cloud_xyzrgb.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 <sensor_msgs/image_encodings.h>
00048 #include <sensor_msgs/point_cloud2_iterator.h>
00049 #include <sensor_msgs/PointCloud2.h>
00050 #include <image_geometry/pinhole_camera_model.h>
00051 #include <depth_image_proc/depth_traits.h>
00052 #include <cv_bridge/cv_bridge.h>
00053 #include <opencv2/imgproc/imgproc.hpp>
00054 
00055 namespace depth_image_proc {
00056 
00057 using namespace message_filters::sync_policies;
00058 namespace enc = sensor_msgs::image_encodings;
00059 
00060 class PointCloudXyzrgbNodelet : public nodelet::Nodelet
00061 {
00062   ros::NodeHandlePtr rgb_nh_;
00063   boost::shared_ptr<image_transport::ImageTransport> rgb_it_, depth_it_;
00064   
00065   // Subscriptions
00066   image_transport::SubscriberFilter sub_depth_, sub_rgb_;
00067   message_filters::Subscriber<sensor_msgs::CameraInfo> sub_info_;
00068   typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicy;
00069   typedef ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> ExactSyncPolicy;
00070   typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
00071   typedef message_filters::Synchronizer<ExactSyncPolicy> ExactSynchronizer;
00072   boost::shared_ptr<Synchronizer> sync_;
00073   boost::shared_ptr<ExactSynchronizer> exact_sync_;
00074 
00075   // Publications
00076   boost::mutex connect_mutex_;
00077   typedef sensor_msgs::PointCloud2 PointCloud;
00078   ros::Publisher pub_point_cloud_;
00079 
00080   image_geometry::PinholeCameraModel model_;
00081 
00082   virtual void onInit();
00083 
00084   void connectCb();
00085 
00086   void imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
00087                const sensor_msgs::ImageConstPtr& rgb_msg,
00088                const sensor_msgs::CameraInfoConstPtr& info_msg);
00089 
00090   template<typename T>
00091   void convert(const sensor_msgs::ImageConstPtr& depth_msg,
00092                const sensor_msgs::ImageConstPtr& rgb_msg,
00093                const PointCloud::Ptr& cloud_msg,
00094                int red_offset, int green_offset, int blue_offset, int color_step);
00095 };
00096 
00097 void PointCloudXyzrgbNodelet::onInit()
00098 {
00099   ros::NodeHandle& nh         = getNodeHandle();
00100   ros::NodeHandle& private_nh = getPrivateNodeHandle();
00101   rgb_nh_.reset( new ros::NodeHandle(nh, "rgb") );
00102   ros::NodeHandle depth_nh(nh, "depth_registered");
00103   rgb_it_  .reset( new image_transport::ImageTransport(*rgb_nh_) );
00104   depth_it_.reset( new image_transport::ImageTransport(depth_nh) );
00105 
00106   // Read parameters
00107   int queue_size;
00108   private_nh.param("queue_size", queue_size, 5);
00109   bool use_exact_sync;
00110   private_nh.param("exact_sync", use_exact_sync, false);
00111 
00112   // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
00113   if (use_exact_sync)
00114   {
00115     exact_sync_.reset( new ExactSynchronizer(ExactSyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_) );
00116     exact_sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, _1, _2, _3));
00117   }
00118   else
00119   {
00120     sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_) );
00121     sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, _1, _2, _3));
00122   }
00123   
00124   // Monitor whether anyone is subscribed to the output
00125   ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzrgbNodelet::connectCb, this);
00126   // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
00127   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00128   pub_point_cloud_ = depth_nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
00129 }
00130 
00131 // Handles (un)subscribing when clients (un)subscribe
00132 void PointCloudXyzrgbNodelet::connectCb()
00133 {
00134   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00135   if (pub_point_cloud_.getNumSubscribers() == 0)
00136   {
00137     sub_depth_.unsubscribe();
00138     sub_rgb_  .unsubscribe();
00139     sub_info_ .unsubscribe();
00140   }
00141   else if (!sub_depth_.getSubscriber())
00142   {
00143     ros::NodeHandle& private_nh = getPrivateNodeHandle();
00144     // parameter for depth_image_transport hint
00145     std::string depth_image_transport_param = "depth_image_transport";
00146 
00147     // depth image can use different transport.(e.g. compressedDepth)
00148     image_transport::TransportHints depth_hints("raw",ros::TransportHints(), private_nh, depth_image_transport_param);
00149     sub_depth_.subscribe(*depth_it_, "image_rect",       1, depth_hints);
00150 
00151     // rgb uses normal ros transport hints.
00152     image_transport::TransportHints hints("raw", ros::TransportHints(), private_nh);
00153     sub_rgb_  .subscribe(*rgb_it_,   "image_rect_color", 1, hints);
00154     sub_info_ .subscribe(*rgb_nh_,   "camera_info",      1);
00155   }
00156 }
00157 
00158 void PointCloudXyzrgbNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
00159                                       const sensor_msgs::ImageConstPtr& rgb_msg_in,
00160                                       const sensor_msgs::CameraInfoConstPtr& info_msg)
00161 {
00162   // Check for bad inputs
00163   if (depth_msg->header.frame_id != rgb_msg_in->header.frame_id)
00164   {
00165     NODELET_ERROR_THROTTLE(5, "Depth image frame id [%s] doesn't match RGB image frame id [%s]",
00166                            depth_msg->header.frame_id.c_str(), rgb_msg_in->header.frame_id.c_str());
00167     return;
00168   }
00169 
00170   // Update camera model
00171   model_.fromCameraInfo(info_msg);
00172 
00173   // Check if the input image has to be resized
00174   sensor_msgs::ImageConstPtr rgb_msg = rgb_msg_in;
00175   if (depth_msg->width != rgb_msg->width || depth_msg->height != rgb_msg->height)
00176   {
00177     sensor_msgs::CameraInfo info_msg_tmp = *info_msg;
00178     info_msg_tmp.width = depth_msg->width;
00179     info_msg_tmp.height = depth_msg->height;
00180     float ratio = float(depth_msg->width)/float(rgb_msg->width);
00181     info_msg_tmp.K[0] *= ratio;
00182     info_msg_tmp.K[2] *= ratio;
00183     info_msg_tmp.K[4] *= ratio;
00184     info_msg_tmp.K[5] *= ratio;
00185     info_msg_tmp.P[0] *= ratio;
00186     info_msg_tmp.P[2] *= ratio;
00187     info_msg_tmp.P[5] *= ratio;
00188     info_msg_tmp.P[6] *= ratio;
00189     model_.fromCameraInfo(info_msg_tmp);
00190 
00191     cv_bridge::CvImageConstPtr cv_ptr;
00192     try
00193     {
00194       cv_ptr = cv_bridge::toCvShare(rgb_msg, rgb_msg->encoding);
00195     }
00196     catch (cv_bridge::Exception& e)
00197     {
00198       ROS_ERROR("cv_bridge exception: %s", e.what());
00199       return;
00200     }
00201     cv_bridge::CvImage cv_rsz;
00202     cv_rsz.header = cv_ptr->header;
00203     cv_rsz.encoding = cv_ptr->encoding;
00204     cv::resize(cv_ptr->image.rowRange(0,depth_msg->height/ratio), cv_rsz.image, cv::Size(depth_msg->width, depth_msg->height));
00205     if ((rgb_msg->encoding == enc::RGB8) || (rgb_msg->encoding == enc::BGR8) || (rgb_msg->encoding == enc::MONO8))
00206       rgb_msg = cv_rsz.toImageMsg();
00207     else
00208       rgb_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::RGB8)->toImageMsg();
00209 
00210     //NODELET_ERROR_THROTTLE(5, "Depth resolution (%ux%u) does not match RGB resolution (%ux%u)",
00211     //                       depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height);
00212     //return;
00213   } else
00214     rgb_msg = rgb_msg_in;
00215 
00216   // Supported color encodings: RGB8, BGR8, MONO8
00217   int red_offset, green_offset, blue_offset, color_step;
00218   if (rgb_msg->encoding == enc::RGB8)
00219   {
00220     red_offset   = 0;
00221     green_offset = 1;
00222     blue_offset  = 2;
00223     color_step   = 3;
00224   }
00225   else if (rgb_msg->encoding == enc::BGR8)
00226   {
00227     red_offset   = 2;
00228     green_offset = 1;
00229     blue_offset  = 0;
00230     color_step   = 3;
00231   }
00232   else if (rgb_msg->encoding == enc::MONO8)
00233   {
00234     red_offset   = 0;
00235     green_offset = 0;
00236     blue_offset  = 0;
00237     color_step   = 1;
00238   }
00239   else
00240   {
00241     try
00242     {
00243       rgb_msg = cv_bridge::toCvCopy(rgb_msg, enc::RGB8)->toImageMsg();
00244     }
00245     catch (cv_bridge::Exception& e)
00246     {
00247       NODELET_ERROR_THROTTLE(5, "Unsupported encoding [%s]: %s", rgb_msg->encoding.c_str(), e.what());
00248       return;
00249     }
00250     red_offset   = 0;
00251     green_offset = 1;
00252     blue_offset  = 2;
00253     color_step   = 3;
00254   }
00255 
00256   // Allocate new point cloud message
00257   PointCloud::Ptr cloud_msg (new PointCloud);
00258   cloud_msg->header = depth_msg->header; // Use depth image time stamp
00259   cloud_msg->height = depth_msg->height;
00260   cloud_msg->width  = depth_msg->width;
00261   cloud_msg->is_dense = false;
00262   cloud_msg->is_bigendian = false;
00263 
00264   sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
00265   pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
00266 
00267   if (depth_msg->encoding == enc::TYPE_16UC1)
00268   {
00269     convert<uint16_t>(depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
00270   }
00271   else if (depth_msg->encoding == enc::TYPE_32FC1)
00272   {
00273     convert<float>(depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
00274   }
00275   else
00276   {
00277     NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
00278     return;
00279   }
00280 
00281   pub_point_cloud_.publish (cloud_msg);
00282 }
00283 
00284 template<typename T>
00285 void PointCloudXyzrgbNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
00286                                       const sensor_msgs::ImageConstPtr& rgb_msg,
00287                                       const PointCloud::Ptr& cloud_msg,
00288                                       int red_offset, int green_offset, int blue_offset, int color_step)
00289 {
00290   // Use correct principal point from calibration
00291   float center_x = model_.cx();
00292   float center_y = model_.cy();
00293 
00294   // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
00295   double unit_scaling = DepthTraits<T>::toMeters( T(1) );
00296   float constant_x = unit_scaling / model_.fx();
00297   float constant_y = unit_scaling / model_.fy();
00298   float bad_point = std::numeric_limits<float>::quiet_NaN ();
00299   
00300   const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
00301   int row_step = depth_msg->step / sizeof(T);
00302   const uint8_t* rgb = &rgb_msg->data[0];
00303   int rgb_skip = rgb_msg->step - rgb_msg->width * color_step;
00304 
00305   sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
00306   sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
00307   sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
00308   sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*cloud_msg, "r");
00309   sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*cloud_msg, "g");
00310   sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*cloud_msg, "b");
00311   sensor_msgs::PointCloud2Iterator<uint8_t> iter_a(*cloud_msg, "a");
00312 
00313   for (int v = 0; v < int(cloud_msg->height); ++v, depth_row += row_step, rgb += rgb_skip)
00314   {
00315     for (int u = 0; u < int(cloud_msg->width); ++u, rgb += color_step, ++iter_x, ++iter_y, ++iter_z, ++iter_a, ++iter_r, ++iter_g, ++iter_b)
00316     {
00317       T depth = depth_row[u];
00318 
00319       // Check for invalid measurements
00320       if (!DepthTraits<T>::valid(depth))
00321       {
00322         *iter_x = *iter_y = *iter_z = bad_point;
00323       }
00324       else
00325       {
00326         // Fill in XYZ
00327         *iter_x = (u - center_x) * depth * constant_x;
00328         *iter_y = (v - center_y) * depth * constant_y;
00329         *iter_z = DepthTraits<T>::toMeters(depth);
00330       }
00331 
00332       // Fill in color
00333       *iter_a = 255;
00334       *iter_r = rgb[red_offset];
00335       *iter_g = rgb[green_offset];
00336       *iter_b = rgb[blue_offset];
00337     }
00338   }
00339 }
00340 
00341 } // namespace depth_image_proc
00342 
00343 // Register as nodelet
00344 #include <pluginlib/class_list_macros.h>
00345 PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyzrgbNodelet,nodelet::Nodelet);


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Wed May 1 2019 02:51:42