Point Cloud Library (PCL) 1.14.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
56template <typename PointT>
58
59template <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
72template <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
97template <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
122template <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
153template <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
227template <typename PointT> void
228pcl::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
277template <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
308template <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
338template <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
395template <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
415template <typename PointT> bool
416pcl::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.
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