point_cloud_xyzi.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 <boost/version.hpp>
35 #if ((BOOST_VERSION / 100) % 1000) >= 53
36 #include <boost/thread/lock_guard.hpp>
37 #endif
38 
39 #include <ros/ros.h>
40 #include <nodelet/nodelet.h>
48 #include <sensor_msgs/PointCloud2.h>
51 #include <cv_bridge/cv_bridge.h>
52 #include <opencv2/imgproc/imgproc.hpp>
53 
54 namespace depth_image_proc {
55 
56 using namespace message_filters::sync_policies;
58 
60 {
63 
64  // Subscriptions
70 
71  // Publications
72  boost::mutex connect_mutex_;
73  typedef sensor_msgs::PointCloud2 PointCloud;
75 
77 
78  virtual void onInit();
79 
80  void connectCb();
81 
82  void imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
83  const sensor_msgs::ImageConstPtr& intensity_msg,
84  const sensor_msgs::CameraInfoConstPtr& info_msg);
85 
86  template<typename T, typename T2>
87  void convert(const sensor_msgs::ImageConstPtr& depth_msg,
88  const sensor_msgs::ImageConstPtr& intensity_msg,
89  const PointCloud::Ptr& cloud_msg);
90 };
91 
93 {
94  ros::NodeHandle& nh = getNodeHandle();
95  ros::NodeHandle& private_nh = getPrivateNodeHandle();
96  intensity_nh_.reset( new ros::NodeHandle(nh, "intensity") );
97  ros::NodeHandle depth_nh(nh, "depth");
98  intensity_it_ .reset( new image_transport::ImageTransport(*intensity_nh_) );
99  depth_it_.reset( new image_transport::ImageTransport(depth_nh) );
100 
101  // Read parameters
102  int queue_size;
103  private_nh.param("queue_size", queue_size, 5);
104 
105  // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
106  sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_, sub_intensity_, sub_info_) );
107  sync_->registerCallback(boost::bind(&PointCloudXyziNodelet::imageCb, this, _1, _2, _3));
108 
109  // Monitor whether anyone is subscribed to the output
110  ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyziNodelet::connectCb, this);
111  // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
112  boost::lock_guard<boost::mutex> lock(connect_mutex_);
113  pub_point_cloud_ = depth_nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
114 }
115 
116 // Handles (un)subscribing when clients (un)subscribe
118 {
119  boost::lock_guard<boost::mutex> lock(connect_mutex_);
120  if (pub_point_cloud_.getNumSubscribers() == 0)
121  {
122  sub_depth_.unsubscribe();
123  sub_intensity_ .unsubscribe();
124  sub_info_ .unsubscribe();
125  }
126  else if (!sub_depth_.getSubscriber())
127  {
128  ros::NodeHandle& private_nh = getPrivateNodeHandle();
129  // parameter for depth_image_transport hint
130  std::string depth_image_transport_param = "depth_image_transport";
131 
132  // depth image can use different transport.(e.g. compressedDepth)
133  image_transport::TransportHints depth_hints("raw",ros::TransportHints(), private_nh, depth_image_transport_param);
134  sub_depth_.subscribe(*depth_it_, "image_rect", 1, depth_hints);
135 
136  // intensity uses normal ros transport hints.
137  image_transport::TransportHints hints("raw", ros::TransportHints(), private_nh);
138  sub_intensity_.subscribe(*intensity_it_, "image_rect", 1, hints);
139  sub_info_.subscribe(*intensity_nh_, "camera_info", 1);
140  }
141 }
142 
143 void PointCloudXyziNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
144  const sensor_msgs::ImageConstPtr& intensity_msg_in,
145  const sensor_msgs::CameraInfoConstPtr& info_msg)
146 {
147  // Check for bad inputs
148  if (depth_msg->header.frame_id != intensity_msg_in->header.frame_id)
149  {
150  NODELET_ERROR_THROTTLE(5, "Depth image frame id [%s] doesn't match image frame id [%s]",
151  depth_msg->header.frame_id.c_str(), intensity_msg_in->header.frame_id.c_str());
152  return;
153  }
154 
155  // Update camera model
156  model_.fromCameraInfo(info_msg);
157 
158  // Check if the input image has to be resized
159  sensor_msgs::ImageConstPtr intensity_msg = intensity_msg_in;
160  if (depth_msg->width != intensity_msg->width || depth_msg->height != intensity_msg->height)
161  {
162  sensor_msgs::CameraInfo info_msg_tmp = *info_msg;
163  info_msg_tmp.width = depth_msg->width;
164  info_msg_tmp.height = depth_msg->height;
165  float ratio = float(depth_msg->width)/float(intensity_msg->width);
166  info_msg_tmp.K[0] *= ratio;
167  info_msg_tmp.K[2] *= ratio;
168  info_msg_tmp.K[4] *= ratio;
169  info_msg_tmp.K[5] *= ratio;
170  info_msg_tmp.P[0] *= ratio;
171  info_msg_tmp.P[2] *= ratio;
172  info_msg_tmp.P[5] *= ratio;
173  info_msg_tmp.P[6] *= ratio;
174  model_.fromCameraInfo(info_msg_tmp);
175 
177  try
178  {
179  cv_ptr = cv_bridge::toCvShare(intensity_msg, intensity_msg->encoding);
180  }
181  catch (cv_bridge::Exception& e)
182  {
183  ROS_ERROR("cv_bridge exception: %s", e.what());
184  return;
185  }
186  cv_bridge::CvImage cv_rsz;
187  cv_rsz.header = cv_ptr->header;
188  cv_rsz.encoding = cv_ptr->encoding;
189  cv::resize(cv_ptr->image.rowRange(0,depth_msg->height/ratio), cv_rsz.image, cv::Size(depth_msg->width, depth_msg->height));
190  if ((intensity_msg->encoding == enc::MONO8) || (intensity_msg->encoding == enc::MONO16))
191  intensity_msg = cv_rsz.toImageMsg();
192  else
193  intensity_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::MONO8)->toImageMsg();
194 
195  //NODELET_ERROR_THROTTLE(5, "Depth resolution (%ux%u) does not match resolution (%ux%u)",
196  // depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height);
197  //return;
198  } else
199  intensity_msg = intensity_msg_in;
200 
201  // Supported color encodings: MONO8, MONO16
202  if (intensity_msg->encoding != enc::MONO8 && intensity_msg->encoding != enc::MONO16)
203  {
204  try
205  {
206  intensity_msg = cv_bridge::toCvCopy(intensity_msg, enc::MONO8)->toImageMsg();
207  }
208  catch (cv_bridge::Exception& e)
209  {
210  NODELET_ERROR_THROTTLE(5, "Unsupported encoding [%s]: %s", intensity_msg->encoding.c_str(), e.what());
211  return;
212  }
213  }
214 
215  // Allocate new point cloud message
216  PointCloud::Ptr cloud_msg (new PointCloud);
217  cloud_msg->header = depth_msg->header; // Use depth image time stamp
218  cloud_msg->height = depth_msg->height;
219  cloud_msg->width = depth_msg->width;
220  cloud_msg->is_dense = false;
221  cloud_msg->is_bigendian = false;
222 
223  sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
224 // pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "i");
225  pcd_modifier.setPointCloud2Fields(4,
226  "x", 1, sensor_msgs::PointField::FLOAT32,
227  "y", 1, sensor_msgs::PointField::FLOAT32,
228  "z", 1, sensor_msgs::PointField::FLOAT32,
229  "intensity", 1, sensor_msgs::PointField::FLOAT32);
230 
231 
232  if (depth_msg->encoding == enc::TYPE_16UC1 &&
233  intensity_msg->encoding == enc::MONO8)
234  {
235  convert<uint16_t, uint8_t>(depth_msg, intensity_msg, cloud_msg);
236  }
237  else if (depth_msg->encoding == enc::TYPE_16UC1 &&
238  intensity_msg->encoding == enc::MONO16)
239  {
240  convert<uint16_t, uint16_t>(depth_msg, intensity_msg, cloud_msg);
241  }
242  else if (depth_msg->encoding == enc::TYPE_16UC1 &&
243  intensity_msg->encoding == enc::TYPE_32FC1)
244  {
245  convert<uint16_t,float>(depth_msg, intensity_msg, cloud_msg);
246  }
247  else if (depth_msg->encoding == enc::TYPE_32FC1 &&
248  intensity_msg->encoding == enc::MONO8)
249  {
250  convert<float, uint8_t>(depth_msg, intensity_msg, cloud_msg);
251  }
252  else if (depth_msg->encoding == enc::TYPE_32FC1 &&
253  intensity_msg->encoding == enc::MONO16)
254  {
255  convert<float, uint16_t>(depth_msg, intensity_msg, cloud_msg);
256  }
257  else if (depth_msg->encoding == enc::TYPE_32FC1 &&
258  intensity_msg->encoding == enc::TYPE_32FC1)
259  {
260  convert<float,float>(depth_msg, intensity_msg, cloud_msg);
261  }
262  else
263  {
264  NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
265  return;
266  }
267 
268  pub_point_cloud_.publish (cloud_msg);
269 }
270 
271 template<typename T, typename T2>
272 void PointCloudXyziNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
273  const sensor_msgs::ImageConstPtr& intensity_msg,
274  const PointCloud::Ptr& cloud_msg)
275 {
276  // Use correct principal point from calibration
277  float center_x = model_.cx();
278  float center_y = model_.cy();
279 
280  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
281  double unit_scaling = DepthTraits<T>::toMeters( T(1) );
282  float constant_x = unit_scaling / model_.fx();
283  float constant_y = unit_scaling / model_.fy();
284  float bad_point = std::numeric_limits<float>::quiet_NaN ();
285 
286  const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
287  int row_step = depth_msg->step / sizeof(T);
288 
289  const T2* inten_row = reinterpret_cast<const T2*>(&intensity_msg->data[0]);
290  int inten_row_step = intensity_msg->step / sizeof(T2);
291 
292  sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
293  sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
294  sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
295  sensor_msgs::PointCloud2Iterator<float> iter_i(*cloud_msg, "intensity");
296 
297  for (int v = 0; v < int(cloud_msg->height); ++v, depth_row += row_step, inten_row += inten_row_step)
298  {
299  for (int u = 0; u < int(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z, ++iter_i)
300  {
301  T depth = depth_row[u];
302  T2 inten = inten_row[u];
303  // Check for invalid measurements
304  if (!DepthTraits<T>::valid(depth))
305  {
306  *iter_x = *iter_y = *iter_z = bad_point;
307  }
308  else
309  {
310  // Fill in XYZ
311  *iter_x = (u - center_x) * depth * constant_x;
312  *iter_y = (v - center_y) * depth * constant_y;
313  *iter_z = DepthTraits<T>::toMeters(depth);
314  }
315 
316  // Fill in intensity
317  *iter_i = inten;
318  }
319  }
320 }
321 
322 } // namespace depth_image_proc
323 
324 // Register as nodelet
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void convert(const sensor_msgs::ImageConstPtr &depth_msg, PointCloud::Ptr &cloud_msg, const image_geometry::PinholeCameraModel &model, double range_max=0.0)
PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyziNodelet, nodelet::Nodelet)
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > SyncPolicy
void imageCb(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::ImageConstPtr &intensity_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
boost::shared_ptr< image_transport::ImageTransport > intensity_it_
#define NODELET_ERROR_THROTTLE(rate,...)
ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > SyncPolicy
std::string encoding
void setPointCloud2Fields(int n_fields,...)
boost::shared_ptr< Synchronizer > sync_
sensor_msgs::ImagePtr toImageMsg() const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
image_geometry::PinholeCameraModel model_
void convert(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::ImageConstPtr &intensity_msg, const PointCloud::Ptr &cloud_msg)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
message_filters::Synchronizer< SyncPolicy > Synchronizer
#define ROS_ERROR(...)
image_transport::SubscriberFilter sub_intensity_
std_msgs::Header header


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Wed Dec 7 2022 03:25:21