organized_multi_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) 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 
37 #include "jsk_recognition_msgs/ModelCoefficientsArray.h"
38 #include <pcl/segmentation/impl/organized_multi_plane_segmentation.hpp>
39 #include <pcl/filters/extract_indices.h>
40 #include <pcl/kdtree/kdtree_flann.h>
41 #include <set>
42 #include <Eigen/StdVector>
43 #include <pcl/surface/convex_hull.h>
44 #include <pcl/filters/project_inliers.h>
45 #include <pcl/features/integral_image_normal.h>
46 
49 
50 #include <boost/format.hpp>
51 #include <pcl/common/centroid.h>
52 #include <visualization_msgs/Marker.h>
54 
55 #include <pcl/sample_consensus/method_types.h>
56 #include <pcl/sample_consensus/model_types.h>
57 #include <pcl/segmentation/sac_segmentation.h>
58 #include <pcl/surface/convex_hull.h>
59 
61 
62 namespace jsk_pcl_ros
63 {
64 
66  {
67  ConnectionBasedNodelet::onInit();
68  pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
70  // prepare diagnostics
73  diagnostic_updater_->setHardwareID(getName());
75  getName() + "::NormalEstimation",
76  boost::bind(
78  this,
79  _1));
81  getName() + "::PlaneSegmentation",
82  boost::bind(
84  this,
85  _1));
86  double vital_rate;
87  pnh_->param("vital_rate", vital_rate, 1.0);
89  new jsk_topic_tools::VitalChecker(1 / vital_rate));
91  new jsk_topic_tools::VitalChecker(1 / vital_rate));
92  estimate_normal_ = true;
93  pnh_->getParam("estimate_normal", estimate_normal_);
95  // prepare publishers
97  pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output", 1);
98  polygon_pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output_polygon", 1);
100  = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_, "output_coefficients", 1);
101  org_pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output_nonconnected", 1);
103  = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output_nonconnected_polygon", 1);
105  = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_,
106  "output_nonconnected_coefficients", 1);
107 
108  refined_pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_,
109  "output_refined", 1);
111  = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output_refined_polygon", 1);
113  = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_,
114  "output_refined_coefficients", 1);
115 
117  = advertise<visualization_msgs::Marker>(*pnh_,
118  "debug_connection_map", 1);
119 
120  if (estimate_normal_) {
122  = advertise<sensor_msgs::PointCloud2>(*pnh_, "output_normal", 1);
123  }
124 
125  srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
126  dynamic_reconfigure::Server<Config>::CallbackType f =
127  boost::bind (
129  srv_->setCallback (f);
130 
131  diagnostics_timer_ = pnh_->createTimer(
132  ros::Duration(1.0),
134  this,
135  _1));
137  }
138 
140  const std::vector<pcl::ModelCoefficients>& coefficients,
141  std::vector<pcl::ModelCoefficients>& output_coefficients)
142  {
143  output_coefficients.resize(coefficients.size());
144  for (size_t i = 0; i < coefficients.size(); i++) {
145  pcl::ModelCoefficients plane_coefficient = coefficients[i];
146  jsk_recognition_utils::Plane plane(coefficients[i].values);
147 
148  Eigen::Vector3f p = plane.getPointOnPlane();
149  Eigen::Vector3f n = plane.getNormal();
150  if (p.dot(n) < 0) {
151  output_coefficients[i] = plane_coefficient;
152  }
153  else {
154  jsk_recognition_utils::Plane flip = plane.flip();
155  pcl::ModelCoefficients new_coefficient;
156  flip.toCoefficients(new_coefficient.values);
157  output_coefficients[i] = new_coefficient;
158  }
159  }
160  }
161 
163  {
164  sub_ = pnh_->subscribe("input", 1,
166  }
167 
169  {
170  sub_.shutdown();
171  }
172 
174  {
175  boost::mutex::scoped_lock lock(mutex_);
176  min_size_ = config.min_size;
177  angular_threshold_ = config.angular_threshold;
178  distance_threshold_ = config.distance_threshold;
179  max_curvature_ = config.max_curvature;
180  connect_plane_angle_threshold_ = config.connect_plane_angle_threshold;
181  connect_distance_threshold_ = config.connect_distance_threshold;
182  max_depth_change_factor_ = config.max_depth_change_factor;
183  normal_smoothing_size_ = config.normal_smoothing_size;
184  depth_dependent_smoothing_ = config.depth_dependent_smoothing;
185  estimation_method_ = config.estimation_method;
186  border_policy_ignore_ = config.border_policy_ignore;
187  publish_normal_ = config.publish_normal;
188  ransac_refine_coefficients_ = config.ransac_refine_coefficients;
190  = config.ransac_refine_outlier_distance_threshold;
191  min_refined_area_threshold_ = config.min_refined_area_threshold;
192  max_refined_area_threshold_ = config.max_refined_area_threshold;
193  //concave_alpha_ = config.concave_alpha;
194  }
195 
197  const pcl::PointCloud<PointT>::Ptr& input,
198  const std::vector<pcl::ModelCoefficients>& model_coefficients,
199  const std::vector<pcl::PointIndices>& boundary_indices,
201  {
202  NODELET_DEBUG("size of model_coefficients: %lu", model_coefficients.size());
203  if (model_coefficients.size() == 0) {
204  return; // do nothing
205  }
206 
207  if (model_coefficients.size() == 1) {
208  connection_map[0]= std::vector<int>();
209  return;
210  }
211 
212  pcl::ExtractIndices<PointT> extract;
213  extract.setInputCloud(input);
214  for (size_t i = 0; i < model_coefficients.size(); i++) {
215  // initialize connection_map[i]
216  connection_map[i] = std::vector<int>();
217  connection_map[i].push_back(i);
218  for (size_t j = i + 1; j < model_coefficients.size(); j++) {
219  // check if i and j can be connected
220  pcl::ModelCoefficients a_coefficient = model_coefficients[i];
221  pcl::ModelCoefficients b_coefficient = model_coefficients[j];
222  Eigen::Vector3f a_normal(a_coefficient.values[0], a_coefficient.values[1], a_coefficient.values[2]);
223  Eigen::Vector3f b_normal(b_coefficient.values[0], b_coefficient.values[1], b_coefficient.values[2]);
224  double a_distance = a_coefficient.values[3];
225  double b_distance = b_coefficient.values[3];
226  // force to check the coefficients is normalized
227  if (a_normal.norm() != 1.0) {
228  a_distance = a_distance / a_normal.norm();
229  a_normal = a_normal / a_normal.norm();
230  }
231  if (b_normal.norm() != 1.0) {
232  b_distance = b_distance / b_normal.norm();
233  b_normal = b_normal / b_normal.norm();
234  }
235  double theta = fabs(acos(a_normal.dot(b_normal)));
236  NODELET_DEBUG("%lu - %lu angle: %f", i, j, theta);
237  if (theta > M_PI / 2.0) {
238  theta = M_PI - theta;
239  }
240  if (theta > connect_plane_angle_threshold_) {
241  continue;
242  }
243  // the planes are near enough as a plane formula.
244 
245  // compute the distance between two boundaries.
246  // if they are near enough, we can regard these two map should connect
247  pcl::PointIndices::Ptr a_indices
248  = boost::make_shared<pcl::PointIndices>(boundary_indices[i]);
249  pcl::PointIndices::Ptr b_indices
250  = boost::make_shared<pcl::PointIndices>(boundary_indices[j]);
251  pcl::PointCloud<PointT> a_cloud, b_cloud;
252  extract.setIndices(a_indices);
253  extract.filter(a_cloud);
254  extract.setIndices(b_indices);
255  extract.filter(b_cloud);
256  if (a_cloud.points.size() > 0) {
257  pcl::KdTreeFLANN<PointT> kdtree;
258  kdtree.setInputCloud(a_cloud.makeShared());
259  bool foundp = false;
260  for (size_t pi = 0; pi < b_cloud.points.size(); pi++) {
261  PointT p = b_cloud.points[pi];
262  std::vector<int> k_indices;
263  std::vector<float> k_sqr_distances;
264  if (kdtree.radiusSearch(
265  p, connect_distance_threshold_, k_indices, k_sqr_distances, 1) > 0) {
266  NODELET_DEBUG("%lu - %lu connected", i, j);
267  foundp = true;
268  break;
269  }
270  }
271  if (foundp) {
272  connection_map[i].push_back(j);
273  }
274  }
275  }
276  }
277  }
278 
280  const std::vector<pcl::PointIndices>& inlier_indices,
281  const std_msgs::Header& header,
282  jsk_recognition_msgs::ClusterPointIndices& output_indices)
283  {
284  for (size_t i = 0; i < inlier_indices.size(); i++) {
285  pcl::PointIndices inlier = inlier_indices[i];
286  PCLIndicesMsg one_indices;
287  one_indices.header = header;
288  one_indices.indices = inlier.indices;
289  output_indices.cluster_indices.push_back(one_indices);
290  }
291  }
292 
294  const pcl::PointCloud<PointT>& input,
295  geometry_msgs::Polygon& polygon)
296  {
297  for (size_t i = 0; i < input.points.size(); i++) {
298  geometry_msgs::Point32 point;
299  point.x = input.points[i].x;
300  point.y = input.points[i].y;
301  point.z = input.points[i].z;
302  polygon.points.push_back(point);
303  }
304  }
305 
307  const pcl::PointCloud<PointT>::Ptr& input,
308  const std_msgs::Header& header,
309  const std::vector<pcl::PointIndices>& inlier_indices,
310  const std::vector<pcl::PointIndices>& boundary_indices,
311  const std::vector<pcl::ModelCoefficients>& model_coefficients,
312  const jsk_recognition_utils::IntegerGraphMap& connection_map,
313  std::vector<pcl::PointIndices>& output_indices,
314  std::vector<pcl::ModelCoefficients>& output_coefficients,
315  std::vector<pcl::PointCloud<PointT> >& output_boundary_clouds)
316  {
317  std::vector<std::set<int> > cloud_sets;
318  NODELET_DEBUG("connection_map:");
319  for (jsk_recognition_utils::IntegerGraphMap::const_iterator it = connection_map.begin();
320  it != connection_map.end();
321  ++it) {
322  int from_index = it->first;
323  std::stringstream ss;
324  ss << "connection map: " << from_index << " [";
325  for (size_t i = 0; i < it->second.size(); i++) {
326  ss << i << ", ";
327  }
328  NODELET_DEBUG("%s", ss.str().c_str());
329  }
330 
331  jsk_recognition_utils::buildAllGroupsSetFromGraphMap(connection_map, cloud_sets);
332  connected_plane_num_counter_.add(cloud_sets.size());
333  for (size_t i = 0; i < cloud_sets.size(); i++) {
334  pcl::PointIndices one_indices;
335  pcl::PointIndices one_boundaries;
336  std::vector<float> new_coefficients;
337  new_coefficients.resize(4, 0);
338  for (std::set<int>::iterator it = cloud_sets[i].begin();
339  it != cloud_sets[i].end();
340  ++it) {
341  NODELET_DEBUG("%lu includes %d", i, *it);
342  new_coefficients = model_coefficients[*it].values;
343  pcl::PointIndices inlier = inlier_indices[*it];
344  pcl::PointIndices boundary_inlier = boundary_indices[*it];
345  // append indices...
346  one_indices = *jsk_recognition_utils::addIndices(one_indices, inlier);
347  one_boundaries = *jsk_recognition_utils::addIndices(one_boundaries, boundary_inlier);
348  }
349  if (one_indices.indices.size() == 0) {
350  continue;
351  }
352  // normalize coefficients
353  double norm = sqrt(new_coefficients[0] * new_coefficients[0]
354  + new_coefficients[1] * new_coefficients[1]
355  + new_coefficients[2] * new_coefficients[2]);
356  new_coefficients[0] /= norm;
357  new_coefficients[1] /= norm;
358  new_coefficients[2] /= norm;
359  new_coefficients[3] /= norm;
360 
361  // take the average of the coefficients
362  pcl::ModelCoefficients pcl_new_coefficients;
363  pcl_new_coefficients.values = new_coefficients;
364  // estimate concave hull
365  pcl::PointIndices::Ptr indices_ptr
366  = boost::make_shared<pcl::PointIndices>(one_boundaries);
367  pcl::ModelCoefficients::Ptr coefficients_ptr
368  = boost::make_shared<pcl::ModelCoefficients>(pcl_new_coefficients);
370  = jsk_recognition_utils::convexFromCoefficientsAndInliers<PointT>(
371  input, indices_ptr, coefficients_ptr);
372  if (convex) {
373  pcl::PointCloud<PointT> chull_output;
374  convex->boundariesToPointCloud<PointT>(chull_output);
375  output_indices.push_back(one_indices);
376  output_coefficients.push_back(pcl_new_coefficients);
377  output_boundary_clouds.push_back(chull_output);
378  }
379  else {
380  NODELET_ERROR("failed to build convex");
381  }
382  }
383 
384  }
385 
387  // simple PCL wrapper
390  pcl::PointCloud<PointT>::Ptr input,
391  pcl::PointCloud<pcl::Normal>::Ptr normal,
392  PlanarRegionVector& regions,
393  std::vector<pcl::ModelCoefficients>& model_coefficients,
394  std::vector<pcl::PointIndices>& inlier_indices,
395  pcl::PointCloud<pcl::Label>::Ptr& labels,
396  std::vector<pcl::PointIndices>& label_indices,
397  std::vector<pcl::PointIndices>& boundary_indices)
398  {
400  pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
401  mps.setMinInliers(min_size_);
402  mps.setAngularThreshold(angular_threshold_);
403  mps.setDistanceThreshold(distance_threshold_);
404  mps.setMaximumCurvature(max_curvature_);
405  mps.setInputCloud(input);
406  mps.setInputNormals(normal);
407  {
409  mps.segmentAndRefine(
410  regions, model_coefficients, inlier_indices,
411  labels, label_indices, boundary_indices);
412  }
413  }
414 
416  const std_msgs::Header& header,
417  const pcl::PointCloud<PointT>::Ptr input,
418  ros::Publisher& indices_pub,
419  ros::Publisher& polygon_pub,
421  const std::vector<pcl::PointIndices>& inlier_indices,
422  const std::vector<pcl::PointCloud<PointT> >& boundaries,
423  const std::vector<pcl::ModelCoefficients>& model_coefficients)
424  {
425  jsk_recognition_msgs::ClusterPointIndices indices;
426  jsk_recognition_msgs::ModelCoefficientsArray coefficients_array;
427  jsk_recognition_msgs::PolygonArray polygon_array;
428  indices.header = header;
429  polygon_array.header = header;
430  coefficients_array.header = header;
431 
433  // publish inliers
435  pclIndicesArrayToClusterPointIndices(inlier_indices, header, indices);
436  indices_pub.publish(indices);
437 
439  // boundaries as polygon
441  for (size_t i = 0; i < boundaries.size(); i++) {
442  geometry_msgs::PolygonStamped polygon;
443  pcl::PointCloud<PointT> boundary_cloud = boundaries[i];
444  pointCloudToPolygon(boundary_cloud, polygon.polygon);
445  polygon.header = header;
446  polygon_array.polygons.push_back(polygon);
447  }
448  polygon_pub.publish(polygon_array);
449 
451  // publish coefficients
453  for (size_t i = 0; i < model_coefficients.size(); i++) {
454  PCLModelCoefficientMsg coefficient;
455  coefficient.values = model_coefficients[i].values;
456  coefficient.header = header;
457  coefficients_array.coefficients.push_back(coefficient);
458  }
459  coefficients_pub.publish(coefficients_array);
460  }
461 
463  const std_msgs::Header& header,
464  const pcl::PointCloud<PointT>::Ptr input,
465  ros::Publisher& indices_pub,
466  ros::Publisher& polygon_pub,
468  const std::vector<pcl::PointIndices>& inlier_indices,
469  const std::vector<pcl::PointIndices>& boundary_indices,
470  const std::vector<pcl::ModelCoefficients>& model_coefficients)
471  {
472  // convert boundary_indices into boundary pointcloud
473  std::vector<pcl::PointCloud<PointT> > boundaries;
474  pcl::ExtractIndices<PointT> extract;
475  extract.setInputCloud(input);
476  for (size_t i = 0; i < boundary_indices.size(); i++) {
477  pcl::PointCloud<PointT> boundary_cloud;
478  pcl::PointIndices boundary_one_indices = boundary_indices[i];
479  pcl::PointIndices::Ptr indices_ptr = boost::make_shared<pcl::PointIndices>(boundary_indices[i]);
480  extract.setIndices(indices_ptr);
481  extract.filter(boundary_cloud);
482  boundaries.push_back(boundary_cloud);
483  }
484 
486  header, input, indices_pub, polygon_pub, coefficients_pub,
487  inlier_indices, boundaries, model_coefficients);
488  }
489 
492  const pcl::PointCloud<PointT>::Ptr cloud,
493  const std::vector<pcl::PointIndices>& inliers,
494  const std_msgs::Header& header)
495  {
497  // visualize connection as lines
499  visualization_msgs::Marker connection_marker;
500  connection_marker.type = visualization_msgs::Marker::LINE_LIST;
501  connection_marker.scale.x = 0.01;
502  connection_marker.header = header;
503  connection_marker.pose.orientation.w = 1.0;
504  connection_marker.color = jsk_topic_tools::colorCategory20(0);
505 
507  // first, compute centroids for each clusters
510  for (size_t i = 0; i < inliers.size(); i++) {
511  pcl::PointIndices::Ptr target_inliers
512  = boost::make_shared<pcl::PointIndices>(inliers[i]);
513  pcl::PointCloud<PointT>::Ptr target_cloud (new pcl::PointCloud<PointT>);
514  Eigen::Vector4f centroid;
515  pcl::ExtractIndices<PointT> ex;
516  ex.setInputCloud(cloud);
517  ex.setIndices(target_inliers);
518  ex.filter(*target_cloud);
519  pcl::compute3DCentroid(*target_cloud, centroid);
520  Eigen::Vector3f centroid_3f(centroid[0], centroid[1], centroid[2]);
521  centroids.push_back(centroid_3f);
522  }
523 
524  for (jsk_recognition_utils::IntegerGraphMap::iterator it = connection_map.begin();
525  it != connection_map.end();
526  ++it) {
527  // from = i
528  int from_index = it->first;
529  std::vector<int> the_connection_map = connection_map[from_index];
530  for (size_t j = 0; j < the_connection_map.size(); j++) {
531  int to_index = the_connection_map[j];
532  //std::cout << "connection: " << from_index << " --> " << to_index << std::endl;
533  Eigen::Vector3f from_point = centroids[from_index];
534  Eigen::Vector3f to_point = centroids[to_index];
535  geometry_msgs::Point from_point_ros, to_point_ros;
536  jsk_recognition_utils::pointFromVectorToXYZ<Eigen::Vector3f, geometry_msgs::Point>(
537  from_point, from_point_ros);
538  jsk_recognition_utils::pointFromVectorToXYZ<Eigen::Vector3f, geometry_msgs::Point>(
539  to_point, to_point_ros);
540  connection_marker.points.push_back(from_point_ros);
541  connection_marker.points.push_back(to_point_ros);
542  connection_marker.colors.push_back(jsk_topic_tools::colorCategory20(from_index));
543  connection_marker.colors.push_back(jsk_topic_tools::colorCategory20(from_index));
544  }
545  }
546  pub_connection_marker_.publish(connection_marker);
547  }
548 
550  pcl::PointCloud<PointT>::Ptr input,
551  pcl::PointCloud<pcl::Normal>::Ptr normal,
552  const std_msgs::Header& header)
553  {
554  PlanarRegionVector regions;
555  std::vector<pcl::ModelCoefficients> model_coefficients;
556  std::vector<pcl::PointIndices> inlier_indices;
557  pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>());
558  std::vector<pcl::PointIndices> label_indices;
559  std::vector<pcl::PointIndices> boundary_indices;
561  // segment planes based on pcl's organized multi plane
562  // segmentation.
564  segmentOrganizedMultiPlanes(input, normal, regions, model_coefficients,
565  inlier_indices, labels, label_indices,
566  boundary_indices);
567  std::vector<pcl::ModelCoefficients> fixed_model_coefficients;
568  forceToDirectOrigin(model_coefficients, fixed_model_coefficients);
569  model_coefficients = fixed_model_coefficients;
570 
571  original_plane_num_counter_.add(regions.size());
573  header, input,
575  inlier_indices, boundary_indices, model_coefficients);
576 
578  // segmentation by PCL organized multiplane segmentation
579  // is not enough. we "connect" planes like graph problem.
582  connectPlanesMap(input, model_coefficients, boundary_indices, connection_map);
583  publishMarkerOfConnection(connection_map, input, inlier_indices, header);
584  std::vector<pcl::PointIndices> output_nonrefined_indices;
585  std::vector<pcl::ModelCoefficients> output_nonrefined_coefficients;
586  std::vector<pcl::PointCloud<PointT> > output_nonrefined_boundary_clouds;
587  buildConnectedPlanes(input, header,
588  inlier_indices,
589  boundary_indices,
590  model_coefficients,
591  connection_map,
592  output_nonrefined_indices,
593  output_nonrefined_coefficients,
594  output_nonrefined_boundary_clouds);
595  std::vector<pcl::ModelCoefficients> fixed_output_nonrefined_coefficients;
596  forceToDirectOrigin(output_nonrefined_coefficients, fixed_output_nonrefined_coefficients);
597  output_nonrefined_coefficients = fixed_output_nonrefined_coefficients;
599  header, input,
601  output_nonrefined_indices,
602  output_nonrefined_boundary_clouds,
603  output_nonrefined_coefficients);
605  // refine coefficients based on RANSAC
608  std::vector<pcl::PointIndices> refined_inliers;
609  std::vector<pcl::ModelCoefficients> refined_coefficients;
610  std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> refined_convexes;
612  input, output_nonrefined_indices, output_nonrefined_coefficients,
613  refined_inliers, refined_coefficients, refined_convexes);
614  std::vector<pcl::PointCloud<PointT> > refined_boundary_clouds;
615  for (size_t i = 0; i < refined_convexes.size(); i++) {
616  pcl::PointCloud<PointT> refined_boundary;
617  refined_convexes[i]->boundariesToPointCloud(refined_boundary);
618  refined_boundary_clouds.push_back(refined_boundary);
619  }
620  std::vector<pcl::ModelCoefficients> fixed_refined_coefficients;
621  forceToDirectOrigin(refined_coefficients, fixed_refined_coefficients);
622  refined_coefficients = fixed_refined_coefficients;
624  header, input,
626  refined_inliers, refined_boundary_clouds, refined_coefficients);
627  }
628  }
629 
631  const pcl::PointCloud<PointT>::Ptr input,
632  const std::vector<pcl::PointIndices>& input_indices,
633  const std::vector<pcl::ModelCoefficients>& input_coefficients,
634  std::vector<pcl::PointIndices>& output_indices,
635  std::vector<pcl::ModelCoefficients>& output_coefficients,
636  std::vector<jsk_recognition_utils::ConvexPolygon::Ptr>& output_convexes)
637  {
640  for (size_t i = 0; i < input_indices.size(); i++) {
641  pcl::PointIndices::Ptr input_indices_ptr
642  = boost::make_shared<pcl::PointIndices>(input_indices[i]);
643  Eigen::Vector3f normal(input_coefficients[i].values[0],
644  input_coefficients[i].values[1],
645  input_coefficients[i].values[2]);
646  normal.normalize();
648  // run RANSAC
650  pcl::SACSegmentation<PointT> seg;
651  seg.setOptimizeCoefficients (true);
652  seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
653  seg.setMethodType (pcl::SAC_RANSAC);
654  seg.setDistanceThreshold (ransac_refine_outlier_distance_threshold_);
655  seg.setInputCloud(input);
656  seg.setIndices(input_indices_ptr);
657  seg.setMaxIterations (10000);
658  seg.setAxis(normal);
659  seg.setEpsAngle(pcl::deg2rad(20.0));
660  pcl::PointIndices::Ptr refined_inliers (new pcl::PointIndices);
661  pcl::ModelCoefficients::Ptr refined_coefficients(new pcl::ModelCoefficients);
662  seg.segment(*refined_inliers, *refined_coefficients);
663  if (refined_inliers->indices.size() > 0) {
665  // compute boundaries from convex hull of
667  jsk_recognition_utils::ConvexPolygon::Ptr convex = jsk_recognition_utils::convexFromCoefficientsAndInliers<PointT>(
668  input, refined_inliers, refined_coefficients);
669  if (convex) {
670  // check area threshold
671  double area = convex->area();
672  if (area > min_refined_area_threshold_ &&
674  output_convexes.push_back(convex);
675  output_indices.push_back(*refined_inliers);
676  output_coefficients.push_back(*refined_coefficients);
677  }
678  }
679  }
680  }
681  }
682 
683  void OrganizedMultiPlaneSegmentation::estimateNormal(pcl::PointCloud<PointT>::Ptr input,
684  pcl::PointCloud<pcl::Normal>::Ptr output)
685  {
687  pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
688  if (estimation_method_ == 0) {
689  ne.setNormalEstimationMethod (ne.AVERAGE_3D_GRADIENT);
690  }
691  else if (estimation_method_ == 1) {
692  ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
693  }
694  else if (estimation_method_ == 2) {
695  ne.setNormalEstimationMethod (ne.AVERAGE_DEPTH_CHANGE);
696  }
697  else {
698  NODELET_FATAL("unknown estimation method, force to use COVARIANCE_MATRIX: %d",
700  ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
701  }
702 
703  if (border_policy_ignore_) {
704  ne.setBorderPolicy(pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_IGNORE);
705  }
706  else {
707  ne.setBorderPolicy(pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_MIRROR);
708  }
709 
710  ne.setMaxDepthChangeFactor(max_depth_change_factor_);
711  ne.setNormalSmoothingSize(normal_smoothing_size_);
712  ne.setDepthDependentSmoothing(depth_dependent_smoothing_);
713  ne.setInputCloud(input);
714  ne.compute(*output);
715  }
716 
718  (const sensor_msgs::PointCloud2::ConstPtr& msg)
719  {
720  boost::mutex::scoped_lock lock(mutex_);
721 
722  // if estimate_normal_ is true, we run integral image normal estimation
723  // before segmenting planes
724  pcl::PointCloud<PointT>::Ptr input(new pcl::PointCloud<PointT>());
725  pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>());
726  pcl::fromROSMsg(*msg, *input);
727 
728  if (estimate_normal_) {
730  estimateNormal(input, normal);
731  // publish normal to ros
732  if (publish_normal_) {
733  sensor_msgs::PointCloud2 normal_ros_cloud;
734  pcl::toROSMsg(*normal, normal_ros_cloud);
735  normal_ros_cloud.header = msg->header;
736  normal_pub_.publish(normal_ros_cloud);
737  }
738  }
739  else {
740  pcl::fromROSMsg(*msg, *normal);
741  }
742 
743  segmentFromNormals(input, normal, msg->header);
744  diagnostic_updater_->update();
745  }
746 
749  {
750  if (estimate_normal_) {
751  bool alivep = normal_estimation_vital_checker_->isAlive();
752  if (alivep) {
753  stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "NormalEstimation running");
755  "Time to estimate normal", normal_estimation_time_acc_, stat);
756  // normal estimation parameters
757  if (estimation_method_ == 0) {
758  stat.add("Estimation Method", "AVERAGE_3D_GRADIENT");
759  }
760  else if (estimation_method_ == 1) {
761  stat.add("Estimation Method", "COVARIANCE_MATRIX");
762  }
763  else if (estimation_method_ == 2) {
764  stat.add("Estimation Method", "AVERAGE_DEPTH_CHANGE");
765  }
766  if (border_policy_ignore_) {
767  stat.add("Border Policy", "ignore");
768  }
769  else {
770  stat.add("Border Policy", "mirror");
771  }
772  stat.add("Max Depth Change Factor", max_depth_change_factor_);
773  stat.add("Normal Smoothing Size", normal_smoothing_size_);
775  stat.add("Depth Dependent Smooting", "Enabled");
776  }
777  else {
778  stat.add("Depth Dependent Smooting", "Disabled");
779  }
780  if (publish_normal_) {
781  stat.add("Publish Normal", "Enabled");
782  }
783  else {
784  stat.add("Publish Normal", "Disabled");
785  }
786  }
787  else {
788  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
789  (boost::format("NormalEstimation not running for %f sec")
790  % normal_estimation_vital_checker_->deadSec()).str());
791  }
792 
793  }
794  else {
795  stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
796  "NormalEstimation is not activated");
797  }
798  }
799 
802  {
803  bool alivep = plane_segmentation_vital_checker_->isAlive();
804  if (alivep) {
805  stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
806  "PlaneSegmentation running");
808  "Time to segment planes", plane_segmentation_time_acc_, stat);
811  "Time to refine by RANSAC", ransac_refinement_time_acc_, stat);
812  }
813  stat.add("Minimum Inliers", min_size_);
814  stat.add("Angular Threshold (rad)", angular_threshold_);
815  stat.add("Angular Threshold (deg)", angular_threshold_ / M_PI * 180.0);
816  stat.add("Distance Threshold", distance_threshold_);
817  stat.add("Max Curvature", max_curvature_);
819  stat.add("Use RANSAC refinement", "True");
820  stat.add("RANSAC refinement distance threshold",
822  }
823  else {
824  stat.add("Use RANSAC refinement", "False");
825  }
826 
827  stat.add("Number of original segmented planes (Avg.)",
829  stat.add("Number of connected segmented planes (Avg.)",
831  }
832  else {
833  stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
834  (boost::format("PlaneSegmentation not running for %f sec")
835  % plane_segmentation_vital_checker_->deadSec()).str());
836  }
837 
838  }
839 
841  const ros::TimerEvent& event)
842  {
843  boost::mutex::scoped_lock lock(mutex_);
844  diagnostic_updater_->update();
845  }
846 
847 }
long long values[]
jsk_topic_tools::VitalChecker::Ptr normal_estimation_vital_checker_
#define NODELET_ERROR(...)
timer
void publish(const boost::shared_ptr< M > &message) const
void addDiagnosticInformation(const std::string &string_prefix, jsk_topic_tools::TimeAccumulator &accumulator, diagnostic_updater::DiagnosticStatusWrapper &stat)
void summary(unsigned char lvl, const std::string s)
virtual Eigen::Vector3f getPointOnPlane()
GLfloat n[6][3]
const std::string & getName() const
jsk_pcl_ros::OrganizedMultiPlaneSegmentationConfig Config
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
virtual void pclIndicesArrayToClusterPointIndices(const std::vector< pcl::PointIndices > &inlier_indices, const std_msgs::Header &header, jsk_recognition_msgs::ClusterPointIndices &output_indices)
jsk_topic_tools::VitalChecker::Ptr plane_segmentation_vital_checker_
virtual void publishMarkerOfConnection(jsk_recognition_utils::IntegerGraphMap connection_map, const pcl::PointCloud< PointT >::Ptr cloud, const std::vector< pcl::PointIndices > &inliers, const std_msgs::Header &header)
std::map< int, std::vector< int > > IntegerGraphMap
virtual void toCoefficients(std::vector< float > &output)
virtual void publishSegmentationInformation(const std_msgs::Header &header, const pcl::PointCloud< PointT >::Ptr input, ros::Publisher &indices_pub, ros::Publisher &polygon_pub, ros::Publisher &coefficients_pub, const std::vector< pcl::PointIndices > &inlier_indices, const std::vector< pcl::PointCloud< PointT > > &boundaries, const std::vector< pcl::ModelCoefficients > &model_coefficients)
virtual void estimateNormal(pcl::PointCloud< PointT >::Ptr input, pcl::PointCloud< pcl::Normal >::Ptr output)
virtual void forceToDirectOrigin(const std::vector< pcl::ModelCoefficients > &coefficients, std::vector< pcl::ModelCoefficients > &output_coefficients)
#define M_PI
double sqrt()
boost::shared_ptr< diagnostic_updater::Updater > diagnostic_updater_
virtual void segmentOrganizedMultiPlanes(pcl::PointCloud< PointT >::Ptr input, pcl::PointCloud< pcl::Normal >::Ptr normal, PlanarRegionVector &regions, std::vector< pcl::ModelCoefficients > &model_coefficients, std::vector< pcl::PointIndices > &inlier_indices, pcl::PointCloud< pcl::Label >::Ptr &labels, std::vector< pcl::PointIndices > &label_indices, std::vector< pcl::PointIndices > &boundary_indices)
boost::shared_ptr< ros::NodeHandle > pnh_
string str
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
std::vector< int > addIndices(const std::vector< int > &a, const std::vector< int > &b)
begin
virtual void connectPlanesMap(const pcl::PointCloud< PointT >::Ptr &input, const std::vector< pcl::ModelCoefficients > &model_coefficients, const std::vector< pcl::PointIndices > &boundary_indices, jsk_recognition_utils::IntegerGraphMap &connection_map)
virtual void add(double v)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
virtual void segment(const sensor_msgs::PointCloud2::ConstPtr &msg)
double x
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
void buildAllGroupsSetFromGraphMap(IntegerGraphMap graph_map, std::vector< std::set< int > > &output_sets)
std_msgs::ColorRGBA colorCategory20(int i)
virtual void updateDiagnosticNormalEstimation(diagnostic_updater::DiagnosticStatusWrapper &stat)
labels
INLINE Rall1d< T, V, S > acos(const Rall1d< T, V, S > &x)
virtual void refineBasedOnRANSAC(const pcl::PointCloud< PointT >::Ptr input, const std::vector< pcl::PointIndices > &input_indices, const std::vector< pcl::ModelCoefficients > &input_coefficients, std::vector< pcl::PointIndices > &output_indices, std::vector< pcl::ModelCoefficients > &output_coefficients, std::vector< jsk_recognition_utils::ConvexPolygon::Ptr > &output_boundaries)
virtual void buildConnectedPlanes(const pcl::PointCloud< PointT >::Ptr &input, const std_msgs::Header &header, const std::vector< pcl::PointIndices > &inlier_indices, const std::vector< pcl::PointIndices > &boundary_indices, const std::vector< pcl::ModelCoefficients > &model_coefficients, const jsk_recognition_utils::IntegerGraphMap &connection_map, std::vector< pcl::PointIndices > &output_indices, std::vector< pcl::ModelCoefficients > &output_coefficients, std::vector< pcl::PointCloud< PointT > > &output_boundary_clouds)
p
std::vector< pcl::PlanarRegion< PointT >, Eigen::aligned_allocator< pcl::PlanarRegion< PointT > > > PlanarRegionVector
int theta
void add(const std::string &key, const T &val)
#define NODELET_FATAL(...)
virtual void updateDiagnosticPlaneSegmentation(diagnostic_updater::DiagnosticStatusWrapper &stat)
pcl::PointIndices PCLIndicesMsg
virtual void segmentFromNormals(pcl::PointCloud< PointT >::Ptr input, pcl::PointCloud< pcl::Normal >::Ptr normal, const std_msgs::Header &header)
#define NODELET_DEBUG(...)
pcl::ModelCoefficients PCLModelCoefficientMsg
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::OrganizedMultiPlaneSegmentation, nodelet::Nodelet)
virtual void pointCloudToPolygon(const pcl::PointCloud< PointT > &input, geometry_msgs::Polygon &polygon)
virtual Eigen::Vector3f getNormal()
virtual ScopedTimer scopedTimer()


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