Point Cloud Library (PCL)  1.9.1-dev
cpc_segmentation.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_
39 #define PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_
40 
41 #include <pcl/segmentation/cpc_segmentation.h>
42 
43 template <typename PointT>
45  max_cuts_ (20),
46  min_segment_size_for_cutting_ (400),
47  min_cut_score_ (0.16),
48  use_local_constrains_ (true),
49  use_directed_weights_ (true),
50  ransac_itrs_ (10000)
51 {
52 }
53 
54 template <typename PointT>
56 {
57 }
58 
59 template <typename PointT> void
61 {
62  if (supervoxels_set_)
63  {
64  // Calculate for every Edge if the connection is convex or invalid
65  // This effectively performs the segmentation.
67 
68  // Correct edge relations using extended convexity definition if k>0
70 
71  // Determine whether to use cutting planes
72  doGrouping ();
73 
74  grouping_data_valid_ = true;
75 
76  applyCuttingPlane (max_cuts_);
77 
78  // merge small segments
80  }
81  else
82  PCL_WARN ("[pcl::CPCSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
83 }
84 
85 template <typename PointT> void
86 pcl::CPCSegmentation<PointT>::applyCuttingPlane (uint32_t depth_levels_left)
87 {
88  typedef std::map<uint32_t, pcl::PointCloud<WeightSACPointType>::Ptr> SegLabel2ClusterMap;
89 
90  pcl::console::print_info ("Cutting at level %d (maximum %d)\n", max_cuts_ - depth_levels_left + 1, max_cuts_);
91  // stop if we reached the 0 level
92  if (depth_levels_left <= 0)
93  return;
94 
95  SegLabel2ClusterMap seg_to_edge_points_map;
96  std::map<uint32_t, std::vector<EdgeID> > seg_to_edgeIDs_map;
97  EdgeIterator edge_itr, edge_itr_end, next_edge;
98  boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_);
99  for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
100  {
101  next_edge++; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge
102  uint32_t source_sv_label = sv_adjacency_list_[boost::source (*edge_itr, sv_adjacency_list_)];
103  uint32_t target_sv_label = sv_adjacency_list_[boost::target (*edge_itr, sv_adjacency_list_)];
104 
105  uint32_t source_segment_label = sv_label_to_seg_label_map_[source_sv_label];
106  uint32_t target_segment_label = sv_label_to_seg_label_map_[target_sv_label];
107 
108  // do not process edges which already split two segments
109  if (source_segment_label != target_segment_label)
110  continue;
111 
112  // if edge has been used for cutting already do not use it again
113  if (sv_adjacency_list_[*edge_itr].used_for_cutting)
114  continue;
115  // get centroids of vertices
116  const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_;
117  const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_;
118 
119  // stores the information about the edge cloud (used for the weighted ransac)
120  // we use the normal to express the direction of the connection
121  // we use the intensity to express the normal differences between supervoxel patches. <=0: Convex, >0: Concave
122  WeightSACPointType edge_centroid;
123  edge_centroid.getVector3fMap () = (source_centroid.getVector3fMap () + target_centroid.getVector3fMap ()) / 2;
124 
125  // we use the normal to express the direction of the connection!
126  edge_centroid.getNormalVector3fMap () = (target_centroid.getVector3fMap () - source_centroid.getVector3fMap ()).normalized ();
127 
128  // we use the intensity to express the normal differences between supervoxel patches. <=0: Convex, >0: Concave
129  edge_centroid.intensity = sv_adjacency_list_[*edge_itr].is_convex ? -sv_adjacency_list_[*edge_itr].normal_difference : sv_adjacency_list_[*edge_itr].normal_difference;
130  if (seg_to_edge_points_map.find (source_segment_label) == seg_to_edge_points_map.end ())
131  {
132  seg_to_edge_points_map[source_segment_label] = pcl::PointCloud<WeightSACPointType>::Ptr (new pcl::PointCloud<WeightSACPointType> ());
133  }
134  seg_to_edge_points_map[source_segment_label]->push_back (edge_centroid);
135  seg_to_edgeIDs_map[source_segment_label].push_back (*edge_itr);
136  }
137  bool cut_found = false;
138  // do the following processing for each segment separately
139  for (SegLabel2ClusterMap::iterator itr = seg_to_edge_points_map.begin (); itr != seg_to_edge_points_map.end (); ++itr)
140  {
141  // if too small do not process
142  if (itr->second->size () < min_segment_size_for_cutting_)
143  {
144  continue;
145  }
146 
147  std::vector<double> weights;
148  weights.resize (itr->second->size ());
149  for (std::size_t cp = 0; cp < itr->second->size (); ++cp)
150  {
151  float& cur_weight = itr->second->points[cp].intensity;
152  cur_weight = cur_weight < concavity_tolerance_threshold_ ? 0 : 1;
153  weights[cp] = cur_weight;
154  }
155 
156  pcl::PointCloud<WeightSACPointType>::Ptr edge_cloud_cluster = itr->second;
158 
159  WeightedRandomSampleConsensus weight_sac (model_p, seed_resolution_, true);
160 
161  weight_sac.setWeights (weights, use_directed_weights_);
162  weight_sac.setMaxIterations (ransac_itrs_);
163 
164  // if not enough inliers are found
165  if (!weight_sac.computeModel ())
166  {
167  continue;
168  }
169 
170  Eigen::VectorXf model_coefficients;
171  weight_sac.getModelCoefficients (model_coefficients);
172 
173  model_coefficients[3] += std::numeric_limits<float>::epsilon ();
174 
175  std::vector<int> support_indices;
176  weight_sac.getInliers (support_indices);
177 
178  // the support_indices which are actually cut (if not locally constrain: cut_support_indices = support_indices
179  std::vector<int> cut_support_indices;
180 
181  if (use_local_constrains_)
182  {
183  Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
184  // Cut the connections.
185  // We only iterate through the points which are within the support (when we are local, otherwise all points in the segment).
186  // We also just actually cut when the edge goes through the plane. This is why we check the planedistance
187  std::vector<pcl::PointIndices> cluster_indices;
190  tree->setInputCloud (edge_cloud_cluster);
191  euclidean_clusterer.setClusterTolerance (seed_resolution_);
192  euclidean_clusterer.setMinClusterSize (1);
193  euclidean_clusterer.setMaxClusterSize (25000);
194  euclidean_clusterer.setSearchMethod (tree);
195  euclidean_clusterer.setInputCloud (edge_cloud_cluster);
196  euclidean_clusterer.setIndices (boost::make_shared <std::vector <int> > (support_indices));
197  euclidean_clusterer.extract (cluster_indices);
198 // sv_adjacency_list_[seg_to_edgeID_map[itr->first][point_index]].used_for_cutting = true;
199 
200  for (size_t cc = 0; cc < cluster_indices.size (); ++cc)
201  {
202  // get centroids of vertices
203  int cluster_concave_pts = 0;
204  float cluster_score = 0;
205 // std::cout << "Cluster has " << cluster_indices[cc].indices.size () << " points" << std::endl;
206  for (size_t cp = 0; cp < cluster_indices[cc].indices.size (); ++cp)
207  {
208  int current_index = cluster_indices[cc].indices[cp];
209  double index_score;
210  if (use_directed_weights_)
211  index_score = weights[current_index] * 1.414 * (fabsf (plane_normal.dot (edge_cloud_cluster->at (current_index).getNormalVector3fMap ())));
212  else
213  index_score = weights[current_index];
214  cluster_score += index_score;
215  if (weights[current_index] > 0)
216  ++cluster_concave_pts;
217  }
218  // check if the score is below the threshold. If that is the case this segment should not be split
219  cluster_score = cluster_score * 1.0 / cluster_indices[cc].indices.size ();
220 // std::cout << "Cluster score: " << cluster_score << std::endl;
221  if (cluster_score >= min_cut_score_)
222  {
223  cut_support_indices.insert (cut_support_indices.end (), cluster_indices[cc].indices.begin (), cluster_indices[cc].indices.end ());
224  }
225  }
226  if (cut_support_indices.size () == 0)
227  {
228 // std::cout << "Could not find planes which exceed required minimum score (threshold " << min_cut_score_ << "), not cutting" << std::endl;
229  continue;
230  }
231  }
232  else
233  {
234  double current_score = weight_sac.getBestScore ();
235  cut_support_indices = support_indices;
236  // check if the score is below the threshold. If that is the case this segment should not be split
237  if (current_score < min_cut_score_)
238  {
239 // std::cout << "Score too low, no cutting" << std::endl;
240  continue;
241  }
242  }
243 
244  int number_connections_cut = 0;
245  for (size_t cs = 0; cs < cut_support_indices.size (); ++cs)
246  {
247  const int point_index = cut_support_indices[cs];
248 
249  if (use_clean_cutting_)
250  {
251  // skip edges where both centroids are on one side of the cutting plane
252  uint32_t source_sv_label = sv_adjacency_list_[boost::source (seg_to_edgeIDs_map[itr->first][point_index], sv_adjacency_list_)];
253  uint32_t target_sv_label = sv_adjacency_list_[boost::target (seg_to_edgeIDs_map[itr->first][point_index], sv_adjacency_list_)];
254  // get centroids of vertices
255  const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_;
256  const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_;
257  // this makes a clean cut
258  if (pcl::pointToPlaneDistanceSigned (source_centroid, model_coefficients) * pcl::pointToPlaneDistanceSigned (target_centroid, model_coefficients) > 0)
259  {
260  continue;
261  }
262  }
263  sv_adjacency_list_[seg_to_edgeIDs_map[itr->first][point_index]].used_for_cutting = true;
264  if (sv_adjacency_list_[seg_to_edgeIDs_map[itr->first][point_index]].is_valid)
265  {
266  ++number_connections_cut;
267  sv_adjacency_list_[seg_to_edgeIDs_map[itr->first][point_index]].is_valid = false;
268  }
269  }
270 // std::cout << "We cut " << number_connections_cut << " connections" << std::endl;
271  if (number_connections_cut > 0)
272  cut_found = true;
273  }
274 
275  // if not cut has been performed we can stop the recursion
276  if (cut_found)
277  {
278  doGrouping ();
279  --depth_levels_left;
280  applyCuttingPlane (depth_levels_left);
281  }
282  else
283  pcl::console::print_info ("Could not find any more cuts, stopping recursion\n");
284 }
285 
286 /******************************************* Directional weighted RANSAC definitions ******************************************************************/
287 
288 
289 template <typename PointT> bool
291 {
292  // Warn and exit if no threshold was set
293  if (threshold_ == std::numeric_limits<double>::max ())
294  {
295  PCL_ERROR ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No threshold set!\n");
296  return (false);
297  }
298 
299  iterations_ = 0;
300  best_score_ = -std::numeric_limits<double>::max ();
301 
302  std::vector<int> selection;
303  Eigen::VectorXf model_coefficients;
304 
305  unsigned skipped_count = 0;
306  // suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
307  const unsigned max_skip = max_iterations_ * 10;
308 
309  // Iterate
310  while (iterations_ < max_iterations_ && skipped_count < max_skip)
311  {
312  // Get X samples which satisfy the model criteria and which have a weight > 0
313  sac_model_->setIndices (model_pt_indices_);
314  sac_model_->getSamples (iterations_, selection);
315 
316  if (selection.empty ())
317  {
318  PCL_ERROR ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No samples could be selected!\n");
319  break;
320  }
321 
322  // Search for inliers in the point cloud for the current plane model M
323  if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
324  {
325  //++iterations_;
326  ++skipped_count;
327  continue;
328  }
329  // weight distances to get the score (only using connected inliers)
330  sac_model_->setIndices (full_cloud_pt_indices_);
331 
332  boost::shared_ptr<std::vector<int> > current_inliers (new std::vector<int>);
333  sac_model_->selectWithinDistance (model_coefficients, threshold_, *current_inliers);
334  double current_score = 0;
335  Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
336  for (size_t i = 0; i < current_inliers->size (); ++i)
337  {
338  int current_index = current_inliers->at (i);
339  double index_score;
340  if (use_directed_weights_)
341  // the sqrt(2) factor was used in the paper and was meant for making the scores better comparable between directed and undirected weights
342  index_score = weights_[current_index] * 1.414 * (fabsf (plane_normal.dot (point_cloud_ptr_->at (current_index).getNormalVector3fMap ())));
343  else
344  index_score = weights_[current_index];
345 
346  current_score += index_score;
347  }
348  // normalize by the total number of inliers
349  current_score = current_score * 1.0 / current_inliers->size ();
350 
351  // Better match ?
352  if (current_score > best_score_)
353  {
354  best_score_ = current_score;
355  // Save the current model/inlier/coefficients selection as being the best so far
356  model_ = selection;
357  model_coefficients_ = model_coefficients;
358  }
359 
360  ++iterations_;
361  PCL_DEBUG ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] Trial %d (max %d): score is %f (best is: %f so far).\n", iterations_, max_iterations_, current_score, best_score_);
362  if (iterations_ > max_iterations_)
363  {
364  PCL_DEBUG ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
365  break;
366  }
367  }
368 // std::cout << "Took us " << iterations_ - 1 << " iterations" << std::endl;
369  PCL_DEBUG ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] Model: %lu size, %f score.\n", model_.size (), best_score_);
370 
371  if (model_.empty ())
372  {
373  inliers_.clear ();
374  return (false);
375  }
376 
377  // Get the set of inliers that correspond to the best model found so far
378  sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
379  return (true);
380 }
381 
382 #endif // PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_
double pointToPlaneDistanceSigned(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:61
void doGrouping()
Perform depth search on the graph and recursively group all supervoxels with convex connections...
void segment()
Merge supervoxels using cuts through local convexities.
boost::shared_ptr< SampleConsensusModelPlane > Ptr
PCL_EXPORTS void print_info(const char *format,...)
Print an info message on stream with colors.
A segmentation algorithm partitioning a supervoxel graph.
void setClusterTolerance(double tolerance)
Set the spatial cluster tolerance as a measure in the L2 Euclidean space.
A point structure representing Euclidean xyz coordinates, and the RGBA color.
uint32_t k_factor_
Factor used for k-convexity.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:427
std::map< uint32_t, typename pcl::Supervoxel< PointT >::Ptr > sv_label_to_supervoxel_map_
map from the supervoxel labels to the supervoxel objects
void setMaxClusterSize(int max_cluster_size)
Set the maximum number of points that a cluster needs to contain in order to be considered valid...
float seed_resolution_
Seed resolution of the supervoxels (used only for smoothness check)
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:78
void setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition: kdtree.hpp:77
A point structure representing Euclidean xyz coordinates, intensity, together with normal coordinates...
void extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
bool supervoxels_set_
Marks if supervoxels have been set by calling setInputSupervoxels.
void mergeSmallSegments()
Segments smaller than min_segment_size_ are merged to the label of largest neighbor.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:73
void setMinClusterSize(int min_cluster_size)
Set the minimum number of points that a cluster needs to contain in order to be considered valid...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:66
std::map< uint32_t, uint32_t > sv_label_to_seg_label_map_
Storing relation between original SuperVoxel Labels and new segmantion labels.
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
float concavity_tolerance_threshold_
*** Parameters *** ///
void calculateConvexConnections(SupervoxelAdjacencyList &adjacency_list_arg)
Calculates convexity of edges and saves this to the adjacency graph.
void applyKconvexity(const unsigned int k_arg)
Connections are only convex if this is true for at least k_arg common neighbors of the two patches...
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:282
SampleConsensusModelPlane defines a model for 3D plane segmentation.
bool grouping_data_valid_
Marks if valid grouping data (sv_adjacency_list_, sv_label_to_seg_label_map_, processed_) is availabl...
SupervoxelAdjacencyList sv_adjacency_list_
Adjacency graph with the supervoxel labels as nodes and edges between adjacent supervoxels.
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.