hinted_plane_detector_nodelet.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, JSK Lab
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/o2r other materials provided
16  * with the distribution.
17  * * Neither the name of the JSK Lab 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 #define BOOST_PARAMETER_MAX_ARITY 7
36 #include "pcl_ros/transforms.h"
37 #include <visualization_msgs/Marker.h>
38 #include <pcl/surface/concave_hull.h>
39 #include <pcl/sample_consensus/method_types.h>
40 #include <pcl/sample_consensus/model_types.h>
41 #include <pcl/segmentation/sac_segmentation.h>
42 #include <pcl/filters/project_inliers.h>
43 #include <pcl/filters/extract_indices.h>
44 #include <pcl/ModelCoefficients.h>
45 #include <pcl/kdtree/kdtree.h>
46 #include <pcl/segmentation/extract_clusters.h>
47 #include <pcl/common/centroid.h>
49 
50 namespace jsk_pcl_ros {
51 
53  pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
54  DiagnosticNodelet::onInit();
55  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
56  typename dynamic_reconfigure::Server<Config>::CallbackType f =
57  boost::bind (&HintedPlaneDetector::configCallback, this, _1, _2);
58  srv_->setCallback (f);
59 
60  pub_hint_polygon_ = advertise<geometry_msgs::PolygonStamped>(
61  *pnh_, "output/hint/polygon", 1);
62  pub_hint_polygon_array_ = advertise<jsk_recognition_msgs::PolygonArray>(
63  *pnh_, "output/hint/polygon_array", 1);
64  pub_hint_inliers_ = advertise<PCLIndicesMsg>(
65  *pnh_, "output/hint/inliers", 1);
66  pub_hint_coefficients_ = advertise<PCLModelCoefficientMsg>(
67  *pnh_, "output/hint/coefficients", 1);
68  pub_polygon_ = advertise<geometry_msgs::PolygonStamped>(
69  *pnh_, "output/polygon", 1);
70  pub_polygon_array_ = advertise<jsk_recognition_msgs::PolygonArray>(
71  *pnh_, "output/polygon_array", 1);
72  pub_hint_filtered_indices_ = advertise<PCLIndicesMsg>(
73  *pnh_, "output/hint_filtered_indices", 1);
74  pub_plane_filtered_indices_ = advertise<PCLIndicesMsg>(
75  *pnh_, "output/plane_filtered_indices", 1);
76  pub_density_filtered_indices_ = advertise<PCLIndicesMsg>(
77  *pnh_, "output/density_filtered_indices", 1);
78  pub_euclidean_filtered_indices_ = advertise<PCLIndicesMsg>(
79  *pnh_, "output/euclidean_filtered_indices", 1);
80  pub_inliers_ = advertise<PCLIndicesMsg>(
81  *pnh_, "output/inliers", 1);
82  pub_coefficients_ = advertise<PCLModelCoefficientMsg>(
83  *pnh_, "output/coefficients", 1);
85  }
86 
88  {
89  sub_cloud_.subscribe(*pnh_, "input", 1);
90  sub_hint_cloud_.subscribe(*pnh_, "input/hint/cloud", 1);
91  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
92  sync_->connectInput(sub_cloud_, sub_hint_cloud_);
93  sync_->registerCallback(boost::bind(&HintedPlaneDetector::detect,
94  this, _1, _2));
95  }
96 
98  {
101  }
102 
104  Config &config, uint32_t level)
105  {
106  boost::mutex::scoped_lock lock(mutex_);
107  hint_outlier_threashold_ = config.hint_outlier_threashold;
108  hint_max_iteration_ = config.hint_max_iteration;
109  hint_min_size_ = config.hint_min_size;
110  outlier_threashold_ = config.outlier_threashold;
111  max_iteration_ = config.max_iteration;
112  min_size_ = config.min_size;
113  eps_angle_ = config.eps_angle;
114  normal_filter_eps_angle_ = config.normal_filter_eps_angle;
115  euclidean_clustering_filter_tolerance_ = config.euclidean_clustering_filter_tolerance;
116  euclidean_clustering_filter_min_size_ = config.euclidean_clustering_filter_min_size;
117  density_radius_ = config.density_radius;
118  density_num_ = config.density_num;
119  enable_euclidean_filtering_ = config.enable_euclidean_filtering;
120  enable_normal_filtering_ = config.enable_normal_filtering;
121  enable_distance_filtering_ = config.enable_distance_filtering;
122  enable_density_filtering_ = config.enable_density_filtering;
123  }
124 
126  const sensor_msgs::PointCloud2::ConstPtr& cloud_msg,
127  const sensor_msgs::PointCloud2::ConstPtr& hint_cloud_msg)
128  {
129  vital_checker_->poke();
130  boost::mutex::scoped_lock lock(mutex_);
131  pcl::PointCloud<pcl::PointNormal>::Ptr
132  input_cloud (new pcl::PointCloud<pcl::PointNormal>);
133  pcl::PointCloud<pcl::PointXYZ>::Ptr
134  hint_cloud (new pcl::PointCloud<pcl::PointXYZ>);
135  pcl::fromROSMsg(*cloud_msg, *input_cloud);
136  pcl::fromROSMsg(*hint_cloud_msg, *hint_cloud);
137 
138  // estimate plane out of hint_cloud
139 
141 
142  if (detectHintPlane(hint_cloud, convex) && convex) {
143  if (detectLargerPlane(input_cloud, convex)) {
144  NODELET_INFO("success to detect!");
145  }
146  else {
147  NODELET_ERROR("failed to detect larger plane");
148  }
149  }
150  }
151 
152  pcl::PointIndices::Ptr HintedPlaneDetector::getBestCluster(
153  pcl::PointCloud<pcl::PointNormal>::Ptr input_cloud,
154  const std::vector<pcl::PointIndices>& cluster_indices,
156  {
157  Eigen::Vector3f center = hint_convex->centroid();
158  double min_dist = DBL_MAX;
159  size_t min_index = 0;
160  for (size_t i = 0; i < cluster_indices.size(); i++) {
161  Eigen::Vector4f center_cluster4;
162  pcl::compute3DCentroid<pcl::PointNormal>(*input_cloud,
163  cluster_indices[i].indices,
164  center_cluster4);
165  Eigen::Vector3f center_cluster3(center_cluster4[0], center_cluster4[1], center_cluster4[2]);
166  double dist = (center - center_cluster3).norm();
167  if (dist < min_dist) {
168  min_dist = dist;
169  min_index = i;
170  }
171  }
172  pcl::PointIndices::Ptr ret (new pcl::PointIndices);
173  ret->indices = cluster_indices[min_index].indices;
174  return ret;
175  }
176 
178  const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
179  const pcl::PointIndices::Ptr indices,
180  pcl::PointIndices& output)
181  {
183  pcl::KdTreeFLANN<pcl::PointNormal> kdtree;
184  pcl::KdTreeFLANN<pcl::PointNormal>::IndicesPtr indices_ptr
185  (new std::vector<int>);
186  *indices_ptr = indices->indices;
187  kdtree.setInputCloud(cloud, indices_ptr);
188  for (size_t i = 0; i < indices->indices.size(); i++) {
189  int point_index = indices->indices[i];
190  std::vector<int> result_indices;
191  std::vector<float> result_distances;
192  kdtree.radiusSearch(i, density_radius_,
193  result_indices, result_distances);
194  if (result_distances.size() >= density_num_) {
195  output.indices.push_back(point_index);
196  }
197  }
198  }
199  else {
200  output = *indices;
201  }
202  output.header = cloud->header;
203  PCLIndicesMsg ros_indices;
204  pcl_conversions::fromPCL(output, ros_indices);
206  }
207 
209  const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
210  const pcl::PointIndices::Ptr indices,
212  pcl::PointIndices& output)
213  {
215  pcl::EuclideanClusterExtraction<pcl::PointNormal> ec;
216  ec.setClusterTolerance(euclidean_clustering_filter_tolerance_);
217  pcl::search::KdTree<pcl::PointNormal>::Ptr tree
218  (new pcl::search::KdTree<pcl::PointNormal>);
219  tree->setInputCloud(cloud);
220  ec.setSearchMethod(tree);
221  ec.setIndices(indices);
222  ec.setInputCloud(cloud);
223  //ec.setMinClusterSize ();
224  std::vector<pcl::PointIndices> cluster_indices;
225  ec.extract(cluster_indices);
226  if (cluster_indices.size() == 0) {
227  return;
228  }
229  NODELET_INFO("%lu clusters", cluster_indices.size());
230  pcl::PointIndices::Ptr filtered_indices
231  = getBestCluster(cloud, cluster_indices, hint_convex);
232  output = *filtered_indices;
233  }
234  else {
235  output = *indices;
236  }
237  output.header = cloud->header;
238  PCLIndicesMsg ros_indices;
239  pcl_conversions::fromPCL(output, ros_indices);
241  }
242 
244  const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
246  pcl::PointIndices& output)
247  {
248  for (size_t i = 0; i < cloud->points.size(); i++) {
249  pcl::PointNormal p = cloud->points[i];
250  if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.y)) {
251  Eigen::Vector4f v = p.getVector4fMap();
252  if (!enable_distance_filtering_ || hint_convex->distanceToPoint(v) < outlier_threashold_) {
253  Eigen::Vector3f n(p.normal_x, p.normal_y, p.normal_z);
254  if (!enable_normal_filtering_ || hint_convex->angle(n) < normal_filter_eps_angle_) {
255  output.indices.push_back(i);
256  }
257  }
258  }
259  }
260  output.header = cloud->header;
261  PCLIndicesMsg ros_candidate_inliers;
262  pcl_conversions::fromPCL(output, ros_candidate_inliers);
263  pub_hint_filtered_indices_.publish(ros_candidate_inliers);
264  }
265 
267  const pcl::PointCloud<pcl::PointNormal>::Ptr cloud,
268  const pcl::PointIndices::Ptr indices,
269  const Eigen::Vector3f& normal,
270  pcl::PointIndices& output,
271  pcl::ModelCoefficients& coefficients)
272  {
273  pcl::SACSegmentation<pcl::PointNormal> seg;
274  seg.setOptimizeCoefficients (true);
275  seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
276  seg.setMethodType (pcl::SAC_RANSAC);
277  seg.setDistanceThreshold (outlier_threashold_);
278  seg.setMaxIterations (max_iteration_);
279  seg.setEpsAngle(eps_angle_);
280  seg.setAxis(normal);
281  seg.setInputCloud(cloud);
282  seg.setIndices(indices);
283  seg.segment(output, coefficients);
284  coefficients.header = cloud->header;
285  output.header = cloud->header;
286  PCLIndicesMsg ros_indices;
287  pcl_conversions::fromPCL(output, ros_indices);
289  }
290 
292  pcl::PointCloud<pcl::PointNormal>::Ptr input_cloud,
294  {
295  pcl::PointIndices::Ptr candidate_inliers (new pcl::PointIndices);
296  hintFilter(input_cloud, hint_convex, *candidate_inliers);
297  // filter points based on hint_convex to get better result...
298 
299  pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
300  pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients);
301  planeFilter(input_cloud, candidate_inliers,
302  hint_convex->getNormal(),
303  *plane_inliers, *plane_coefficients);
304  if (plane_inliers->indices.size() < min_size_) { // good!
305  NODELET_ERROR("failed to detect by plane fitting filtering");
306  return false;
307  }
308  // Check direction of plane_coefficients
309  Eigen::Vector3f plane_normal(plane_coefficients->values[0], plane_coefficients->values[1], plane_coefficients->values[2]);
310  if (plane_normal.dot(Eigen::Vector3f::UnitZ()) > 0) {
311  // flip
312  plane_coefficients->values[0] = -plane_coefficients->values[0];
313  plane_coefficients->values[1] = -plane_coefficients->values[1];
314  plane_coefficients->values[2] = -plane_coefficients->values[2];
315  plane_coefficients->values[3] = -plane_coefficients->values[3];
316  }
317  // filtering by euclidean clustering
318  pcl::PointIndices::Ptr euclidean_filtered_indices(new pcl::PointIndices);
319  euclideanFilter(input_cloud, plane_inliers, hint_convex,
320  *euclidean_filtered_indices);
321  if (euclidean_filtered_indices->indices.size() < min_size_) {
322  NODELET_ERROR("failed to detect by euclidean filtering");
323  return false;
324  }
325  pcl::PointIndices::Ptr density_filtered_indices (new pcl::PointIndices);
327  input_cloud, euclidean_filtered_indices, *density_filtered_indices);
328 
329  if (density_filtered_indices->indices.size() < min_size_) {
330  NODELET_ERROR("failed to detect by density filtering");
331  return false;
332  }
334  = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointNormal>(
335  input_cloud, density_filtered_indices, plane_coefficients);
336  // publish to ROS
338  input_cloud->header);
339  PCLIndicesMsg ros_inliers;
340  pcl_conversions::fromPCL(*density_filtered_indices, ros_inliers);
341  pub_inliers_.publish(ros_inliers);
342  PCLModelCoefficientMsg ros_coefficients;
343  pcl_conversions::fromPCL(*plane_coefficients, ros_coefficients);
344  pub_coefficients_.publish(ros_coefficients);
345  return true;
346  }
347 
351  ros::Publisher& pub_polygon_array,
352  const pcl::PCLHeader& header)
353  {
354  geometry_msgs::PolygonStamped ros_polygon;
355  ros_polygon.polygon = convex->toROSMsg();
356  pcl_conversions::fromPCL(header, ros_polygon.header);
357  jsk_recognition_msgs::PolygonArray ros_polygon_array;
358  pcl_conversions::fromPCL(header, ros_polygon_array.header);
359  ros_polygon_array.polygons.push_back(
360  ros_polygon);
361  pub_polygon_array.publish(ros_polygon_array);
362  pub_polygon.publish(ros_polygon);
363  }
364 
366  pcl::PointCloud<pcl::PointXYZ>::Ptr hint_cloud,
368  {
369  pcl::PointIndices::Ptr hint_inliers (new pcl::PointIndices);
370  pcl::ModelCoefficients::Ptr hint_coefficients(new pcl::ModelCoefficients);
371  pcl::SACSegmentation<pcl::PointXYZ> seg;
372  seg.setOptimizeCoefficients (true);
373  seg.setModelType (pcl::SACMODEL_PLANE);
374  seg.setMethodType (pcl::SAC_RANSAC);
375  seg.setDistanceThreshold (hint_outlier_threashold_);
376  seg.setMaxIterations (hint_max_iteration_);
377  seg.setInputCloud(hint_cloud);
378  seg.segment(*hint_inliers, *hint_coefficients);
379  if (hint_inliers->indices.size() > hint_min_size_) { // good!
380  convex = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZ>(
381  hint_cloud, hint_inliers, hint_coefficients);
382  // publish hint results for debug/visualization
383  publishPolygon(convex,
385  hint_cloud->header);
386  PCLIndicesMsg ros_indices;
387  PCLModelCoefficientMsg ros_coefficients;
388  pcl_conversions::fromPCL(*hint_inliers, ros_indices);
389  pub_hint_inliers_.publish(ros_indices);
390  pcl_conversions::fromPCL(*hint_coefficients, ros_coefficients);
391  pub_hint_coefficients_.publish(ros_coefficients);
392  return true;
393  }
394  else {
395  NODELET_ERROR("Failed to find hint plane");
396  return false;
397  }
398  }
399 
400 }
401 
virtual bool detectHintPlane(pcl::PointCloud< pcl::PointXYZ >::Ptr hint_cloud, jsk_recognition_utils::ConvexPolygon::Ptr &convex)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
#define NODELET_ERROR(...)
virtual void detect(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg, const sensor_msgs::PointCloud2::ConstPtr &hint_cloud_msg)
void publish(const boost::shared_ptr< M > &message) const
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
GLfloat n[6][3]
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::HintedPlaneDetector, nodelet::Nodelet)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual pcl::PointIndices::Ptr getBestCluster(pcl::PointCloud< pcl::PointNormal >::Ptr input_cloud, const std::vector< pcl::PointIndices > &cluster_indices, const jsk_recognition_utils::ConvexPolygon::Ptr hint_convex)
virtual void hintFilter(const pcl::PointCloud< pcl::PointNormal >::Ptr cloud, const jsk_recognition_utils::ConvexPolygon::Ptr hint_convex, pcl::PointIndices &output)
int min_index
virtual void publishPolygon(const jsk_recognition_utils::ConvexPolygon::Ptr convex, ros::Publisher &pub_polygon, ros::Publisher &pub_polygon_array, const pcl::PCLHeader &header)
virtual void planeFilter(const pcl::PointCloud< pcl::PointNormal >::Ptr cloud, const pcl::PointIndices::Ptr indices, const Eigen::Vector3f &normal, pcl::PointIndices &output, pcl::ModelCoefficients &coefficients)
virtual bool detectLargerPlane(pcl::PointCloud< pcl::PointNormal >::Ptr input_cloud, jsk_recognition_utils::ConvexPolygon::Ptr hint_convex)
HintedPlaneDetectorConfig Config
boost::shared_ptr< ros::NodeHandle > pnh_
virtual void euclideanFilter(const pcl::PointCloud< pcl::PointNormal >::Ptr cloud, const pcl::PointIndices::Ptr indices, const jsk_recognition_utils::ConvexPolygon::Ptr hint_convex, pcl::PointIndices &output)
virtual void configCallback(Config &config, uint32_t level)
virtual void densityFilter(const pcl::PointCloud< pcl::PointNormal >::Ptr cloud, const pcl::PointIndices::Ptr indices, pcl::PointIndices &output)
jsk_topic_tools::VitalChecker::Ptr vital_checker_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_cloud_
#define NODELET_INFO(...)
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)
p
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_hint_cloud_
GLfloat v[8][3]
pcl::PointIndices PCLIndicesMsg
pcl::ModelCoefficients PCLModelCoefficientMsg
std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header)


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