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


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