point_cloud_xyz_radial.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 #include <ros/ros.h>
35 #include <nodelet/nodelet.h>
39 #include <boost/thread.hpp>
41 
43 
44 namespace depth_image_proc {
45 
47 
49  {
50  // Subscriptions
54 
55  // Publications
56  boost::mutex connect_mutex_;
57  typedef sensor_msgs::PointCloud2 PointCloud;
59 
60 
61  std::vector<double> D_;
62  boost::array<double, 9> K_;
63 
64  int width_;
65  int height_;
66 
67  cv::Mat binned;
68 
69  virtual void onInit();
70 
71  void connectCb();
72 
73  void depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
74  const sensor_msgs::CameraInfoConstPtr& info_msg);
75 
76  // Handles float or uint16 depths
77  template<typename T>
78  void convert(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg);
79  };
80 
81  cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial)
82  {
83  int i,j;
84  int totalsize = width*height;
85  cv::Mat pixelVectors(1,totalsize,CV_32FC3);
86  cv::Mat dst(1,totalsize,CV_32FC3);
87 
88  cv::Mat sensorPoints(cv::Size(height,width), CV_32FC2);
89  cv::Mat undistortedSensorPoints(1,totalsize, CV_32FC2);
90 
91  std::vector<cv::Mat> ch;
92  for(j = 0; j < height; j++)
93  {
94  for(i = 0; i < width; i++)
95  {
96  cv::Vec2f &p = sensorPoints.at<cv::Vec2f>(i,j);
97  p[0] = i;
98  p[1] = j;
99  }
100  }
101 
102  sensorPoints = sensorPoints.reshape(2,1);
103 
104  cv::undistortPoints(sensorPoints, undistortedSensorPoints, cameraMatrix, distCoeffs);
105 
106  ch.push_back(undistortedSensorPoints);
107  ch.push_back(cv::Mat::ones(1,totalsize,CV_32FC1));
108  cv::merge(ch,pixelVectors);
109 
110  if(radial)
111  {
112  for(i = 0; i < totalsize; i++)
113  {
114  normalize(pixelVectors.at<cv::Vec3f>(i),
115  dst.at<cv::Vec3f>(i));
116  }
117  pixelVectors = dst;
118  }
119  return pixelVectors.reshape(3,width);
120  }
121 
122 
124  {
126  ros::NodeHandle& private_nh = getPrivateNodeHandle();
127  it_.reset(new image_transport::ImageTransport(nh));
128 
129  // Read parameters
130  private_nh.param("queue_size", queue_size_, 5);
131 
132  // Monitor whether anyone is subscribed to the output
133  ros::SubscriberStatusCallback connect_cb =
134  boost::bind(&PointCloudXyzRadialNodelet::connectCb, this);
135  // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
136  boost::lock_guard<boost::mutex> lock(connect_mutex_);
137  pub_point_cloud_ = nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
138  }
139 
140  // Handles (un)subscribing when clients (un)subscribe
142  {
143  boost::lock_guard<boost::mutex> lock(connect_mutex_);
145  {
147  }
148  else if (!sub_depth_)
149  {
151  sub_depth_ = it_->subscribeCamera("image_raw",
152  queue_size_,
154  this, hints);
155  }
156  }
157 
158  void PointCloudXyzRadialNodelet::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
159  const sensor_msgs::CameraInfoConstPtr& info_msg)
160  {
161  PointCloud::Ptr cloud_msg(new PointCloud);
162  cloud_msg->header = depth_msg->header;
163  cloud_msg->height = depth_msg->height;
164  cloud_msg->width = depth_msg->width;
165  cloud_msg->is_dense = false;
166  cloud_msg->is_bigendian = false;
167 
168  sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
169  pcd_modifier.setPointCloud2FieldsByString(1, "xyz");
170 
171  if(info_msg->D != D_ || info_msg->K != K_ || width_ != info_msg->width ||
172  height_ != info_msg->height)
173  {
174  D_ = info_msg->D;
175  K_ = info_msg->K;
176  width_ = info_msg->width;
177  height_ = info_msg->height;
178  binned = initMatrix(cv::Mat_<double>(3, 3, &K_[0]),cv::Mat(D_),width_,height_,true);
179  }
180 
181  if (depth_msg->encoding == enc::TYPE_16UC1)
182  {
183  convert<uint16_t>(depth_msg, cloud_msg);
184  }
185  else if (depth_msg->encoding == enc::TYPE_32FC1)
186  {
187  convert<float>(depth_msg, cloud_msg);
188  }
189  else
190  {
191  NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
192  return;
193  }
194 
195  pub_point_cloud_.publish (cloud_msg);
196  }
197 
198  template<typename T>
199  void PointCloudXyzRadialNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg)
200  {
201  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
202  double unit_scaling = DepthTraits<T>::toMeters( T(1) );
203  float bad_point = std::numeric_limits<float>::quiet_NaN();
204 
205  sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
206  sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
207  sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
208  const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
209  int row_step = depth_msg->step / sizeof(T);
210  for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step)
211  {
212  for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z)
213  {
214  T depth = depth_row[u];
215 
216  // Missing points denoted by NaNs
217  if (!DepthTraits<T>::valid(depth))
218  {
219  *iter_x = *iter_y = *iter_z = bad_point;
220  continue;
221  }
222  const cv::Vec3f &cvPoint = binned.at<cv::Vec3f>(u,v) * DepthTraits<T>::toMeters(depth);
223  // Fill in XYZ
224  *iter_x = cvPoint(0);
225  *iter_y = cvPoint(1);
226  *iter_z = cvPoint(2);
227  }
228  }
229  }
230 
231 } // namespace depth_image_proc
232 
233 // Register as nodelet
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void publish(const boost::shared_ptr< M > &message) const
#define NODELET_ERROR_THROTTLE(rate,...)
ros::NodeHandle & getPrivateNodeHandle() const
PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyzRadialNodelet, nodelet::Nodelet)
void convert(const sensor_msgs::ImageConstPtr &depth_msg, PointCloud::Ptr &cloud_msg)
cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle & getNodeHandle() const
uint32_t getNumSubscribers() const
void setPointCloud2FieldsByString(int n_fields,...)
void depthCb(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
boost::shared_ptr< image_transport::ImageTransport > it_


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Thu Nov 7 2019 03:44:56