Point Cloud Library (PCL)  1.14.1-dev
lccp_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_LCCP_SEGMENTATION_HPP_
39 #define PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
40 
41 #include <pcl/segmentation/lccp_segmentation.h>
42 #include <pcl/common/common.h>
43 
44 
45 //////////////////////////////////////////////////////////
46 //////////////////////////////////////////////////////////
47 /////////////////// Public Functions /////////////////////
48 //////////////////////////////////////////////////////////
49 //////////////////////////////////////////////////////////
50 
51 
52 
53 template <typename PointT>
55 
56 template <typename PointT>
58 
59 template <typename PointT> void
61 {
62  sv_adjacency_list_.clear ();
63  processed_.clear ();
64  sv_label_to_supervoxel_map_.clear ();
65  sv_label_to_seg_label_map_.clear ();
66  seg_label_to_sv_list_map_.clear ();
67  seg_label_to_neighbor_set_map_.clear ();
68  grouping_data_valid_ = false;
69  supervoxels_set_ = false;
70 }
71 
72 template <typename PointT> void
74 {
75  if (supervoxels_set_)
76  {
77  // Calculate for every Edge if the connection is convex or invalid
78  // This effectively performs the segmentation.
79  calculateConvexConnections (sv_adjacency_list_);
80 
81  // Correct edge relations using extended convexity definition if k>0
82  applyKconvexity (k_factor_);
83 
84  // group supervoxels
85  doGrouping ();
86 
87  grouping_data_valid_ = true;
88 
89  // merge small segments
90  mergeSmallSegments ();
91  }
92  else
93  PCL_WARN ("[pcl::LCCPSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
94 }
95 
96 
97 template <typename PointT> void
99 {
100  if (grouping_data_valid_)
101  {
102  // Relabel all Points in cloud with new labels
103  for (auto &voxel : labeled_cloud_arg)
104  {
105  voxel.label = sv_label_to_seg_label_map_[voxel.label];
106  }
107  }
108  else
109  {
110  PCL_WARN ("[pcl::LCCPSegmentation::relabelCloud] WARNING: Call function segment first. Nothing has been done. \n");
111  }
112 }
113 
114 
115 
116 //////////////////////////////////////////////////////////
117 //////////////////////////////////////////////////////////
118 /////////////////// Protected Functions //////////////////
119 //////////////////////////////////////////////////////////
120 //////////////////////////////////////////////////////////
121 
122 template <typename PointT> void
124 {
125  seg_label_to_neighbor_set_map_.clear ();
126 
127  std::uint32_t current_segLabel;
128  std::uint32_t neigh_segLabel;
129 
130  VertexIterator sv_itr, sv_itr_end;
131  //The vertices in the supervoxel adjacency list are the supervoxel centroids
132  // For every Supervoxel..
133  for(std::tie(sv_itr, sv_itr_end) = boost::vertices(sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels
134  {
135  const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
136  current_segLabel = sv_label_to_seg_label_map_[sv_label];
137 
138  AdjacencyIterator itr_neighbor, itr_neighbor_end;
139  // ..look at all neighbors and insert their labels into the neighbor set
140  for (std::tie(itr_neighbor, itr_neighbor_end) = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_); itr_neighbor != itr_neighbor_end; ++itr_neighbor)
141  {
142  const std::uint32_t& neigh_label = sv_adjacency_list_[*itr_neighbor];
143  neigh_segLabel = sv_label_to_seg_label_map_[neigh_label];
144 
145  if (current_segLabel != neigh_segLabel)
146  {
147  seg_label_to_neighbor_set_map_[current_segLabel].insert (neigh_segLabel);
148  }
149  }
150  }
151 }
152 
153 template <typename PointT> void
155 {
156  if (min_segment_size_ == 0)
157  return;
158 
159  computeSegmentAdjacency ();
160 
161  std::set<std::uint32_t> filteredSegLabels;
162 
163  bool continue_filtering = true;
164 
165  while (continue_filtering)
166  {
167  continue_filtering = false;
168 
169  VertexIterator sv_itr, sv_itr_end;
170  // Iterate through all supervoxels, check if they are in a "small" segment -> change label to largest neighborID
171  for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels
172  {
173  const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
174  std::uint32_t current_seg_label = sv_label_to_seg_label_map_[sv_label];
175  std::uint32_t largest_neigh_seg_label = current_seg_label;
176  std::uint32_t largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size ();
177 
178  const std::uint32_t& nr_neighbors = seg_label_to_neighbor_set_map_[current_seg_label].size ();
179  if (nr_neighbors == 0)
180  continue;
181 
182  if (seg_label_to_sv_list_map_[current_seg_label].size () <= min_segment_size_)
183  {
184  continue_filtering = true;
185 
186  // Find largest neighbor
187  for (auto neighbors_itr = seg_label_to_neighbor_set_map_[current_seg_label].cbegin (); neighbors_itr != seg_label_to_neighbor_set_map_[current_seg_label].cend (); ++neighbors_itr)
188  {
189  if (seg_label_to_sv_list_map_[*neighbors_itr].size () >= largest_neigh_size)
190  {
191  largest_neigh_seg_label = *neighbors_itr;
192  largest_neigh_size = seg_label_to_sv_list_map_[*neighbors_itr].size ();
193  }
194  }
195 
196  // Add to largest neighbor
197  if (largest_neigh_seg_label != current_seg_label)
198  {
199  if (filteredSegLabels.count (largest_neigh_seg_label) > 0)
200  continue; // If neighbor was already assigned to someone else
201 
202  sv_label_to_seg_label_map_[sv_label] = largest_neigh_seg_label;
203  filteredSegLabels.insert (current_seg_label);
204 
205  // Assign supervoxel labels of filtered segment to new owner
206  for (auto sv_ID_itr = seg_label_to_sv_list_map_[current_seg_label].cbegin (); sv_ID_itr != seg_label_to_sv_list_map_[current_seg_label].cend (); ++sv_ID_itr)
207  {
208  seg_label_to_sv_list_map_[largest_neigh_seg_label].insert (*sv_ID_itr);
209  }
210  }
211  }
212  }
213 
214  // Erase filtered Segments from segment map
215  for (const unsigned int &filteredSegLabel : filteredSegLabels)
216  {
217  seg_label_to_sv_list_map_.erase (filteredSegLabel);
218  }
219 
220  // After filtered Segments are deleted, compute completely new adjacency map
221  // NOTE Recomputing the adjacency of every segment in every iteration is an easy but inefficient solution.
222  // Because the number of segments in an average scene is usually well below 1000, the time spend for noise filtering is still negligible in most cases
223  computeSegmentAdjacency ();
224  } // End while (Filtering)
225 }
226 
227 template <typename PointT> void
228 pcl::LCCPSegmentation<PointT>::prepareSegmentation (const std::map<std::uint32_t, typename pcl::Supervoxel<PointT>::Ptr>& supervoxel_clusters_arg,
229  const std::multimap<std::uint32_t, std::uint32_t>& label_adjaceny_arg)
230 {
231  // Clear internal data
232  reset ();
233 
234  // Copy map with supervoxel pointers
235  sv_label_to_supervoxel_map_ = supervoxel_clusters_arg;
236 
237  // Build a boost adjacency list from the adjacency multimap
238  std::map<std::uint32_t, VertexID> label_ID_map;
239 
240  // Add all supervoxel labels as vertices
241  for (auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
242  svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
243  {
244  const std::uint32_t& sv_label = svlabel_itr->first;
245  VertexID node_id = boost::add_vertex (sv_adjacency_list_);
246  sv_adjacency_list_[node_id] = sv_label;
247  label_ID_map[sv_label] = node_id;
248  }
249 
250  // Add all edges
251  for (const auto &sv_neighbors_itr : label_adjaceny_arg)
252  {
253  const std::uint32_t& sv_label = sv_neighbors_itr.first;
254  const std::uint32_t& neighbor_label = sv_neighbors_itr.second;
255 
256  VertexID u = label_ID_map[sv_label];
257  VertexID v = label_ID_map[neighbor_label];
258 
259  boost::add_edge (u, v, sv_adjacency_list_);
260  }
261 
262  // Initialization
263  // clear the processed_ map
264  seg_label_to_sv_list_map_.clear ();
265  for (auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
266  svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
267  {
268  const std::uint32_t& sv_label = svlabel_itr->first;
269  processed_[sv_label] = false;
270  sv_label_to_seg_label_map_[sv_label] = 0;
271  }
272 }
273 
274 
275 
276 
277 template <typename PointT> void
279 {
280  // clear the processed_ map
281  seg_label_to_sv_list_map_.clear ();
282  for (auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
283  svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
284  {
285  const std::uint32_t& sv_label = svlabel_itr->first;
286  processed_[sv_label] = false;
287  sv_label_to_seg_label_map_[sv_label] = 0;
288  }
289 
290  VertexIterator sv_itr, sv_itr_end;
291  // Perform depth search on the graph and recursively group all supervoxels with convex connections
292  //The vertices in the supervoxel adjacency list are the supervoxel centroids
293  // Note: *sv_itr is of type " boost::graph_traits<VoxelAdjacencyList>::vertex_descriptor " which it nothing but a typedef of std::size_t..
294  unsigned int segment_label = 1; // This starts at 1, because 0 is reserved for errors
295  for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr) // For all supervoxels
296  {
297  const VertexID sv_vertex_id = *sv_itr;
298  const std::uint32_t& sv_label = sv_adjacency_list_[sv_vertex_id];
299  if (!processed_[sv_label])
300  {
301  // Add neighbors (and their neighbors etc.) to group if similarity constraint is met
302  recursiveSegmentGrowing (sv_vertex_id, segment_label);
303  ++segment_label; // After recursive grouping ended (no more neighbors to consider) -> go to next group
304  }
305  }
306 }
307 
308 template <typename PointT> void
310  const unsigned int segment_label)
311 {
312  const std::uint32_t& sv_label = sv_adjacency_list_[query_point_id];
313 
314  processed_[sv_label] = true;
315 
316  // The next two lines add the supervoxel to the segment
317  sv_label_to_seg_label_map_[sv_label] = segment_label;
318  seg_label_to_sv_list_map_[segment_label].insert (sv_label);
319 
320  OutEdgeIterator out_Edge_itr, out_Edge_itr_end;
321  // Iterate through all neighbors of this supervoxel and check whether they should be merged with the current supervoxel
322  // boost::out_edges (query_point_id, sv_adjacency_list_): adjacent vertices to node (*itr) in graph sv_adjacency_list_
323  for (std::tie(out_Edge_itr, out_Edge_itr_end) = boost::out_edges (query_point_id, sv_adjacency_list_); out_Edge_itr != out_Edge_itr_end; ++out_Edge_itr)
324  {
325  const VertexID neighbor_ID = boost::target (*out_Edge_itr, sv_adjacency_list_);
326  const std::uint32_t& neighbor_label = sv_adjacency_list_[neighbor_ID];
327 
328  if (!processed_[neighbor_label]) // If neighbor was not already processed
329  {
330  if (sv_adjacency_list_[*out_Edge_itr].is_valid)
331  {
332  recursiveSegmentGrowing (neighbor_ID, segment_label);
333  }
334  }
335  } // End neighbor loop
336 }
337 
338 template <typename PointT> void
340 {
341  if (k_arg == 0)
342  return;
343 
344  EdgeIterator edge_itr, edge_itr_end, next_edge;
345  // Check all edges in the graph for k-convexity
346  for (std::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_), next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
347  {
348  ++next_edge; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge
349 
350  bool is_convex = sv_adjacency_list_[*edge_itr].is_convex;
351 
352  if (is_convex) // If edge is (0-)convex
353  {
354  unsigned int kcount = 0;
355 
356  const VertexID source = boost::source (*edge_itr, sv_adjacency_list_);
357  const VertexID target = boost::target (*edge_itr, sv_adjacency_list_);
358 
359  OutEdgeIterator source_neighbors_itr, source_neighbors_itr_end;
360  // Find common neighbors, check their connection
361  for (std::tie(source_neighbors_itr, source_neighbors_itr_end) = boost::out_edges (source, sv_adjacency_list_); source_neighbors_itr != source_neighbors_itr_end; ++source_neighbors_itr) // For all supervoxels
362  {
363  VertexID source_neighbor_ID = boost::target (*source_neighbors_itr, sv_adjacency_list_);
364 
365  OutEdgeIterator target_neighbors_itr, target_neighbors_itr_end;
366  for (std::tie(target_neighbors_itr, target_neighbors_itr_end) = boost::out_edges (target, sv_adjacency_list_); target_neighbors_itr != target_neighbors_itr_end; ++target_neighbors_itr) // For all supervoxels
367  {
368  VertexID target_neighbor_ID = boost::target (*target_neighbors_itr, sv_adjacency_list_);
369  if (source_neighbor_ID == target_neighbor_ID) // Common neighbor
370  {
371  EdgeID src_edge = boost::edge (source, source_neighbor_ID, sv_adjacency_list_).first;
372  EdgeID tar_edge = boost::edge (target, source_neighbor_ID, sv_adjacency_list_).first;
373 
374  bool src_is_convex = (sv_adjacency_list_)[src_edge].is_convex;
375  bool tar_is_convex = (sv_adjacency_list_)[tar_edge].is_convex;
376 
377  if (src_is_convex && tar_is_convex)
378  ++kcount;
379 
380  break;
381  }
382  }
383 
384  if (kcount >= k_arg) // Connection is k-convex, stop search
385  break;
386  }
387 
388  // Check k convexity
389  if (kcount < k_arg)
390  (sv_adjacency_list_)[*edge_itr].is_valid = false;
391  }
392  }
393 }
394 
395 template <typename PointT> void
397 {
398 
399  EdgeIterator edge_itr, edge_itr_end, next_edge;
400  for (std::tie(edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg), next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
401  {
402  ++next_edge; // next_edge iterator is necessary, because removing an edge invalidates the iterator to the current edge
403 
404  std::uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)];
405  std::uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
406 
407  float normal_difference;
408  bool is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference);
409  adjacency_list_arg[*edge_itr].is_convex = is_convex;
410  adjacency_list_arg[*edge_itr].is_valid = is_convex;
411  adjacency_list_arg[*edge_itr].normal_difference = normal_difference;
412  }
413 }
414 
415 template <typename PointT> bool
416 pcl::LCCPSegmentation<PointT>::connIsConvex (const std::uint32_t source_label_arg,
417  const std::uint32_t target_label_arg,
418  float &normal_angle)
419 {
420  typename pcl::Supervoxel<PointT>::Ptr& sv_source = sv_label_to_supervoxel_map_[source_label_arg];
421  typename pcl::Supervoxel<PointT>::Ptr& sv_target = sv_label_to_supervoxel_map_[target_label_arg];
422 
423  const Eigen::Vector3f& source_centroid = sv_source->centroid_.getVector3fMap ();
424  const Eigen::Vector3f& target_centroid = sv_target->centroid_.getVector3fMap ();
425 
426  const Eigen::Vector3f& source_normal = sv_source->normal_.getNormalVector3fMap (). normalized ();
427  const Eigen::Vector3f& target_normal = sv_target->normal_.getNormalVector3fMap (). normalized ();
428 
429  //NOTE For angles below 0 nothing will be merged
430  if (concavity_tolerance_threshold_ < 0)
431  {
432  return (false);
433  }
434 
435  bool is_convex = true;
436  bool is_smooth = true;
437 
438  normal_angle = getAngle3D (source_normal, target_normal, true);
439  // Geometric comparisons
440  Eigen::Vector3f vec_t_to_s, vec_s_to_t;
441 
442  vec_t_to_s = source_centroid - target_centroid;
443  vec_s_to_t = -vec_t_to_s;
444 
445  Eigen::Vector3f ncross;
446  ncross = source_normal.cross (target_normal);
447 
448  // Smoothness Check: Check if there is a step between adjacent patches
449  if (use_smoothness_check_)
450  {
451  float expected_distance = ncross.norm () * seed_resolution_;
452  float dot_p_1 = vec_t_to_s.dot (source_normal);
453  float dot_p_2 = vec_s_to_t.dot (target_normal);
454  float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2);
455  const float dist_smoothing = smoothness_threshold_ * voxel_resolution_; // This is a slacking variable especially important for patches with very similar normals
456 
457  if (point_dist > (expected_distance + dist_smoothing))
458  {
459  is_smooth &= false;
460  }
461  }
462  // ----------------
463 
464  // Sanity Criterion: Check if definition convexity/concavity makes sense for connection of given patches
465  float intersection_angle = getAngle3D (ncross, vec_t_to_s, true);
466  float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle;
467 
468  float intersect_thresh = 60. * 1. / (1. + std::exp (-0.25 * (normal_angle - 25.)));
469  if (min_intersect_angle < intersect_thresh && use_sanity_check_)
470  {
471  // std::cout << "Concave/Convex not defined for given case!" << std::endl;
472  is_convex &= false;
473  }
474 
475 
476  // vec_t_to_s is the reference direction for angle measurements
477  // Convexity Criterion: Check if connection of patches is convex. If this is the case the two supervoxels should be merged.
478  if ((getAngle3D (vec_t_to_s, source_normal) - getAngle3D (vec_t_to_s, target_normal)) <= 0)
479  {
480  is_convex &= true; // connection convex
481  }
482  else
483  {
484  is_convex &= (normal_angle < concavity_tolerance_threshold_); // concave connections will be accepted if difference of normals is small
485  }
486  return (is_convex && is_smooth);
487 }
488 
489 #endif // PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
virtual ~LCCPSegmentation()
typename boost::graph_traits< SupervoxelAdjacencyList >::vertex_iterator VertexIterator
typename boost::graph_traits< SupervoxelAdjacencyList >::edge_iterator EdgeIterator
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, std::uint32_t, EdgeProperties > SupervoxelAdjacencyList
void recursiveSegmentGrowing(const VertexID &queryPointID, const unsigned int group_label)
Assigns neighbors of the query point to the same group as the query point.
void calculateConvexConnections(SupervoxelAdjacencyList &adjacency_list_arg)
Calculates convexity of edges and saves this to the adjacency graph.
void computeSegmentAdjacency()
Compute the adjacency of the segments.
void relabelCloud(pcl::PointCloud< pcl::PointXYZL > &labeled_cloud_arg)
Relabels cloud with supervoxel labels with the computed segment labels.
void mergeSmallSegments()
Segments smaller than min_segment_size_ are merged to the label of largest neighbor.
void prepareSegmentation(const std::map< std::uint32_t, typename pcl::Supervoxel< PointT >::Ptr > &supervoxel_clusters_arg, const std::multimap< std::uint32_t, std::uint32_t > &label_adjacency_arg)
Is called within setInputSupervoxels mainly to reserve required memory.
void segment()
Merge supervoxels using local convexity.
typename boost::graph_traits< SupervoxelAdjacencyList >::out_edge_iterator OutEdgeIterator
typename boost::graph_traits< SupervoxelAdjacencyList >::adjacency_iterator AdjacencyIterator
bool connIsConvex(const std::uint32_t source_label_arg, const std::uint32_t target_label_arg, float &normal_angle)
Returns true if the connection between source and target is convex.
void doGrouping()
Perform depth search on the graph and recursively group all supervoxels with convex connections.
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.
typename boost::graph_traits< SupervoxelAdjacencyList >::edge_descriptor EdgeID
typename boost::graph_traits< SupervoxelAdjacencyList >::vertex_descriptor VertexID
void reset()
Reset internal memory.
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
pcl::PointXYZRGBA centroid_
The centroid of the supervoxel - average voxel.
shared_ptr< Supervoxel< PointT > > Ptr
pcl::Normal normal_
The normal calculated for the voxels contained in the supervoxel.
Define standard C methods and C++ classes that are common to all methods.
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree.
Definition: common.hpp:47