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