point_cloud_xyz.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <ros/ros.h>
30 #include <nodelet/nodelet.h>
31 
33 
34 #include <pcl/point_cloud.h>
35 #include <pcl/point_types.h>
37 
38 #include <sensor_msgs/PointCloud2.h>
39 #include <sensor_msgs/Image.h>
41 #include <sensor_msgs/CameraInfo.h>
42 #include <stereo_msgs/DisparityImage.h>
43 
46 
48 
52 
53 #include <cv_bridge/cv_bridge.h>
54 #include <opencv2/highgui/highgui.hpp>
55 
56 #include "rtabmap/core/util2d.h"
57 #include "rtabmap/core/util3d.h"
61 #include "rtabmap/utilite/UStl.h"
62 
63 namespace rtabmap_ros
64 {
65 
67 {
68 public:
70  maxDepth_(0.0),
71  minDepth_(0.0),
72  voxelSize_(0.0),
73  decimation_(1),
74  noiseFilterRadius_(0.0),
76  normalK_(0),
77  normalRadius_(0.0),
81  exactSyncDepth_(0),
83  {}
84 
85  virtual ~PointCloudXYZ()
86  {
88  delete approxSyncDepth_;
90  delete approxSyncDisparity_;
91  if(exactSyncDepth_)
92  delete exactSyncDepth_;
94  delete exactSyncDisparity_;
95  }
96 
97 private:
98  virtual void onInit()
99  {
102 
103  int queueSize = 10;
104  bool approxSync = true;
105  std::string roiStr;
106  pnh.param("approx_sync", approxSync, approxSync);
107  pnh.param("queue_size", queueSize, queueSize);
108  pnh.param("max_depth", maxDepth_, maxDepth_);
109  pnh.param("min_depth", minDepth_, minDepth_);
110  pnh.param("voxel_size", voxelSize_, voxelSize_);
111  pnh.param("decimation", decimation_, decimation_);
112  pnh.param("noise_filter_radius", noiseFilterRadius_, noiseFilterRadius_);
113  pnh.param("noise_filter_min_neighbors", noiseFilterMinNeighbors_, noiseFilterMinNeighbors_);
114  pnh.param("normal_k", normalK_, normalK_);
115  pnh.param("normal_radius", normalRadius_, normalRadius_);
116  pnh.param("filter_nans", filterNaNs_, filterNaNs_);
117  pnh.param("roi_ratios", roiStr, roiStr);
118 
119  // Deprecated
120  if(pnh.hasParam("cut_left"))
121  {
122  ROS_ERROR("\"cut_left\" parameter is replaced by \"roi_ratios\". It will be ignored.");
123  }
124  if(pnh.hasParam("cut_right"))
125  {
126  ROS_ERROR("\"cut_right\" parameter is replaced by \"roi_ratios\". It will be ignored.");
127  }
128  if(pnh.hasParam("special_filter_close_object"))
129  {
130  ROS_ERROR("\"special_filter_close_object\" parameter is removed. This kind of processing "
131  "should be done before or after this nodelet. See old implementation here: "
132  "https://github.com/introlab/rtabmap_ros/blob/f0026b071c7c54fbcc71df778dd7e17f52f78fc4/src/nodelets/point_cloud_xyz.cpp#L178-L201.");
133  }
134 
135  //parse roi (region of interest)
136  roiRatios_.resize(4, 0);
137  if(!roiStr.empty())
138  {
139  std::list<std::string> strValues = uSplit(roiStr, ' ');
140  if(strValues.size() != 4)
141  {
142  ROS_ERROR("The number of values must be 4 (\"roi_ratios\"=\"%s\")", roiStr.c_str());
143  }
144  else
145  {
146  std::vector<float> tmpValues(4);
147  unsigned int i=0;
148  for(std::list<std::string>::iterator jter = strValues.begin(); jter!=strValues.end(); ++jter)
149  {
150  tmpValues[i] = uStr2Float(*jter);
151  ++i;
152  }
153 
154  if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
155  tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
156  tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
157  tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
158  {
159  roiRatios_ = tmpValues;
160  }
161  else
162  {
163  ROS_ERROR("The roi ratios are not valid (\"roi_ratios\"=\"%s\")", roiStr.c_str());
164  }
165  }
166  }
167 
168  NODELET_INFO("Approximate time sync = %s", approxSync?"true":"false");
169 
170  if(approxSync)
171  {
173  approxSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
174 
176  approxSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
177  }
178  else
179  {
181  exactSyncDepth_->registerCallback(boost::bind(&PointCloudXYZ::callback, this, _1, _2));
182 
184  exactSyncDisparity_->registerCallback(boost::bind(&PointCloudXYZ::callbackDisparity, this, _1, _2));
185  }
186 
187  ros::NodeHandle depth_nh(nh, "depth");
188  ros::NodeHandle depth_pnh(pnh, "depth");
189  image_transport::ImageTransport depth_it(depth_nh);
190  image_transport::TransportHints hintsDepth("raw", ros::TransportHints(), depth_pnh);
191 
192  imageDepthSub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
193  cameraInfoSub_.subscribe(depth_nh, "camera_info", 1);
194 
195  disparitySub_.subscribe(nh, "disparity/image", 1);
196  disparityCameraInfoSub_.subscribe(nh, "disparity/camera_info", 1);
197 
198  cloudPub_ = nh.advertise<sensor_msgs::PointCloud2>("cloud", 1);
199  }
200 
201  void callback(
202  const sensor_msgs::ImageConstPtr& depth,
203  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
204  {
205  if(depth->encoding.compare(sensor_msgs::image_encodings::TYPE_16UC1)!=0 &&
206  depth->encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1)!=0 &&
207  depth->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0)
208  {
209  NODELET_ERROR("Input type depth=32FC1,16UC1,MONO16");
210  return;
211  }
212 
214  {
216 
217  cv_bridge::CvImageConstPtr imageDepthPtr = cv_bridge::toCvShare(depth);
218  cv::Rect roi = rtabmap::util2d::computeRoi(imageDepthPtr->image, roiRatios_);
219 
221  model.fromCameraInfo(*cameraInfo);
222 
223  pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
225  model.fx(),
226  model.fy(),
227  model.cx()-roiRatios_[0]*double(imageDepthPtr->image.cols),
228  model.cy()-roiRatios_[2]*double(imageDepthPtr->image.rows));
229 
230  pcl::IndicesPtr indices(new std::vector<int>);
232  cv::Mat(imageDepthPtr->image, roi),
233  m,
234  decimation_,
235  maxDepth_,
236  minDepth_,
237  indices.get());
238  processAndPublish(pclCloud, indices, depth->header);
239 
240  NODELET_DEBUG("point_cloud_xyz from depth time = %f s", (ros::WallTime::now() - time).toSec());
241  }
242  }
243 
245  const stereo_msgs::DisparityImageConstPtr& disparityMsg,
246  const sensor_msgs::CameraInfoConstPtr& cameraInfo)
247  {
248  if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) !=0 &&
249  disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_16SC1) !=0)
250  {
251  NODELET_ERROR("Input type must be disparity=32FC1 or 16SC1");
252  return;
253  }
254 
255  cv::Mat disparity;
256  if(disparityMsg->image.encoding.compare(sensor_msgs::image_encodings::TYPE_32FC1) == 0)
257  {
258  disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_32FC1, const_cast<uchar*>(disparityMsg->image.data.data()));
259  }
260  else
261  {
262  disparity = cv::Mat(disparityMsg->image.height, disparityMsg->image.width, CV_16SC1, const_cast<uchar*>(disparityMsg->image.data.data()));
263  }
264 
266  {
268 
269  cv::Rect roi = rtabmap::util2d::computeRoi(disparity, roiRatios_);
270 
271  pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
272  rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
273  rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx()-roiRatios_[0]*double(disparity.cols), leftModel.cy()-roiRatios_[2]*double(disparity.rows), disparityMsg->T);
274  pcl::IndicesPtr indices(new std::vector<int>);
276  cv::Mat(disparity, roi),
277  stereoModel,
278  decimation_,
279  maxDepth_,
280  minDepth_,
281  indices.get());
282 
283  processAndPublish(pclCloud, indices, disparityMsg->header);
284 
285  NODELET_DEBUG("point_cloud_xyz from disparity time = %f s", (ros::WallTime::now() - time).toSec());
286  }
287  }
288 
289  void processAndPublish(pcl::PointCloud<pcl::PointXYZ>::Ptr & pclCloud, pcl::IndicesPtr & indices, const std_msgs::Header & header)
290  {
291  if(indices->size() && voxelSize_ > 0.0)
292  {
293  pclCloud = rtabmap::util3d::voxelize(pclCloud, indices, voxelSize_);
294  }
295 
296  // Do radius filtering after voxel filtering ( a lot faster)
297  if(pclCloud->size() && noiseFilterRadius_ > 0.0 && noiseFilterMinNeighbors_ > 0)
298  {
299  if(pclCloud->is_dense)
300  {
302  }
303  else
304  {
306  }
307  pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
308  pcl::copyPointCloud(*pclCloud, *indices, *tmp);
309  pclCloud = tmp;
310  }
311 
312  sensor_msgs::PointCloud2 rosCloud;
313  if(pclCloud->size() && (normalK_ > 0 || normalRadius_ > 0.0f))
314  {
315  //compute normals
316  pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(pclCloud, normalK_, normalRadius_);
317  pcl::PointCloud<pcl::PointNormal>::Ptr pclCloudNormal(new pcl::PointCloud<pcl::PointNormal>);
318  pcl::concatenateFields(*pclCloud, *normals, *pclCloudNormal);
319  if(filterNaNs_)
320  {
321  pclCloudNormal = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclCloudNormal);
322  }
323  pcl::toROSMsg(*pclCloudNormal, rosCloud);
324  }
325  else
326  {
327  if(filterNaNs_ && !pclCloud->is_dense)
328  {
329  pclCloud = rtabmap::util3d::removeNaNFromPointCloud(pclCloud);
330  }
331  pcl::toROSMsg(*pclCloud, rosCloud);
332  }
333  rosCloud.header.stamp = header.stamp;
334  rosCloud.header.frame_id = header.frame_id;
335 
336  //publish the message
337  cloudPub_.publish(rosCloud);
338  }
339 
340 private:
341 
342  double maxDepth_;
343  double minDepth_;
344  double voxelSize_;
348  int normalK_;
351  std::vector<float> roiRatios_;
352 
354 
357 
360 
363 
366 
369 
372 
373 };
374 
376 }
377 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
std::vector< float > roiRatios_
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP cloudFromDisparity(const cv::Mat &imageDisparity, const StereoCameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
void callbackDisparity(const stereo_msgs::DisparityImageConstPtr &disparityMsg, const sensor_msgs::CameraInfoConstPtr &cameraInfo)
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
message_filters::Synchronizer< MyExactSyncDepthPolicy > * exactSyncDepth_
f
std::string resolveName(const std::string &name, bool remap=true) const
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
image_transport::SubscriberFilter imageDepthSub_
message_filters::sync_policies::ApproximateTime< stereo_msgs::DisparityImage, sensor_msgs::CameraInfo > MyApproxSyncDisparityPolicy
message_filters::Synchronizer< MyApproxSyncDisparityPolicy > * approxSyncDisparity_
message_filters::Subscriber< sensor_msgs::CameraInfo > disparityCameraInfoSub_
message_filters::Synchronizer< MyExactSyncDisparityPolicy > * exactSyncDisparity_
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP cloudFromDepth(const cv::Mat &imageDepth, const CameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
ros::NodeHandle & getPrivateNodeHandle() const
message_filters::sync_policies::ExactTime< stereo_msgs::DisparityImage, sensor_msgs::CameraInfo > MyExactSyncDisparityPolicy
void processAndPublish(pcl::PointCloud< pcl::PointXYZ >::Ptr &pclCloud, pcl::IndicesPtr &indices, const std_msgs::Header &header)
double cx() const
message_filters::Subscriber< stereo_msgs::DisparityImage > disparitySub_
message_filters::Synchronizer< MyApproxSyncDepthPolicy > * approxSyncDepth_
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
const std::string TYPE_32FC1
rtabmap::CameraModel cameraModelFromROS(const sensor_msgs::CameraInfo &camInfo, const rtabmap::Transform &localTransform=rtabmap::Transform::getIdentity())
cv::Rect RTABMAP_EXP computeRoi(const cv::Mat &image, const std::string &roiRatios)
message_filters::Subscriber< sensor_msgs::CameraInfo > cameraInfoSub_
const std::string TYPE_16UC1
cv::Mat RTABMAP_EXP computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const std::string MONO16
ros::NodeHandle & getNodeHandle() const
double cy() const
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
#define false
pcl::IndicesPtr RTABMAP_EXP radiusFiltering(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, float radiusSearch, int minNeighborsInRadius)
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::CameraInfo > MyApproxSyncDepthPolicy
#define NODELET_INFO(...)
uint32_t getNumSubscribers() const
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
static WallTime now()
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
float uStr2Float(const std::string &str)
void callback(const sensor_msgs::ImageConstPtr &depth, const sensor_msgs::CameraInfoConstPtr &cameraInfo)
PLUGINLIB_EXPORT_CLASS(rtabmap_ros::CoreWrapper, nodelet::Nodelet)
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP removeNaNNormalsFromPointCloud(const pcl::PointCloud< pcl::PointNormal >::Ptr &cloud)
bool hasParam(const std::string &key) const
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
#define ROS_ERROR(...)
#define NODELET_DEBUG(...)
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::CameraInfo > MyExactSyncDepthPolicy
const std::string TYPE_16SC1


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:04