point_cloud_xyzi_radial.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 <ros/ros.h>
00035 #include <nodelet/nodelet.h>
00036 #include <image_transport/image_transport.h>
00037 #include <sensor_msgs/image_encodings.h>
00038 #include <image_transport/subscriber_filter.h>
00039 #include <message_filters/subscriber.h>
00040 #include <message_filters/synchronizer.h>
00041 #include <message_filters/sync_policies/exact_time.h>
00042 #include <image_geometry/pinhole_camera_model.h>
00043 #include <boost/thread.hpp>
00044 #include <depth_image_proc/depth_traits.h>
00045 
00046 #include <sensor_msgs/point_cloud2_iterator.h>
00047 
00048 namespace depth_image_proc {
00049 
00050     using namespace message_filters::sync_policies;
00051     namespace enc = sensor_msgs::image_encodings;
00052     typedef ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicy;
00053 
00054     class PointCloudXyziRadialNodelet : public nodelet::Nodelet
00055     {
00056         ros::NodeHandlePtr intensity_nh_;
00057 
00058         // Subscriptions
00059         boost::shared_ptr<image_transport::ImageTransport> intensity_it_, depth_it_;
00060         image_transport::SubscriberFilter sub_depth_, sub_intensity_;
00061         message_filters::Subscriber<sensor_msgs::CameraInfo> sub_info_;
00062 
00063         int queue_size_;
00064 
00065         // Publications
00066         boost::mutex connect_mutex_;
00067         typedef sensor_msgs::PointCloud2 PointCloud;
00068         ros::Publisher pub_point_cloud_;
00069 
00070         
00071         typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
00072         boost::shared_ptr<Synchronizer> sync_;
00073 
00074         std::vector<double> D_;
00075         boost::array<double, 9> K_;
00076   
00077         int width_;
00078         int height_;
00079 
00080         cv::Mat transform_;
00081   
00082         virtual void onInit();
00083 
00084         void connectCb();
00085 
00086         void imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
00087                      const sensor_msgs::ImageConstPtr& intensity_msg_in,
00088                      const sensor_msgs::CameraInfoConstPtr& info_msg);
00089 
00090         // Handles float or uint16 depths
00091         template<typename T>
00092         void convert_depth(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg);
00093 
00094         template<typename T>
00095         void convert_intensity(const sensor_msgs::ImageConstPtr &inten_msg, PointCloud::Ptr& cloud_msg);
00096 
00097         cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial);
00098 
00099     };
00100 
00101     cv::Mat PointCloudXyziRadialNodelet::initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial)
00102     {
00103         int i,j;
00104         int totalsize = width*height;
00105         cv::Mat pixelVectors(1,totalsize,CV_32FC3);
00106         cv::Mat dst(1,totalsize,CV_32FC3);
00107 
00108         cv::Mat sensorPoints(cv::Size(height,width), CV_32FC2);
00109         cv::Mat undistortedSensorPoints(1,totalsize, CV_32FC2);
00110 
00111         std::vector<cv::Mat> ch;
00112         for(j = 0; j < height; j++)
00113         {
00114             for(i = 0; i < width; i++)
00115             {
00116                 cv::Vec2f &p = sensorPoints.at<cv::Vec2f>(i,j);
00117                 p[0] = i;
00118                 p[1] = j;
00119             }
00120         }
00121 
00122         sensorPoints = sensorPoints.reshape(2,1);
00123 
00124         cv::undistortPoints(sensorPoints, undistortedSensorPoints, cameraMatrix, distCoeffs);
00125 
00126         ch.push_back(undistortedSensorPoints);
00127         ch.push_back(cv::Mat::ones(1,totalsize,CV_32FC1));
00128         cv::merge(ch,pixelVectors);
00129 
00130         if(radial)
00131         {
00132             for(i = 0; i < totalsize; i++)
00133             {
00134                 normalize(pixelVectors.at<cv::Vec3f>(i),
00135                           dst.at<cv::Vec3f>(i));
00136             }
00137             pixelVectors = dst;
00138         }
00139         return pixelVectors.reshape(3,width);
00140     }
00141   
00142 
00143     void PointCloudXyziRadialNodelet::onInit()
00144     {
00145         ros::NodeHandle& nh         = getNodeHandle();
00146         ros::NodeHandle& private_nh = getPrivateNodeHandle();
00147 
00148         intensity_nh_.reset( new ros::NodeHandle(nh, "intensity") );
00149         ros::NodeHandle depth_nh(nh, "depth");
00150         intensity_it_  .reset( new image_transport::ImageTransport(*intensity_nh_) );
00151         depth_it_.reset( new image_transport::ImageTransport(depth_nh) );
00152 
00153         // Read parameters
00154         private_nh.param("queue_size", queue_size_, 5);
00155 
00156         // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
00157         sync_.reset( new Synchronizer(SyncPolicy(queue_size_), sub_depth_, sub_intensity_, sub_info_) );
00158         sync_->registerCallback(boost::bind(&PointCloudXyziRadialNodelet::imageCb, this, _1, _2, _3));
00159     
00160         // Monitor whether anyone is subscribed to the output
00161         ros::SubscriberStatusCallback connect_cb = 
00162             boost::bind(&PointCloudXyziRadialNodelet::connectCb, this);
00163         // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
00164         boost::lock_guard<boost::mutex> lock(connect_mutex_);
00165         pub_point_cloud_ = nh.advertise<PointCloud>("points", 20, connect_cb, connect_cb);
00166     }
00167 
00168     // Handles (un)subscribing when clients (un)subscribe
00169     void PointCloudXyziRadialNodelet::connectCb()
00170     {
00171         boost::lock_guard<boost::mutex> lock(connect_mutex_);
00172 
00173         if (pub_point_cloud_.getNumSubscribers() == 0)
00174         {
00175             sub_depth_.unsubscribe();
00176             sub_intensity_.unsubscribe();
00177             sub_info_.unsubscribe();
00178         }
00179         else if (!sub_depth_.getSubscriber())
00180         {
00181             ros::NodeHandle& private_nh = getPrivateNodeHandle();
00182             // parameter for depth_image_transport hint
00183             std::string depth_image_transport_param = "depth_image_transport";
00184         
00185             // depth image can use different transport.(e.g. compressedDepth)
00186             image_transport::TransportHints depth_hints("raw",ros::TransportHints(), private_nh, depth_image_transport_param);
00187             sub_depth_.subscribe(*depth_it_, "image_raw",       5, depth_hints);
00188         
00189             // intensity uses normal ros transport hints.
00190             image_transport::TransportHints hints("raw", ros::TransportHints(), private_nh);
00191             sub_intensity_.subscribe(*intensity_it_,   "image_raw", 5, hints);
00192             sub_info_.subscribe(*intensity_nh_,   "camera_info",      5);
00193         }
00194     }
00195 
00196     void PointCloudXyziRadialNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
00197                                               const sensor_msgs::ImageConstPtr& intensity_msg,
00198                                               const sensor_msgs::CameraInfoConstPtr& info_msg)
00199     {
00200         PointCloud::Ptr cloud_msg(new PointCloud);
00201         cloud_msg->header = depth_msg->header;
00202         cloud_msg->height = depth_msg->height;
00203         cloud_msg->width  = depth_msg->width;
00204         cloud_msg->is_dense = false;
00205         cloud_msg->is_bigendian = false;
00206 
00207         sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
00208         pcd_modifier.setPointCloud2Fields(4,
00209                                           "x", 1, sensor_msgs::PointField::FLOAT32,
00210                                           "y", 1, sensor_msgs::PointField::FLOAT32,
00211                                           "z", 1, sensor_msgs::PointField::FLOAT32,
00212                                           "intensity", 1, sensor_msgs::PointField::FLOAT32);
00213 
00214 
00215         if(info_msg->D != D_ || info_msg->K != K_ || width_ != info_msg->width ||
00216            height_ != info_msg->height)
00217         {
00218             D_ = info_msg->D;
00219             K_ = info_msg->K;
00220             width_ = info_msg->width;
00221             height_ = info_msg->height;
00222             transform_ = initMatrix(cv::Mat_<double>(3, 3, &K_[0]),cv::Mat(D_),width_,height_,true);
00223         }
00224 
00225         if (depth_msg->encoding == enc::TYPE_16UC1)
00226         {
00227             convert_depth<uint16_t>(depth_msg, cloud_msg);
00228         }
00229         else if (depth_msg->encoding == enc::TYPE_32FC1)
00230         {
00231             convert_depth<float>(depth_msg, cloud_msg);
00232         }
00233         else
00234         {
00235             NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
00236             return;
00237         }
00238 
00239         if(intensity_msg->encoding == enc::TYPE_16UC1)
00240         {
00241             convert_intensity<uint16_t>(intensity_msg, cloud_msg);
00242 
00243         }
00244         else if(intensity_msg->encoding == enc::MONO8)
00245         {
00246             convert_intensity<uint8_t>(intensity_msg, cloud_msg);
00247         }
00248         else
00249         {
00250             NODELET_ERROR_THROTTLE(5, "Intensity image has unsupported encoding [%s]", intensity_msg->encoding.c_str());
00251             return;
00252         }
00253 
00254         pub_point_cloud_.publish (cloud_msg);
00255     }
00256 
00257     template<typename T>
00258     void PointCloudXyziRadialNodelet::convert_depth(const sensor_msgs::ImageConstPtr& depth_msg,
00259                                                     PointCloud::Ptr& cloud_msg)
00260     {
00261         // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
00262         double unit_scaling = DepthTraits<T>::toMeters( T(1) );
00263         float bad_point = std::numeric_limits<float>::quiet_NaN();
00264 
00265         sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
00266         sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
00267         sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
00268         const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
00269     
00270         int row_step   = depth_msg->step / sizeof(T);
00271         for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step)
00272         {
00273             for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z)
00274             {
00275                 T depth = depth_row[u];
00276 
00277                 // Missing points denoted by NaNs
00278                 if (!DepthTraits<T>::valid(depth))
00279                 {
00280                     *iter_x = *iter_y = *iter_z = bad_point;
00281                     continue;
00282                 }
00283                 const cv::Vec3f &cvPoint = transform_.at<cv::Vec3f>(u,v) * DepthTraits<T>::toMeters(depth);
00284                 // Fill in XYZ
00285                 *iter_x = cvPoint(0);
00286                 *iter_y = cvPoint(1);
00287                 *iter_z = cvPoint(2);
00288             }
00289         }
00290     }
00291 
00292     template<typename T>
00293     void PointCloudXyziRadialNodelet::convert_intensity(const sensor_msgs::ImageConstPtr& intensity_msg,
00294                                                         PointCloud::Ptr& cloud_msg)
00295     {
00296         sensor_msgs::PointCloud2Iterator<float> iter_i(*cloud_msg, "intensity");
00297         const T* inten_row = reinterpret_cast<const T*>(&intensity_msg->data[0]);
00298 
00299         const int i_row_step = intensity_msg->step/sizeof(T);
00300         for (int v = 0; v < (int)cloud_msg->height; ++v, inten_row += i_row_step)
00301         {
00302             for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_i)
00303             {
00304                 *iter_i = inten_row[u];
00305             }
00306         }
00307     }
00308 
00309 } // namespace depth_image_proc
00310 
00311 // Register as nodelet
00312 #include <pluginlib/class_list_macros.h>
00313 PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyziRadialNodelet,nodelet::Nodelet);


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