point_cloud_xyzi_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>
43 #include <boost/thread.hpp>
45 
47 
49 
50  using namespace message_filters::sync_policies;
53 
55  {
57 
58  // Subscriptions
62 
64 
65  // Publications
66  boost::mutex connect_mutex_;
67  typedef sensor_msgs::PointCloud2 PointCloud;
69 
70 
73 
74  std::vector<double> D_;
75  boost::array<double, 9> K_;
76 
77  int width_;
78  int height_;
79 
80  cv::Mat transform_;
81 
82  virtual void onInit();
83 
84  void connectCb();
85 
86  void imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
87  const sensor_msgs::ImageConstPtr& intensity_msg_in,
88  const sensor_msgs::CameraInfoConstPtr& info_msg);
89 
90  // Handles float or uint16 depths
91  template<typename T>
92  void convert_depth(const sensor_msgs::ImageConstPtr& depth_msg, PointCloud::Ptr& cloud_msg);
93 
94  template<typename T>
95  void convert_intensity(const sensor_msgs::ImageConstPtr &inten_msg, PointCloud::Ptr& cloud_msg);
96 
97  cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial);
98 
99  };
100 
101  cv::Mat PointCloudXyziRadialNodelet::initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial)
102  {
103  int i,j;
104  int totalsize = width*height;
105  cv::Mat pixelVectors(1,totalsize,CV_32FC3);
106  cv::Mat dst(1,totalsize,CV_32FC3);
107 
108  cv::Mat sensorPoints(cv::Size(height,width), CV_32FC2);
109  cv::Mat undistortedSensorPoints(1,totalsize, CV_32FC2);
110 
111  std::vector<cv::Mat> ch;
112  for(j = 0; j < height; j++)
113  {
114  for(i = 0; i < width; i++)
115  {
116  cv::Vec2f &p = sensorPoints.at<cv::Vec2f>(i,j);
117  p[0] = i;
118  p[1] = j;
119  }
120  }
121 
122  sensorPoints = sensorPoints.reshape(2,1);
123 
124  cv::undistortPoints(sensorPoints, undistortedSensorPoints, cameraMatrix, distCoeffs);
125 
126  ch.push_back(undistortedSensorPoints);
127  ch.push_back(cv::Mat::ones(1,totalsize,CV_32FC1));
128  cv::merge(ch,pixelVectors);
129 
130  if(radial)
131  {
132  for(i = 0; i < totalsize; i++)
133  {
134  normalize(pixelVectors.at<cv::Vec3f>(i),
135  dst.at<cv::Vec3f>(i));
136  }
137  pixelVectors = dst;
138  }
139  return pixelVectors.reshape(3,width);
140  }
141 
142 
144  {
145  ros::NodeHandle& nh = getNodeHandle();
146  ros::NodeHandle& private_nh = getPrivateNodeHandle();
147 
148  intensity_nh_.reset( new ros::NodeHandle(nh, "intensity") );
149  ros::NodeHandle depth_nh(nh, "depth");
150  intensity_it_ .reset( new image_transport::ImageTransport(*intensity_nh_) );
151  depth_it_.reset( new image_transport::ImageTransport(depth_nh) );
152 
153  // Read parameters
154  private_nh.param("queue_size", queue_size_, 5);
155 
156  // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
157  sync_.reset( new Synchronizer(SyncPolicy(queue_size_), sub_depth_, sub_intensity_, sub_info_) );
158  sync_->registerCallback(boost::bind(&PointCloudXyziRadialNodelet::imageCb, this, _1, _2, _3));
159 
160  // Monitor whether anyone is subscribed to the output
161  ros::SubscriberStatusCallback connect_cb =
162  boost::bind(&PointCloudXyziRadialNodelet::connectCb, this);
163  // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
164  boost::lock_guard<boost::mutex> lock(connect_mutex_);
165  pub_point_cloud_ = nh.advertise<PointCloud>("points", 20, connect_cb, connect_cb);
166  }
167 
168  // Handles (un)subscribing when clients (un)subscribe
170  {
171  boost::lock_guard<boost::mutex> lock(connect_mutex_);
172 
173  if (pub_point_cloud_.getNumSubscribers() == 0)
174  {
175  sub_depth_.unsubscribe();
176  sub_intensity_.unsubscribe();
177  sub_info_.unsubscribe();
178  }
179  else if (!sub_depth_.getSubscriber())
180  {
181  ros::NodeHandle& private_nh = getPrivateNodeHandle();
182  // parameter for depth_image_transport hint
183  std::string depth_image_transport_param = "depth_image_transport";
184 
185  // depth image can use different transport.(e.g. compressedDepth)
186  image_transport::TransportHints depth_hints("raw",ros::TransportHints(), private_nh, depth_image_transport_param);
187  sub_depth_.subscribe(*depth_it_, "image_raw", 5, depth_hints);
188 
189  // intensity uses normal ros transport hints.
190  image_transport::TransportHints hints("raw", ros::TransportHints(), private_nh);
191  sub_intensity_.subscribe(*intensity_it_, "image_raw", 5, hints);
192  sub_info_.subscribe(*intensity_nh_, "camera_info", 5);
193  }
194  }
195 
196  void PointCloudXyziRadialNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
197  const sensor_msgs::ImageConstPtr& intensity_msg,
198  const sensor_msgs::CameraInfoConstPtr& info_msg)
199  {
200  PointCloud::Ptr cloud_msg(new PointCloud);
201  cloud_msg->header = depth_msg->header;
202  cloud_msg->height = depth_msg->height;
203  cloud_msg->width = depth_msg->width;
204  cloud_msg->is_dense = false;
205  cloud_msg->is_bigendian = false;
206 
207  sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
208  pcd_modifier.setPointCloud2Fields(4,
209  "x", 1, sensor_msgs::PointField::FLOAT32,
210  "y", 1, sensor_msgs::PointField::FLOAT32,
211  "z", 1, sensor_msgs::PointField::FLOAT32,
212  "intensity", 1, sensor_msgs::PointField::FLOAT32);
213 
214 
215  if(info_msg->D != D_ || info_msg->K != K_ || width_ != info_msg->width ||
216  height_ != info_msg->height)
217  {
218  D_ = info_msg->D;
219  K_ = info_msg->K;
220  width_ = info_msg->width;
221  height_ = info_msg->height;
222  transform_ = initMatrix(cv::Mat_<double>(3, 3, &K_[0]),cv::Mat(D_),width_,height_,true);
223  }
224 
225  if (depth_msg->encoding == enc::TYPE_16UC1)
226  {
227  convert_depth<uint16_t>(depth_msg, cloud_msg);
228  }
229  else if (depth_msg->encoding == enc::TYPE_32FC1)
230  {
231  convert_depth<float>(depth_msg, cloud_msg);
232  }
233  else
234  {
235  NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
236  return;
237  }
238 
239  if(intensity_msg->encoding == enc::TYPE_16UC1)
240  {
241  convert_intensity<uint16_t>(intensity_msg, cloud_msg);
242 
243  }
244  else if(intensity_msg->encoding == enc::MONO8)
245  {
246  convert_intensity<uint8_t>(intensity_msg, cloud_msg);
247  }
248  else
249  {
250  NODELET_ERROR_THROTTLE(5, "Intensity image has unsupported encoding [%s]", intensity_msg->encoding.c_str());
251  return;
252  }
253 
254  pub_point_cloud_.publish (cloud_msg);
255  }
256 
257  template<typename T>
258  void PointCloudXyziRadialNodelet::convert_depth(const sensor_msgs::ImageConstPtr& depth_msg,
259  PointCloud::Ptr& cloud_msg)
260  {
261  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
262  double unit_scaling = DepthTraits<T>::toMeters( T(1) );
263  float bad_point = std::numeric_limits<float>::quiet_NaN();
264 
265  sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
266  sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
267  sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
268  const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
269 
270  int row_step = depth_msg->step / sizeof(T);
271  for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step)
272  {
273  for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z)
274  {
275  T depth = depth_row[u];
276 
277  // Missing points denoted by NaNs
278  if (!DepthTraits<T>::valid(depth))
279  {
280  *iter_x = *iter_y = *iter_z = bad_point;
281  continue;
282  }
283  const cv::Vec3f &cvPoint = transform_.at<cv::Vec3f>(u,v) * DepthTraits<T>::toMeters(depth);
284  // Fill in XYZ
285  *iter_x = cvPoint(0);
286  *iter_y = cvPoint(1);
287  *iter_z = cvPoint(2);
288  }
289  }
290  }
291 
292  template<typename T>
293  void PointCloudXyziRadialNodelet::convert_intensity(const sensor_msgs::ImageConstPtr& intensity_msg,
294  PointCloud::Ptr& cloud_msg)
295  {
296  sensor_msgs::PointCloud2Iterator<float> iter_i(*cloud_msg, "intensity");
297  const T* inten_row = reinterpret_cast<const T*>(&intensity_msg->data[0]);
298 
299  const int i_row_step = intensity_msg->step/sizeof(T);
300  for (int v = 0; v < (int)cloud_msg->height; ++v, inten_row += i_row_step)
301  {
302  for (int u = 0; u < (int)cloud_msg->width; ++u, ++iter_i)
303  {
304  *iter_i = inten_row[u];
305  }
306  }
307  }
308 
309 } // namespace xiaoqiang_depth_image_proc
310 
311 // Register as nodelet
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
PLUGINLIB_EXPORT_CLASS(xiaoqiang_depth_image_proc::PointCloudXyziRadialNodelet, nodelet::Nodelet)
cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial)
#define NODELET_ERROR_THROTTLE(rate,...)
void convert_intensity(const sensor_msgs::ImageConstPtr &inten_msg, PointCloud::Ptr &cloud_msg)
void setPointCloud2Fields(int n_fields,...)
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > SyncPolicy
bool param(const std::string &param_name, T &param_val, const T &default_val) const
boost::shared_ptr< image_transport::ImageTransport > intensity_it_
cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void convert_depth(const sensor_msgs::ImageConstPtr &depth_msg, PointCloud::Ptr &cloud_msg)
message_filters::Synchronizer< SyncPolicy > Synchronizer
void imageCb(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::ImageConstPtr &intensity_msg_in, const sensor_msgs::CameraInfoConstPtr &info_msg)


xiaoqiang_depth_image_proc
Author(s): Xie fusheng
autogenerated on Mon Jun 10 2019 15:53:04