primitive_shape_classifier_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: C++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2017, 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 /*
36  * primitive_shape_classifier_nodelet.cpp
37  * Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
38  */
39 
41 #include <algorithm>
42 #include <iterator>
43 #include <pcl/common/centroid.h>
44 #include <pcl/features/boundary.h>
45 #include <pcl/filters/extract_indices.h>
46 #include <pcl/filters/project_inliers.h>
47 #include <pcl/sample_consensus/method_types.h>
48 #include <pcl/sample_consensus/model_types.h>
49 #include <pcl/segmentation/sac_segmentation.h>
51 
52 namespace jsk_pcl_ros
53 {
55  {
56  DiagnosticNodelet::onInit();
57 
58  srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
59  dynamic_reconfigure::Server<Config>::CallbackType f =
60  boost::bind(&PrimitiveShapeClassifier::configCallback, this, _1, _2);
61  srv_->setCallback(f);
62 
63  pub_class_ = advertise<jsk_recognition_msgs::ClassificationResult>(*pnh_, "output", 1);
65  advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "debug/boundary_indices", 1);
67  advertise<sensor_msgs::PointCloud2>(*pnh_, "debug/projected_cloud", 1);
68 
70  }
71 
73  {
74  sub_cloud_.subscribe(*pnh_, "input", 1);
75  sub_normal_.subscribe(*pnh_, "input/normal", 1);
76  sub_indices_.subscribe(*pnh_, "input/indices", 1);
77  sub_polygons_.subscribe(*pnh_, "input/polygons", 1);
78 
79  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
81  sync_->registerCallback(boost::bind(&PrimitiveShapeClassifier::process, this, _1, _2, _3, _4));
82  }
83 
85  {
90  }
91 
92  void PrimitiveShapeClassifier::configCallback(Config& config, uint32_t level)
93  {
94  boost::mutex::scoped_lock lock(mutex_);
95  min_points_num_ = config.min_points_num;
96 
97  sac_max_iterations_ = config.sac_max_iterations;
98  sac_distance_threshold_ = config.sac_distance_threshold;
99  if (config.sac_radius_limit_min < config.sac_radius_limit_max) {
100  sac_radius_limit_min_ = config.sac_radius_limit_min;
101  sac_radius_limit_max_ = config.sac_radius_limit_max;
102  } else {
103  config.sac_radius_limit_min = sac_radius_limit_min_;
104  config.sac_radius_limit_max = sac_radius_limit_max_;
105  }
106 
107  box_threshold_ = config.box_threshold;
108  circle_threshold_ = config.circle_threshold;
109 
110  if (queue_size_ != config.queue_size) {
111  queue_size_ = config.queue_size;
112  if (isSubscribed()) {
113  unsubscribe();
114  subscribe();
115  }
116  }
117  }
118 
119  bool
120  PrimitiveShapeClassifier::checkFrameId(const sensor_msgs::PointCloud2::ConstPtr& ros_cloud,
121  const sensor_msgs::PointCloud2::ConstPtr& ros_normal,
122  const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& ros_indices,
123  const jsk_recognition_msgs::PolygonArray::ConstPtr& ros_polygons)
124  {
125  std::string cloud_topic = ros::names::resolve(sub_cloud_.getTopic());
126  std::string normal_topic = ros::names::resolve(sub_normal_.getTopic());
127  std::string indices_topic = ros::names::resolve(sub_indices_.getTopic());
128  std::string polygons_topic = ros::names::resolve(sub_polygons_.getTopic());
129  if (!jsk_recognition_utils::isSameFrameId(ros_cloud->header, ros_normal->header)) {
130  NODELET_ERROR_STREAM(cloud_topic << " and " << normal_topic << " must have same frame_id");
131  return false;
132  }
133  if (!jsk_recognition_utils::isSameFrameId(ros_cloud->header, ros_indices->header)) {
134  NODELET_ERROR_STREAM(cloud_topic << " and " << indices_topic << " must have same frame_id");
135  return false;
136  }
137  if (!jsk_recognition_utils::isSameFrameId(ros_cloud->header, ros_polygons->header)) {
138  NODELET_ERROR_STREAM(cloud_topic << " and " << polygons_topic << " must have same frame_id");
139  return false;
140  }
141  NODELET_DEBUG_STREAM("Frame id is ok: " << ros_cloud->header.frame_id);
142  return true;
143  }
144 
145  void
146  PrimitiveShapeClassifier::process(const sensor_msgs::PointCloud2::ConstPtr& ros_cloud,
147  const sensor_msgs::PointCloud2::ConstPtr& ros_normal,
148  const jsk_recognition_msgs::ClusterPointIndices::ConstPtr& ros_indices,
149  const jsk_recognition_msgs::PolygonArray::ConstPtr& ros_polygons)
150  {
151  boost::mutex::scoped_lock lock(mutex_);
152 
153  if (!checkFrameId(ros_cloud, ros_normal, ros_indices, ros_polygons)) return;
154 
155  pcl::PointCloud<PointT>::Ptr input(new pcl::PointCloud<PointT>);
156  pcl::fromROSMsg(*ros_cloud, *input);
157 
158  pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
159  pcl::fromROSMsg(*ros_normal, *normal);
160 
161  pcl::ExtractIndices<PointT> ext_input;
162  ext_input.setInputCloud(input);
163  pcl::ExtractIndices<pcl::Normal> ext_normal;
164  ext_normal.setInputCloud(normal);
165 
166  std::vector<jsk_recognition_utils::Polygon::Ptr> polygons
168 
169  jsk_recognition_msgs::ClassificationResult result;
170  result.header = ros_cloud->header;
171  result.classifier = "primitive_shape_classifier";
172  result.target_names.push_back("box");
173  result.target_names.push_back("circle");
174  result.target_names.push_back("other");
175 
176  pcl::PointCloud<PointT>::Ptr projected_cloud(new pcl::PointCloud<PointT>);
177  std::vector<pcl::PointIndices::Ptr> boundary_indices;
178 
179  NODELET_DEBUG_STREAM("Cluster num: " << ros_indices->cluster_indices.size());
180  for (size_t i = 0; i < ros_indices->cluster_indices.size(); ++i)
181  {
182  pcl::PointIndices::Ptr indices(new pcl::PointIndices);
183  indices->indices = ros_indices->cluster_indices[i].indices;
184  NODELET_DEBUG_STREAM("Estimating cluster #" << i << " (" << indices->indices.size() << " points)");
185 
186  pcl::PointCloud<PointT>::Ptr cluster_cloud(new pcl::PointCloud<PointT>);
187  ext_input.setIndices(indices);
188  ext_input.filter(*cluster_cloud);
189 
190  pcl::PointCloud<pcl::Normal>::Ptr cluster_normal(new pcl::PointCloud<pcl::Normal>);
191  ext_normal.setIndices(indices);
192  ext_normal.filter(*cluster_normal);
193 
194  pcl::ModelCoefficients::Ptr support_plane(new pcl::ModelCoefficients);
195  if (!getSupportPlane(cluster_cloud, polygons, support_plane))
196  {
197  NODELET_ERROR_STREAM("cloud " << i << " has no support plane. skipped");
198  continue;
199  }
200 
201  pcl::PointIndices::Ptr b(new pcl::PointIndices);
202  pcl::PointCloud<PointT>::Ptr pc(new pcl::PointCloud<PointT>);
203  float circle_likelihood, box_likelihood;
204  estimate(cluster_cloud, cluster_normal, support_plane,
205  b, pc,
206  circle_likelihood, box_likelihood);
207  boundary_indices.push_back(std::move(b));
208  *projected_cloud += *pc;
209 
210  if (circle_likelihood > circle_threshold_) {
211  // circle
212  result.labels.push_back(1);
213  result.label_names.push_back("circle");
214  result.label_proba.push_back(circle_likelihood);
215  } else if (box_likelihood > box_threshold_) {
216  // box
217  result.labels.push_back(0);
218  result.label_names.push_back("box");
219  result.label_proba.push_back(box_likelihood);
220  } else {
221  // other
222  result.labels.push_back(3);
223  result.label_names.push_back("other");
224  result.label_proba.push_back(0.0);
225  }
226  }
227 
228  // publish results
230  sensor_msgs::PointCloud2 ros_projected_cloud;
231  pcl::toROSMsg(*projected_cloud, ros_projected_cloud);
232  ros_projected_cloud.header = ros_cloud->header;
233  pub_projected_cloud_.publish(ros_projected_cloud);
234  }
235 
237  jsk_recognition_msgs::ClusterPointIndices ros_boundary_indices;
238  ros_boundary_indices.header = ros_cloud->header;
239  for (size_t i = 0; i < boundary_indices.size(); ++i)
240  {
241  pcl_msgs::PointIndices ri;
242  pcl_conversions::moveFromPCL(*boundary_indices[i], ri);
243  ros_boundary_indices.cluster_indices.push_back(ri);
244  }
245  pub_boundary_indices_.publish(ros_boundary_indices);
246  }
247 
248  pub_class_.publish(result);
249  }
250 
251  bool
252  PrimitiveShapeClassifier::estimate(const pcl::PointCloud<PointT>::Ptr& cloud,
253  const pcl::PointCloud<pcl::Normal>::Ptr& normal,
254  const pcl::ModelCoefficients::Ptr& plane,
255  pcl::PointIndices::Ptr& boundary_indices,
256  pcl::PointCloud<PointT>::Ptr& projected_cloud,
257  float& circle_likelihood,
258  float& box_likelihood)
259  {
260  // estimate boundaries
261  pcl::PointCloud<pcl::Boundary>::Ptr boundaries(new pcl::PointCloud<pcl::Boundary>);
262  pcl::BoundaryEstimation<PointT, pcl::Normal, pcl::Boundary> b;
263  b.setInputCloud(cloud);
264  b.setInputNormals(normal);
265  b.setSearchMethod(typename pcl::search::KdTree<PointT>::Ptr(new pcl::search::KdTree<PointT>));
266  b.setAngleThreshold(DEG2RAD(70));
267  b.setRadiusSearch(0.03);
268  b.compute(*boundaries);
269 
270  // set boundaries as indices
271  for (size_t i = 0; i < boundaries->points.size(); ++i)
272  if ((int)boundaries->points[i].boundary_point)
273  boundary_indices->indices.push_back(i);
274 
275  // extract boundaries
276  pcl::PointCloud<PointT>::Ptr boundary_cloud(new pcl::PointCloud<PointT>);
277  pcl::ExtractIndices<PointT> ext;
278  ext.setInputCloud(cloud);
279  ext.setIndices(boundary_indices);
280  ext.filter(*boundary_cloud);
281 
282  // thresholding with min points num
283  if (boundary_indices->indices.size() < min_points_num_)
284  return false;
285 
286  // project to supporting plane
287  pcl::ProjectInliers<PointT> proj;
288  proj.setModelType(pcl::SACMODEL_PLANE);
289  proj.setInputCloud(boundary_cloud);
290  proj.setModelCoefficients(plane);
291  proj.filter(*projected_cloud);
292 
293  // estimate circles
294  pcl::PointIndices::Ptr circle_inliers(new pcl::PointIndices);
295  pcl::ModelCoefficients::Ptr circle_coeffs(new pcl::ModelCoefficients);
296  pcl::PointIndices::Ptr line_inliers(new pcl::PointIndices);
297  pcl::ModelCoefficients::Ptr line_coeffs(new pcl::ModelCoefficients);
298 
299  pcl::SACSegmentation<PointT> seg;
300  seg.setInputCloud(projected_cloud);
301 
302  seg.setOptimizeCoefficients(true);
303  seg.setMethodType(pcl::SAC_RANSAC);
304  seg.setMaxIterations(sac_max_iterations_);
305  seg.setModelType(pcl::SACMODEL_CIRCLE3D);
306  seg.setDistanceThreshold(sac_distance_threshold_);
307  seg.setRadiusLimits(sac_radius_limit_min_, sac_radius_limit_max_);
308  seg.segment(*circle_inliers, *circle_coeffs);
309 
310  seg.setOptimizeCoefficients(true);
311  seg.setMethodType(pcl::SAC_RANSAC);
312  seg.setMaxIterations(sac_max_iterations_);
313  seg.setModelType(pcl::SACMODEL_LINE);
314  seg.setDistanceThreshold(sac_distance_threshold_);
315  seg.segment(*line_inliers, *line_coeffs);
316 
317  // compute likelihood
318  circle_likelihood =
319  1.0f * circle_inliers->indices.size() / projected_cloud->points.size();
320  box_likelihood =
321  1.0f * line_inliers->indices.size() / projected_cloud->points.size();
322 
323  NODELET_DEBUG_STREAM("Projected cloud has " << projected_cloud->points.size() << " points");
324  NODELET_DEBUG_STREAM(circle_inliers->indices.size() << " circle inliers found");
325  NODELET_DEBUG_STREAM("circle confidence: " << circle_likelihood);
326  NODELET_DEBUG_STREAM(line_inliers->indices.size() << " line inliers found");
327  NODELET_DEBUG_STREAM("box confidence: " << box_likelihood);
328 
329  return true;
330  }
331 
332  bool
333  PrimitiveShapeClassifier::getSupportPlane(const pcl::PointCloud<PointT>::Ptr& cloud,
334  const std::vector<jsk_recognition_utils::Polygon::Ptr>& polygons,
335  pcl::ModelCoefficients::Ptr& coeff)
336  {
337  Eigen::Vector4f c;
338  pcl::compute3DCentroid(*cloud, c);
339  Eigen::Vector3f centroid(c[0], c[1], c[2]);
340  Eigen::Vector3f projected;
341  for (size_t i = 0; i < polygons.size(); ++i)
342  {
344  p->project(centroid, projected);
345  if (p->isInside(projected)) {
346  p->toCoefficients(coeff->values);
347  return true;
348  }
349  }
350  return false;
351  }
352 }
353 
std::string getTopic() const
void publish(const boost::shared_ptr< M > &message) const
virtual void configCallback(Config &config, uint32_t level)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual bool estimate(const pcl::PointCloud< PointT >::Ptr &cloud, const pcl::PointCloud< pcl::Normal >::Ptr &normal, const pcl::ModelCoefficients::Ptr &plane, pcl::PointIndices::Ptr &boundary_indices, pcl::PointCloud< PointT >::Ptr &projected_cloud, float &circle_likelihood, float &box_likelihood)
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
static Polygon fromROSMsg(const geometry_msgs::Polygon &polygon)
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
message_filters::Subscriber< jsk_recognition_msgs::ClusterPointIndices > sub_indices_
#define NODELET_ERROR_STREAM(...)
result
boost::shared_ptr< ros::NodeHandle > pnh_
message_filters::Subscriber< jsk_recognition_msgs::PolygonArray > sub_polygons_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_cloud_
virtual void process(const sensor_msgs::PointCloud2::ConstPtr &ros_cloud, const sensor_msgs::PointCloud2::ConstPtr &ros_normal, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &ros_indices, const jsk_recognition_msgs::PolygonArray::ConstPtr &ros_polygons)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
#define NODELET_DEBUG_STREAM(...)
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_normal_
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::PrimitiveShapeClassifier, nodelet::Nodelet)
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)
virtual bool checkFrameId(const sensor_msgs::PointCloud2::ConstPtr &ros_cloud, const sensor_msgs::PointCloud2::ConstPtr &ros_normal, const jsk_recognition_msgs::ClusterPointIndices::ConstPtr &ros_indices, const jsk_recognition_msgs::PolygonArray::ConstPtr &ros_polygons)
p
bool isSameFrameId(const std::string &a, const std::string &b)
c
#define DEG2RAD(x)
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
virtual bool getSupportPlane(const pcl::PointCloud< PointT >::Ptr &cloud, const std::vector< jsk_recognition_utils::Polygon::Ptr > &polygons, pcl::ModelCoefficients::Ptr &coeff)
polygons


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