multi_plane_extraction_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: C++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2013, Ryohei Ueda and JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
38 #include <pcl/filters/extract_indices.h>
39 #include <pcl/segmentation/extract_polygonal_prism_data.h>
42 #include <pcl/filters/project_inliers.h>
43 #include <set>
44 #include <sstream>
45 
46 namespace jsk_pcl_ros
47 {
48 
50  {
51  pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
52  DiagnosticNodelet::onInit();
53  pnh_->param("use_indices", use_indices_, true);
54  pnh_->param("use_async", use_async_, false);
56  // Publishers
58  pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
59  nonplane_pub_ = advertise<pcl::PointCloud<pcl::PointXYZRGB> >(*pnh_, "output_nonplane_cloud", 1);
60  pub_indices_ = advertise<PCLIndicesMsg>(*pnh_, "output/indices", 1);
61  if (!pnh_->getParam("max_queue_size", maximum_queue_size_)) {
62  maximum_queue_size_ = 100;
63  }
64 
65  pnh_->param("use_coefficients", use_coefficients_, false);
66  pnh_->param("use_sensor_frame", use_sensor_frame_, false);
67  if (use_sensor_frame_) {
68  pnh_->param("sensor_frame", sensor_frame_, std::string("head_root"));
70  if (use_coefficients_) {
71  NODELET_WARN("'~use_sensor_frame' and '~use_coefficients' cannot be enabled at the same time."
72  " disabled '~use_coefficients'");
73  use_coefficients_ = false;
74  }
75  } else if (!use_coefficients_) {
76  NODELET_WARN("'~use_coefficients' and '~use_sensor_frame' are both disabled."
77  " Normal axes of planes are estimated with PCA"
78  " and they may be flipped unintentionally.");
79  }
80 
82  // Dynamic Reconfigure
84  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
85  dynamic_reconfigure::Server<Config>::CallbackType f =
86  boost::bind (&MultiPlaneExtraction::configCallback, this, _1, _2);
87  srv_->setCallback (f);
89  }
90 
92  {
94  // Subscribe
96 
97  sub_input_.subscribe(*pnh_, "input", 1);
98  sub_polygons_.subscribe(*pnh_, "input_polygons", 1);
99 
100  if (use_coefficients_) {
101  sub_coefficients_.subscribe(*pnh_, "input_coefficients", 1);
102  } else {
104  boost::bind(&MultiPlaneExtraction::fillEmptyCoefficients, this, _1));
105  }
106 
107  if (use_indices_) {
108  sub_indices_.subscribe(*pnh_, "indices", 1);
109  } else {
111  boost::bind(&MultiPlaneExtraction::fillEmptyIndices, this, _1));
112  }
113 
114  if (use_async_) {
115  async_ = boost::make_shared<message_filters::Synchronizer<ASyncPolicy> >(maximum_queue_size_);
116  if (use_indices_) {
117  if (use_coefficients_) {
119  } else {
121  }
122  } else {
123  if (use_coefficients_) {
125  } else {
127  }
128  }
129  async_->registerCallback(boost::bind(&MultiPlaneExtraction::extract, this, _1, _2, _3, _4));
130  } else {
131  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(maximum_queue_size_);
132  if (use_indices_) {
133  if (use_coefficients_) {
135  } else {
137  }
138  } else {
139  if (use_coefficients_) {
141  } else {
143  }
144  }
145  sync_->registerCallback(boost::bind(&MultiPlaneExtraction::extract, this, _1, _2, _3, _4));
146  }
147  }
148 
150  {
152  if (use_indices_) {
154  }
156  if (use_coefficients_) {
158  }
159  }
160 
161  void MultiPlaneExtraction::configCallback(Config& config, uint32_t level)
162  {
163  boost::mutex::scoped_lock lock(mutex_);
164  min_height_ = config.min_height;
165  max_height_ = config.max_height;
166  keep_organized_ = config.keep_organized;
167 
168  if (magnify_ != config.magnify)
169  {
170  magnify_ = config.magnify;
171  config.maginify = magnify_;
172  }
173  else if (magnify_ != config.maginify)
174  {
175  ROS_WARN_STREAM_ONCE("parameter 'maginify' is deprecated! Use 'magnify' instead!");
176  magnify_ = config.maginify;
177  config.magnify = magnify_;
178  }
179  }
180 
183  {
184  if (vital_checker_->isAlive()) {
185  stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
186  "MultiPlaneExtraction running");
187  stat.add("Minimum Height", min_height_);
188  stat.add("Maximum Height", max_height_);
189  stat.add("Number of Planes", plane_counter_.mean());
190  }
191  DiagnosticNodelet::updateDiagnostic(stat);
192  }
193 
195  const jsk_recognition_msgs::PolygonArray::ConstPtr &polygons)
196  {
197  jsk_recognition_msgs::ClusterPointIndices indices;
198  indices.header = polygons->header;
199  indices.cluster_indices.resize(polygons->polygons.size());
201  boost::make_shared<jsk_recognition_msgs::ClusterPointIndices>(indices));
202  }
203 
205  const jsk_recognition_msgs::PolygonArray::ConstPtr &polygons)
206  {
207  jsk_recognition_msgs::ModelCoefficientsArray coeffs;
208  coeffs.header = polygons->header;
209  coeffs.coefficients.resize(polygons->polygons.size());
211  boost::make_shared<jsk_recognition_msgs::ModelCoefficientsArray>(coeffs));
212  }
213 
214  void MultiPlaneExtraction::extract(const sensor_msgs::PointCloud2::ConstPtr& input,
215  const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& indices,
216  const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients,
217  const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons)
218  {
219  boost::mutex::scoped_lock lock(mutex_);
220  vital_checker_->poke();
221  // check header
222  if(!jsk_recognition_utils::isSameFrameId(input->header.frame_id,
223  indices->header.frame_id) ||
224  !jsk_recognition_utils::isSameFrameId(input->header.frame_id,
225  coefficients->header.frame_id) ||
226  !jsk_recognition_utils::isSameFrameId(input->header.frame_id,
227  polygons->header.frame_id)) {
228  std::ostringstream oss;
229  oss << "frame id does not match. cloud: " << input->header.frame_id
230  << ", polygons: " << polygons->header.frame_id;
231  if (use_indices_) {
232  oss << ", indices: " << indices->header.frame_id;
233  }
234  if (use_coefficients_) {
235  oss << ", coefficients: " << coefficients->header.frame_id;
236  }
237  NODELET_ERROR_STREAM(oss.str());
238  return;
239  }
240 
241  // set viewpoint to determine normal axes of the planes
242  Eigen::Vector3f viewpoint = Eigen::Vector3f::Zero();
243  if (use_sensor_frame_) {
244  try {
245  tf::StampedTransform transform
247  input->header.frame_id,
249  input->header.stamp,
250  ros::Duration(5.0));
251  Eigen::Affine3f sensor_pose;
252  tf::transformTFToEigen(transform, sensor_pose);
253  viewpoint = Eigen::Vector3f(sensor_pose.translation());
254  }
255  catch (tf2::ConnectivityException &e)
256  {
257  NODELET_ERROR("Transform error: %s", e.what());
258  }
260  {
261  NODELET_ERROR("Transform error: %s", e.what());
262  }
263  catch (...)
264  {
265  NODELET_ERROR("Unknown transform error");
266  }
267  }
268 
269  // convert all to the pcl types
270  pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
271  pcl::PointCloud<pcl::PointXYZRGB>::Ptr nonplane_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
272  pcl::fromROSMsg(*input, *input_cloud);
273  if (indices) {
274  // concat indices into one PointIndices
275  pcl::PointIndices::Ptr all_indices (new pcl::PointIndices);
276  for (size_t i = 0; i < indices->cluster_indices.size(); i++) {
277  std::vector<int> one_indices = indices->cluster_indices[i].indices;
278  for (size_t j = 0; j < one_indices.size(); j++) {
279  all_indices->indices.push_back(one_indices[j]);
280  }
281  }
282 
283 
284  pcl::ExtractIndices<pcl::PointXYZRGB> extract_nonplane;
285  extract_nonplane.setNegative(true);
286  extract_nonplane.setKeepOrganized(keep_organized_);
287  extract_nonplane.setInputCloud(input_cloud);
288  extract_nonplane.setIndices(all_indices);
289  extract_nonplane.filter(*nonplane_cloud);
290  sensor_msgs::PointCloud2 ros_result;
291  pcl::toROSMsg(*nonplane_cloud, ros_result);
292  ros_result.header = input->header;
293  nonplane_pub_.publish(ros_result);
294  }
295  else {
296  nonplane_cloud = input_cloud;
297  }
298  // for each plane, project nonplane_cloud to the plane and find the points
299  // inside of the polygon
300 
301  std::set<int> result_set;
302  plane_counter_.add(polygons->polygons.size());
303  for (size_t plane_i = 0; plane_i < polygons->polygons.size(); plane_i++) {
304  if (use_coefficients_) {
305  // set viewpoint from coefficients
306  for (size_t vec_i = 0; vec_i < 3; ++vec_i)
307  viewpoint[vec_i] = coefficients->coefficients[plane_i].values[vec_i];
308  }
309  pcl::ExtractPolygonalPrismData<pcl::PointXYZRGB> prism_extract;
310  prism_extract.setViewPoint(viewpoint[0], viewpoint[1], viewpoint[2]);
311  pcl::PointCloud<pcl::PointXYZRGB>::Ptr hull_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
312  geometry_msgs::Polygon the_polygon = polygons->polygons[plane_i].polygon;
313  if (the_polygon.points.size() <= 2) {
314  NODELET_WARN("too small polygon");
315  continue;
316  }
317  // compute centroid first
318  Eigen::Vector3f centroid(0, 0, 0);
319  for (size_t i = 0; i < the_polygon.points.size(); i++) {
320  pcl::PointXYZRGB p;
321  jsk_recognition_utils::pointFromXYZToXYZ<geometry_msgs::Point32, pcl::PointXYZRGB>(
322  the_polygon.points[i], p);
323  centroid = centroid + p.getVector3fMap();
324  }
325  centroid = centroid / the_polygon.points.size();
326 
327  for (size_t i = 0; i < the_polygon.points.size(); i++) {
328  pcl::PointXYZRGB p;
329  jsk_recognition_utils::pointFromXYZToXYZ<geometry_msgs::Point32, pcl::PointXYZRGB>(
330  the_polygon.points[i], p);
331  Eigen::Vector3f dir = (p.getVector3fMap() - centroid).normalized();
332  p.getVector3fMap() = dir * magnify_ + p.getVector3fMap();
333  hull_cloud->points.push_back(p);
334  }
335 
336  pcl::PointXYZRGB p_last;
337  jsk_recognition_utils::pointFromXYZToXYZ<geometry_msgs::Point32, pcl::PointXYZRGB>(
338  the_polygon.points[0], p_last);
339  hull_cloud->points.push_back(p_last);
340 
341  prism_extract.setInputCloud(nonplane_cloud);
342  prism_extract.setHeightLimits(min_height_, max_height_);
343  prism_extract.setInputPlanarHull(hull_cloud);
344  pcl::PointIndices output_indices;
345  prism_extract.segment(output_indices);
346  // append output to result_cloud
347  for (size_t i = 0; i < output_indices.indices.size(); i++) {
348  result_set.insert(output_indices.indices[i]);
349  }
350  }
351 
352  // convert std::set to PCLIndicesMsg
353  //PCLIndicesMsg output_indices;
354  pcl::PointCloud<pcl::PointXYZRGB> result_cloud;
355  pcl::PointIndices::Ptr all_result_indices (new pcl::PointIndices());
356  for (std::set<int>::iterator it = result_set.begin();
357  it != result_set.end();
358  it++) {
359  all_result_indices->indices.push_back(*it);
360  }
361 
362  pcl::ExtractIndices<pcl::PointXYZRGB> extract_all_indices;
363  extract_all_indices.setKeepOrganized(keep_organized_);
364  extract_all_indices.setInputCloud(nonplane_cloud);
365  extract_all_indices.setIndices(all_result_indices);
366  extract_all_indices.filter(result_cloud);
367 
368  sensor_msgs::PointCloud2 ros_result;
369  pcl::toROSMsg(result_cloud, ros_result);
370  ros_result.header = input->header;
371  pub_.publish(ros_result);
372  PCLIndicesMsg ros_indices;
373  pcl_conversions::fromPCL(*all_result_indices, ros_indices);
374  ros_indices.header = input->header;
375  pub_indices_.publish(ros_indices);
376  diagnostic_updater_->update();
377  }
378 }
379 
message_filters::Subscriber< jsk_recognition_msgs::ClusterPointIndices > sub_indices_
#define NODELET_ERROR(...)
TimeredDiagnosticUpdater::Ptr diagnostic_updater_
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
virtual void configCallback(Config &config, uint32_t level)
void summary(unsigned char lvl, const std::string s)
tf::StampedTransform lookupTransformWithDuration(tf::TransformListener *listener, const std::string &to_frame, const std::string &from_frame, const ros::Time &stamp, ros::Duration duration)
virtual void fillEmptyIndices(const jsk_recognition_msgs::PolygonArray::ConstPtr &polygons)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
jsk_recognition_utils::Counter plane_counter_
message_filters::PassThrough< jsk_recognition_msgs::ClusterPointIndices > null_indices_
TFSIMD_FORCE_INLINE Vector3 normalized() const
#define NODELET_ERROR_STREAM(...)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
jsk_pcl_ros::MultiPlaneExtractionConfig Config
boost::shared_ptr< ros::NodeHandle > pnh_
boost::shared_ptr< message_filters::Synchronizer< ASyncPolicy > > async_
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_polygons_
virtual void add(double v)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
jsk_topic_tools::VitalChecker::Ptr vital_checker_
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)
message_filters::PassThrough< jsk_recognition_msgs::ModelCoefficientsArray > null_coefficients_
#define ROS_WARN_STREAM_ONCE(args)
p
void add(const MConstPtr &msg)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
bool isSameFrameId(const std::string &a, const std::string &b)
void add(const std::string &key, const T &val)
static tf::TransformListener * getInstance()
pcl::PointIndices PCLIndicesMsg
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::MultiPlaneExtraction, nodelet::Nodelet)
virtual void extract(const sensor_msgs::PointCloud2::ConstPtr &input, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &indices, const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr &coefficients, const jsk_recognition_msgs::PolygonArray::ConstPtr &polygons)
std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header)
virtual void fillEmptyCoefficients(const jsk_recognition_msgs::PolygonArray::ConstPtr &polygons)
message_filters::Subscriber< jsk_recognition_msgs::ModelCoefficientsArray > sub_coefficients_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
Connection registerCallback(const C &callback)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:47