point_cloud_xyz_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_geometry/pinhole_camera_model.h>
00039 #include <boost/thread.hpp>
00040 #include <depth_image_proc/depth_traits.h>
00041 
00042 #include <sensor_msgs/point_cloud2_iterator.h>
00043 
00044 namespace depth_image_proc {
00045 
00046     namespace enc = sensor_msgs::image_encodings;
00047 
00048     class PointCloudXyzRadialNodelet : public nodelet::Nodelet
00049     {
00050         // Subscriptions
00051         boost::shared_ptr<image_transport::ImageTransport> it_;
00052         image_transport::CameraSubscriber sub_depth_;
00053         int queue_size_;
00054 
00055         // Publications
00056         boost::mutex connect_mutex_;
00057         typedef sensor_msgs::PointCloud2 PointCloud;
00058         ros::Publisher pub_point_cloud_;
00059 
00060         
00061         std::vector<double> D_;
00062         boost::array<double, 9> K_;
00063   
00064         int width_;
00065         int height_;
00066 
00067         cv::Mat binned;
00068   
00069         virtual void onInit();
00070 
00071         void connectCb();
00072 
00073         void depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
00074                      const sensor_msgs::CameraInfoConstPtr& info_msg);
00075 
00076         // Handles float or uint16 depths
00077         template<typename T>
00078         void convert(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg);
00079     };
00080 
00081     cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial)
00082     {
00083         int i,j;
00084         int totalsize = width*height;
00085         cv::Mat pixelVectors(1,totalsize,CV_32FC3);
00086         cv::Mat dst(1,totalsize,CV_32FC3);
00087 
00088         cv::Mat sensorPoints(cv::Size(height,width), CV_32FC2);
00089         cv::Mat undistortedSensorPoints(1,totalsize, CV_32FC2);
00090 
00091         std::vector<cv::Mat> ch;
00092         for(j = 0; j < height; j++)
00093         {
00094             for(i = 0; i < width; i++)
00095             {
00096                 cv::Vec2f &p = sensorPoints.at<cv::Vec2f>(i,j);
00097                 p[0] = i;
00098                 p[1] = j;
00099             }
00100         }
00101 
00102         sensorPoints = sensorPoints.reshape(2,1);
00103 
00104         cv::undistortPoints(sensorPoints, undistortedSensorPoints, cameraMatrix, distCoeffs);
00105 
00106         ch.push_back(undistortedSensorPoints);
00107         ch.push_back(cv::Mat::ones(1,totalsize,CV_32FC1));
00108         cv::merge(ch,pixelVectors);
00109 
00110         if(radial)
00111         {
00112             for(i = 0; i < totalsize; i++)
00113             {
00114                 normalize(pixelVectors.at<cv::Vec3f>(i),
00115                           dst.at<cv::Vec3f>(i));
00116             }
00117             pixelVectors = dst;
00118         }
00119         return pixelVectors.reshape(3,width);
00120     }
00121   
00122 
00123     void PointCloudXyzRadialNodelet::onInit()
00124     {
00125         ros::NodeHandle& nh         = getNodeHandle();
00126         ros::NodeHandle& private_nh = getPrivateNodeHandle();
00127         it_.reset(new image_transport::ImageTransport(nh));
00128 
00129         // Read parameters
00130         private_nh.param("queue_size", queue_size_, 5);
00131 
00132         // Monitor whether anyone is subscribed to the output
00133         ros::SubscriberStatusCallback connect_cb = 
00134             boost::bind(&PointCloudXyzRadialNodelet::connectCb, this);
00135         // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
00136         boost::lock_guard<boost::mutex> lock(connect_mutex_);
00137         pub_point_cloud_ = nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
00138     }
00139 
00140     // Handles (un)subscribing when clients (un)subscribe
00141     void PointCloudXyzRadialNodelet::connectCb()
00142     {
00143         boost::lock_guard<boost::mutex> lock(connect_mutex_);
00144         if (pub_point_cloud_.getNumSubscribers() == 0)
00145         {
00146             sub_depth_.shutdown();
00147         }
00148         else if (!sub_depth_)
00149         {
00150             image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00151             sub_depth_ = it_->subscribeCamera("image_raw",
00152                                               queue_size_,
00153                                               &PointCloudXyzRadialNodelet::depthCb,
00154                                               this, hints);
00155         }
00156     }
00157 
00158     void PointCloudXyzRadialNodelet::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
00159                                              const sensor_msgs::CameraInfoConstPtr& info_msg)
00160     {
00161         PointCloud::Ptr cloud_msg(new PointCloud);
00162         cloud_msg->header = depth_msg->header;
00163         cloud_msg->height = depth_msg->height;
00164         cloud_msg->width  = depth_msg->width;
00165         cloud_msg->is_dense = false;
00166         cloud_msg->is_bigendian = false;
00167 
00168         sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
00169         pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
00170 
00171         if(info_msg->D != D_ || info_msg->K != K_ || width_ != info_msg->width ||
00172            height_ != info_msg->height)
00173         {
00174             D_ = info_msg->D;
00175             K_ = info_msg->K;
00176             width_ = info_msg->width;
00177             height_ = info_msg->height;
00178             binned = initMatrix(cv::Mat_<double>(3, 3, &K_[0]),cv::Mat(D_),width_,height_,true);
00179         }
00180 
00181         if (depth_msg->encoding == enc::TYPE_16UC1)
00182         {
00183             convert<uint16_t>(depth_msg, cloud_msg);
00184         }
00185         else if (depth_msg->encoding == enc::TYPE_32FC1)
00186         {
00187             convert<float>(depth_msg, cloud_msg);
00188         }
00189         else
00190         {
00191             NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
00192             return;
00193         }
00194 
00195         pub_point_cloud_.publish (cloud_msg);
00196     }
00197 
00198     template<typename T>
00199     void PointCloudXyzRadialNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg)
00200     {
00201         // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
00202         double unit_scaling = DepthTraits<T>::toMeters( T(1) );
00203         float bad_point = std::numeric_limits<float>::quiet_NaN();
00204 
00205         sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
00206         sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
00207         sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
00208         const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
00209         int row_step = depth_msg->step / sizeof(T);
00210         for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step)
00211         {
00212             for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z)
00213             {
00214                 T depth = depth_row[u];
00215 
00216                 // Missing points denoted by NaNs
00217                 if (!DepthTraits<T>::valid(depth))
00218                 {
00219                     *iter_x = *iter_y = *iter_z = bad_point;
00220                     continue;
00221                 }
00222                 const cv::Vec3f &cvPoint = binned.at<cv::Vec3f>(u,v) * DepthTraits<T>::toMeters(depth);
00223                 // Fill in XYZ
00224                 *iter_x = cvPoint(0);
00225                 *iter_y = cvPoint(1);
00226                 *iter_z = cvPoint(2);
00227             }
00228         }
00229     }
00230 
00231 } // namespace depth_image_proc
00232 
00233 // Register as nodelet
00234 #include <pluginlib/class_list_macros.h>
00235 PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyzRadialNodelet,nodelet::Nodelet);


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