region_growing_multiple_plane_segmentation_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, 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 
39 #include <pcl/segmentation/conditional_euclidean_clustering.h>
40 #include <pcl/sample_consensus/method_types.h>
41 #include <pcl/sample_consensus/model_types.h>
42 #include <pcl/segmentation/sac_segmentation.h>
43 
44 namespace jsk_pcl_ros
45 {
46 
48  {
49  DiagnosticNodelet::onInit();
50 
51  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
52  dynamic_reconfigure::Server<Config>::CallbackType f =
53  boost::bind (
55  srv_->setCallback (f);
56 
57  pub_polygons_ = advertise<jsk_recognition_msgs::PolygonArray>(
58  *pnh_, "output/polygons", 1);
59  pub_inliers_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(
60  *pnh_, "output/inliers", 1);
61  pub_coefficients_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
62  *pnh_, "output/coefficients", 1);
63  pub_clustering_result_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(
64  *pnh_, "output/clustering_result", 1);
65  pub_latest_time_ = advertise<std_msgs::Float32>(
66  *pnh_, "output/latest_time", 1);
67  pub_average_time_ = advertise<std_msgs::Float32>(
68  *pnh_, "output/average_time", 1);
69  done_initialization_ = true;
71  }
72 
74  {
75  sub_input_.subscribe(*pnh_, "input", 1);
76  sub_normal_.subscribe(*pnh_, "input_normal", 1);
77  sync_
78  = boost::make_shared<
80  sync_->connectInput(sub_input_, sub_normal_);
81  sync_->registerCallback(
83  _1, _2));
84  }
85 
87  {
90  }
91 
93  Config &config, uint32_t level)
94  {
95  boost::mutex::scoped_lock lock(mutex_);
96  angular_threshold_ = config.angular_threshold;
97  distance_threshold_ = config.distance_threshold;
98  max_curvature_ = config.max_curvature;
99  min_size_ = config.min_size;
100  max_size_ = config.max_size;
101  min_area_ = config.min_area;
102  max_area_ = config.max_area;
103  cluster_tolerance_ = config.cluster_tolerance;
105  = config.ransac_refine_outlier_distance_threshold;
107  = config.ransac_refine_max_iterations;
108  }
109 
111  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,
112  const pcl::PointIndices::Ptr& indices,
113  pcl::PointIndices& inliers,
114  pcl::ModelCoefficients& coefficient)
115  {
116  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
117  seg.setOptimizeCoefficients (true);
118  seg.setMethodType (pcl::SAC_RANSAC);
119  seg.setDistanceThreshold (ransac_refine_outlier_distance_threshold_);
120  seg.setInputCloud(cloud);
121  seg.setIndices(indices);
122  seg.setMaxIterations (ransac_refine_max_iterations_);
123  seg.setModelType (pcl::SACMODEL_PLANE);
124  seg.segment(inliers, coefficient);
125  }
126 
128  const sensor_msgs::PointCloud2::ConstPtr& msg,
129  const sensor_msgs::PointCloud2::ConstPtr& normal_msg)
130  {
131  boost::mutex::scoped_lock lock(mutex_);
132  if (!done_initialization_) {
133  return;
134  }
135  vital_checker_->poke();
136  {
139  pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
140  pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
141  pcl::fromROSMsg(*msg, *cloud);
142  pcl::fromROSMsg(*normal_msg, *normal);
143  // concatenate fields
144  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
145  all_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
146  pcl::concatenateFields(*cloud, *normal, *all_cloud);
147  pcl::PointIndices::Ptr indices (new pcl::PointIndices);
148  for (size_t i = 0; i < all_cloud->points.size(); i++) {
149  if (!std::isnan(all_cloud->points[i].x) &&
150  !std::isnan(all_cloud->points[i].y) &&
151  !std::isnan(all_cloud->points[i].z) &&
152  !std::isnan(all_cloud->points[i].normal_x) &&
153  !std::isnan(all_cloud->points[i].normal_y) &&
154  !std::isnan(all_cloud->points[i].normal_z) &&
155  !std::isnan(all_cloud->points[i].curvature)) {
156  if (all_cloud->points[i].curvature < max_curvature_) {
157  indices->indices.push_back(i);
158  }
159  }
160  }
161  pcl::ConditionalEuclideanClustering<pcl::PointXYZRGBNormal> cec (true);
162 
163  // vector of pcl::PointIndices
164  pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters);
165  cec.setInputCloud (all_cloud);
166  cec.setIndices(indices);
167  cec.setClusterTolerance(cluster_tolerance_);
168  cec.setMinClusterSize(min_size_);
169  //cec.setMaxClusterSize(max_size_);
170  {
171  boost::mutex::scoped_lock lock2(global_custom_condigion_function_mutex);
173  cec.setConditionFunction(
175  //ros::Time before = ros::Time::now();
176  cec.segment (*clusters);
177  // ros::Time end = ros::Time::now();
178  // ROS_INFO("segment took %f sec", (before - end).toSec());
179  }
180  // Publish raw cluster information
181  // pcl::IndicesCluster is typdefed as std::vector<pcl::PointIndices>
182  jsk_recognition_msgs::ClusterPointIndices ros_clustering_result;
183  ros_clustering_result.cluster_indices
184  = pcl_conversions::convertToROSPointIndices(*clusters, msg->header);
185  ros_clustering_result.header = msg->header;
186  pub_clustering_result_.publish(ros_clustering_result);
187 
188  // estimate planes
189  std::vector<pcl::PointIndices::Ptr> all_inliers;
190  std::vector<pcl::ModelCoefficients::Ptr> all_coefficients;
191  jsk_recognition_msgs::PolygonArray ros_polygon;
192  ros_polygon.header = msg->header;
193  for (size_t i = 0; i < clusters->size(); i++) {
194  pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices);
195  pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients);
196  pcl::PointIndices::Ptr cluster = boost::make_shared<pcl::PointIndices>((*clusters)[i]);
197  ransacEstimation(cloud, cluster,
198  *plane_inliers, *plane_coefficients);
199  if (plane_inliers->indices.size() > 0) {
201  = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZRGB>(
202  cloud, plane_inliers, plane_coefficients);
203  if (convex) {
204  if (min_area_ > convex->area() || convex->area() > max_area_) {
205  continue;
206  }
207  {
208  // check direction consistency of coefficients and convex
209  Eigen::Vector3f coefficient_normal(plane_coefficients->values[0],
210  plane_coefficients->values[1],
211  plane_coefficients->values[2]);
212  if (convex->getNormalFromVertices().dot(coefficient_normal) < 0) {
213  convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex());
214  }
215  }
216  // Normal should direct to origin
217  {
218  Eigen::Vector3f p = convex->getPointOnPlane();
219  Eigen::Vector3f n = convex->getNormal();
220  if (p.dot(n) > 0) {
221  convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex());
222  Eigen::Vector3f new_normal = convex->getNormal();
223  plane_coefficients->values[0] = new_normal[0];
224  plane_coefficients->values[1] = new_normal[1];
225  plane_coefficients->values[2] = new_normal[2];
226  plane_coefficients->values[3] = convex->getD();
227  }
228  }
229 
230  geometry_msgs::PolygonStamped polygon;
231  polygon.polygon = convex->toROSMsg();
232  polygon.header = msg->header;
233  ros_polygon.polygons.push_back(polygon);
234  all_inliers.push_back(plane_inliers);
235  all_coefficients.push_back(plane_coefficients);
236  }
237  }
238  }
239 
240  jsk_recognition_msgs::ClusterPointIndices ros_indices;
241  ros_indices.cluster_indices =
242  pcl_conversions::convertToROSPointIndices(all_inliers, msg->header);
243  ros_indices.header = msg->header;
244  pub_inliers_.publish(ros_indices);
245  jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients;
246  ros_coefficients.header = msg->header;
247  ros_coefficients.coefficients =
249  all_coefficients, msg->header);
250  pub_coefficients_.publish(ros_coefficients);
251  pub_polygons_.publish(ros_polygon);
252  }
253  }
254 
258 }
259 
boost::shared_ptr< message_filters::Synchronizer< NormalSyncPolicy > > sync_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_normal_
void publish(const boost::shared_ptr< M > &message) const
GLfloat n[6][3]
std::vector< PCLModelCoefficientMsg > convertToROSModelCoefficients(const std::vector< pcl::ModelCoefficients::Ptr > &coefficients, const std_msgs::Header &header)
virtual void segment(const sensor_msgs::PointCloud2::ConstPtr &msg, const sensor_msgs::PointCloud2::ConstPtr &normal_msg)
virtual ScopedWallDurationReporter reporter()
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
boost::shared_ptr< ros::NodeHandle > pnh_
void concatenateFields(PointCloud< PointXYZ > &cloud_xyz, PointCloud< RGB > &cloud_rgb, PointCloud< PointXYZRGB > &cloud)
std::vector< PCLIndicesMsg > convertToROSPointIndices(const std::vector< pcl::PointIndices::Ptr > cluster_indices, const std_msgs::Header &header)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
boost::mutex mutex
global mutex.
virtual void ransacEstimation(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::PointIndices::Ptr &indices, pcl::PointIndices &inliers, pcl::ModelCoefficients &coefficient)
jsk_topic_tools::VitalChecker::Ptr vital_checker_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::RegionGrowingMultiplePlaneSegmentation, nodelet::Nodelet)
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
static void setCondifionFunctionParameter(const double angular_threshold, const double distance_threshold)
cloud
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_input_
static bool regionGrowingFunction(const pcl::PointXYZRGBNormal &a, const pcl::PointXYZRGBNormal &b, float distance)


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