Point Cloud Library (PCL) 1.12.1
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/sample_consensus/sac_model_plane.h> // for SampleConsensusModelPlane
42#include <pcl/segmentation/cpc_segmentation.h>
43
44template <typename PointT>
46 max_cuts_ (20),
47 min_segment_size_for_cutting_ (400),
48 min_cut_score_ (0.16),
49 use_local_constrains_ (true),
50 use_directed_weights_ (true),
51 ransac_itrs_ (10000)
52{
53}
54
55template <typename PointT>
57{
58}
59
60template <typename PointT> void
62{
63 if (supervoxels_set_)
64 {
65 // Calculate for every Edge if the connection is convex or invalid
66 // This effectively performs the segmentation.
67 calculateConvexConnections (sv_adjacency_list_);
68
69 // Correct edge relations using extended convexity definition if k>0
70 applyKconvexity (k_factor_);
71
72 // Determine whether to use cutting planes
73 doGrouping ();
74
75 grouping_data_valid_ = true;
76
77 applyCuttingPlane (max_cuts_);
78
79 // merge small segments
80 mergeSmallSegments ();
81 }
82 else
83 PCL_WARN ("[pcl::CPCSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
84}
85
86template <typename PointT> void
87pcl::CPCSegmentation<PointT>::applyCuttingPlane (std::uint32_t depth_levels_left)
88{
89 using SegLabel2ClusterMap = std::map<std::uint32_t, pcl::PointCloud<WeightSACPointType>::Ptr>;
90
91 pcl::console::print_info ("Cutting at level %d (maximum %d)\n", max_cuts_ - depth_levels_left + 1, max_cuts_);
92 // stop if we reached the 0 level
93 if (depth_levels_left <= 0)
94 return;
95
96 pcl::IndicesPtr support_indices (new pcl::Indices);
97 SegLabel2ClusterMap seg_to_edge_points_map;
98 std::map<std::uint32_t, std::vector<EdgeID> > seg_to_edgeIDs_map;
99 EdgeIterator edge_itr, edge_itr_end, next_edge;
100 boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_);
101 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
102 {
103 next_edge++; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge
104 std::uint32_t source_sv_label = sv_adjacency_list_[boost::source (*edge_itr, sv_adjacency_list_)];
105 std::uint32_t target_sv_label = sv_adjacency_list_[boost::target (*edge_itr, sv_adjacency_list_)];
106
107 std::uint32_t source_segment_label = sv_label_to_seg_label_map_[source_sv_label];
108 std::uint32_t target_segment_label = sv_label_to_seg_label_map_[target_sv_label];
109
110 // do not process edges which already split two segments
111 if (source_segment_label != target_segment_label)
112 continue;
113
114 // if edge has been used for cutting already do not use it again
115 if (sv_adjacency_list_[*edge_itr].used_for_cutting)
116 continue;
117 // get centroids of vertices
118 const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_;
119 const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_;
120
121 // stores the information about the edge cloud (used for the weighted ransac)
122 // we use the normal to express the direction of the connection
123 // we use the intensity to express the normal differences between supervoxel patches. <=0: Convex, >0: Concave
124 WeightSACPointType edge_centroid;
125 edge_centroid.getVector3fMap () = (source_centroid.getVector3fMap () + target_centroid.getVector3fMap ()) / 2;
126
127 // we use the normal to express the direction of the connection!
128 edge_centroid.getNormalVector3fMap () = (target_centroid.getVector3fMap () - source_centroid.getVector3fMap ()).normalized ();
129
130 // we use the intensity to express the normal differences between supervoxel patches. <=0: Convex, >0: Concave
131 edge_centroid.intensity = sv_adjacency_list_[*edge_itr].is_convex ? -sv_adjacency_list_[*edge_itr].normal_difference : sv_adjacency_list_[*edge_itr].normal_difference;
132 if (seg_to_edge_points_map.find (source_segment_label) == seg_to_edge_points_map.end ())
133 {
134 seg_to_edge_points_map[source_segment_label] = pcl::PointCloud<WeightSACPointType>::Ptr (new pcl::PointCloud<WeightSACPointType> ());
135 }
136 seg_to_edge_points_map[source_segment_label]->push_back (edge_centroid);
137 seg_to_edgeIDs_map[source_segment_label].push_back (*edge_itr);
138 }
139 bool cut_found = false;
140 // do the following processing for each segment separately
141 for (const auto &seg_to_edge_points : seg_to_edge_points_map)
142 {
143 // if too small do not process
144 if (seg_to_edge_points.second->size () < min_segment_size_for_cutting_)
145 {
146 continue;
147 }
148
149 std::vector<double> weights;
150 weights.resize (seg_to_edge_points.second->size ());
151 for (std::size_t cp = 0; cp < seg_to_edge_points.second->size (); ++cp)
152 {
153 float& cur_weight = (*seg_to_edge_points.second)[cp].intensity;
154 cur_weight = cur_weight < concavity_tolerance_threshold_ ? 0 : 1;
155 weights[cp] = cur_weight;
156 }
157
158 pcl::PointCloud<WeightSACPointType>::Ptr edge_cloud_cluster = seg_to_edge_points.second;
160
161 WeightedRandomSampleConsensus weight_sac (model_p, seed_resolution_, true);
162
163 weight_sac.setWeights (weights, use_directed_weights_);
164 weight_sac.setMaxIterations (ransac_itrs_);
165
166 // if not enough inliers are found
167 if (!weight_sac.computeModel ())
168 {
169 continue;
170 }
171
172 Eigen::VectorXf model_coefficients;
173 weight_sac.getModelCoefficients (model_coefficients);
174
175 model_coefficients[3] += std::numeric_limits<float>::epsilon ();
176
177 weight_sac.getInliers (*support_indices);
178
179 // the support_indices which are actually cut (if not locally constrain: cut_support_indices = support_indices
180 pcl::Indices cut_support_indices;
181
182 if (use_local_constrains_)
183 {
184 Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
185 // Cut the connections.
186 // We only iterate through the points which are within the support (when we are local, otherwise all points in the segment).
187 // We also just actually cut when the edge goes through the plane. This is why we check the planedistance
188 std::vector<pcl::PointIndices> cluster_indices;
191 tree->setInputCloud (edge_cloud_cluster);
192 euclidean_clusterer.setClusterTolerance (seed_resolution_);
193 euclidean_clusterer.setMinClusterSize (1);
194 euclidean_clusterer.setMaxClusterSize (25000);
195 euclidean_clusterer.setSearchMethod (tree);
196 euclidean_clusterer.setInputCloud (edge_cloud_cluster);
197 euclidean_clusterer.setIndices (support_indices);
198 euclidean_clusterer.extract (cluster_indices);
199// sv_adjacency_list_[seg_to_edgeID_map[seg_to_edge_points.first][point_index]].used_for_cutting = true;
200
201 for (const auto &cluster_index : cluster_indices)
202 {
203 // get centroids of vertices
204 int cluster_concave_pts = 0;
205 float cluster_score = 0;
206// std::cout << "Cluster has " << cluster_indices[cc].indices.size () << " points" << std::endl;
207 for (const auto &current_index : cluster_index.indices)
208 {
209 double index_score = weights[current_index];
210 if (use_directed_weights_)
211 index_score *= 1.414 * (std::abs (plane_normal.dot (edge_cloud_cluster->at (current_index).getNormalVector3fMap ())));
212 cluster_score += index_score;
213 if (weights[current_index] > 0)
214 ++cluster_concave_pts;
215 }
216 // check if the score is below the threshold. If that is the case this segment should not be split
217 cluster_score /= cluster_index.indices.size ();
218// std::cout << "Cluster score: " << cluster_score << std::endl;
219 if (cluster_score >= min_cut_score_)
220 {
221 cut_support_indices.insert (cut_support_indices.end (), cluster_index.indices.begin (), cluster_index.indices.end ());
222 }
223 }
224 if (cut_support_indices.empty ())
225 {
226// std::cout << "Could not find planes which exceed required minimum score (threshold " << min_cut_score_ << "), not cutting" << std::endl;
227 continue;
228 }
229 }
230 else
231 {
232 double current_score = weight_sac.getBestScore ();
233 cut_support_indices = *support_indices;
234 // check if the score is below the threshold. If that is the case this segment should not be split
235 if (current_score < min_cut_score_)
236 {
237// std::cout << "Score too low, no cutting" << std::endl;
238 continue;
239 }
240 }
241
242 int number_connections_cut = 0;
243 for (const auto &point_index : cut_support_indices)
244 {
245 if (use_clean_cutting_)
246 {
247 // skip edges where both centroids are on one side of the cutting plane
248 std::uint32_t source_sv_label = sv_adjacency_list_[boost::source (seg_to_edgeIDs_map[seg_to_edge_points.first][point_index], sv_adjacency_list_)];
249 std::uint32_t target_sv_label = sv_adjacency_list_[boost::target (seg_to_edgeIDs_map[seg_to_edge_points.first][point_index], sv_adjacency_list_)];
250 // get centroids of vertices
251 const pcl::PointXYZRGBA source_centroid = sv_label_to_supervoxel_map_[source_sv_label]->centroid_;
252 const pcl::PointXYZRGBA target_centroid = sv_label_to_supervoxel_map_[target_sv_label]->centroid_;
253 // this makes a clean cut
254 if (pcl::pointToPlaneDistanceSigned (source_centroid, model_coefficients) * pcl::pointToPlaneDistanceSigned (target_centroid, model_coefficients) > 0)
255 {
256 continue;
257 }
258 }
259 sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].used_for_cutting = true;
260 if (sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].is_valid)
261 {
262 ++number_connections_cut;
263 sv_adjacency_list_[seg_to_edgeIDs_map[seg_to_edge_points.first][point_index]].is_valid = false;
264 }
265 }
266// std::cout << "We cut " << number_connections_cut << " connections" << std::endl;
267 if (number_connections_cut > 0)
268 cut_found = true;
269 }
270
271 // if not cut has been performed we can stop the recursion
272 if (cut_found)
273 {
274 doGrouping ();
275 --depth_levels_left;
276 applyCuttingPlane (depth_levels_left);
277 }
278 else
279 pcl::console::print_info ("Could not find any more cuts, stopping recursion\n");
280}
281
282/******************************************* Directional weighted RANSAC definitions ******************************************************************/
283
284
285template <typename PointT> bool
287{
288 // Warn and exit if no threshold was set
289 if (threshold_ == std::numeric_limits<double>::max ())
290 {
291 PCL_ERROR ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No threshold set!\n");
292 return (false);
293 }
294
295 iterations_ = 0;
296 best_score_ = -std::numeric_limits<double>::max ();
297
298 pcl::Indices selection;
299 Eigen::VectorXf model_coefficients;
300
301 unsigned skipped_count = 0;
302 // suppress infinite loops by just allowing 10 x maximum allowed iterations for invalid model parameters!
303 const unsigned max_skip = max_iterations_ * 10;
304
305 // Iterate
306 while (iterations_ < max_iterations_ && skipped_count < max_skip)
307 {
308 // Get X samples which satisfy the model criteria and which have a weight > 0
309 sac_model_->setIndices (model_pt_indices_);
310 sac_model_->getSamples (iterations_, selection);
311
312 if (selection.empty ())
313 {
314 PCL_ERROR ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] No samples could be selected!\n");
315 break;
316 }
317
318 // Search for inliers in the point cloud for the current plane model M
319 if (!sac_model_->computeModelCoefficients (selection, model_coefficients))
320 {
321 //++iterations_;
322 ++skipped_count;
323 continue;
324 }
325 // weight distances to get the score (only using connected inliers)
326 sac_model_->setIndices (full_cloud_pt_indices_);
327
328 pcl::IndicesPtr current_inliers (new pcl::Indices);
329 sac_model_->selectWithinDistance (model_coefficients, threshold_, *current_inliers);
330 double current_score = 0;
331 Eigen::Vector3f plane_normal (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
332 for (const auto &current_index : *current_inliers)
333 {
334 double index_score = weights_[current_index];
335 if (use_directed_weights_)
336 // the sqrt(2) factor was used in the paper and was meant for making the scores better comparable between directed and undirected weights
337 index_score *= 1.414 * (std::abs (plane_normal.dot (point_cloud_ptr_->at (current_index).getNormalVector3fMap ())));
338
339 current_score += index_score;
340 }
341 // normalize by the total number of inliers
342 current_score /= current_inliers->size ();
343
344 // Better match ?
345 if (current_score > best_score_)
346 {
347 best_score_ = current_score;
348 // Save the current model/inlier/coefficients selection as being the best so far
349 model_ = selection;
350 model_coefficients_ = model_coefficients;
351 }
352
353 ++iterations_;
354 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_);
355 if (iterations_ > max_iterations_)
356 {
357 PCL_DEBUG ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] RANSAC reached the maximum number of trials.\n");
358 break;
359 }
360 }
361// std::cout << "Took us " << iterations_ - 1 << " iterations" << std::endl;
362 PCL_DEBUG ("[pcl::CPCSegmentation<PointT>::WeightedRandomSampleConsensus::computeModel] Model: %lu size, %f score.\n", model_.size (), best_score_);
363
364 if (model_.empty ())
365 {
366 inliers_.clear ();
367 return (false);
368 }
369
370 // Get the set of inliers that correspond to the best model found so far
371 sac_model_->selectWithinDistance (model_coefficients_, threshold_, inliers_);
372 return (true);
373}
374
375#endif // PCL_SEGMENTATION_IMPL_CPC_SEGMENTATION_HPP_
A segmentation algorithm partitioning a supervoxel graph.
void segment()
Merge supervoxels using cuts through local convexities.
EuclideanClusterExtraction represents a segmentation class for cluster extraction in an Euclidean sen...
void extract(std::vector< PointIndices > &clusters)
Cluster extraction in a PointCloud given by <setInputCloud (), setIndices ()>
void setClusterTolerance(double tolerance)
Set the spatial cluster tolerance as a measure in the L2 Euclidean space.
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.
void setMaxClusterSize(pcl::uindex_t max_cluster_size)
Set the maximum number of points that a cluster needs to contain in order to be considered valid.
void setMinClusterSize(pcl::uindex_t 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:65
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:72
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
const PointT & at(int column, int row) const
Obtain the point given by the (column, row) coordinates.
Definition: point_cloud.h:262
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
SampleConsensusModelPlane defines a model for 3D plane segmentation.
shared_ptr< SampleConsensusModelPlane< PointT > > Ptr
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:75
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.
PCL_EXPORTS void print_info(const char *format,...)
Print an info message on stream with colors.
int cp(int from, int to)
Returns field copy operation code.
Definition: repacks.hpp:56
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
shared_ptr< Indices > IndicesPtr
Definition: pcl_base.h:58
A point structure representing Euclidean xyz coordinates, and the RGBA color.