Point Cloud Library (PCL)  1.14.1
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_
typename boost::graph_traits< SupervoxelAdjacencyList >::out_edge_iterator OutEdgeIterator
void doGrouping()
Perform depth search on the graph and recursively group all supervoxels with convex connections...
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.
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
pcl::PointXYZRGBA centroid_
The centroid of the supervoxel - average voxel.
pcl::Normal normal_
The normal calculated for the voxels contained in the supervoxel.
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.
typename boost::graph_traits< SupervoxelAdjacencyList >::edge_iterator EdgeIterator
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.
typename boost::graph_traits< SupervoxelAdjacencyList >::edge_descriptor EdgeID
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, std::uint32_t, EdgeProperties > SupervoxelAdjacencyList
virtual ~LCCPSegmentation()
void reset()
Reset internal memory.
typename boost::graph_traits< SupervoxelAdjacencyList >::vertex_descriptor VertexID
typename boost::graph_traits< SupervoxelAdjacencyList >::vertex_iterator VertexIterator
shared_ptr< Supervoxel< PointT > > Ptr
void mergeSmallSegments()
Segments smaller than min_segment_size_ are merged to the label of largest neighbor.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Definition: distances.h:55
void segment()
Merge supervoxels using local convexity.
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 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 >::adjacency_iterator AdjacencyIterator