38 #ifndef PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
39 #define PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
41 #include <pcl/segmentation/lccp_segmentation.h>
53 template <
typename Po
intT>
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),
62 voxel_resolution_ (0),
68 template <
typename Po
intT>
73 template <
typename Po
intT>
void
76 sv_adjacency_list_.clear ();
78 sv_label_to_supervoxel_map_.clear ();
79 sv_label_to_seg_label_map_.clear ();
80 seg_label_to_sv_list_map_.clear ();
81 seg_label_to_neighbor_set_map_.clear ();
82 grouping_data_valid_ =
false;
83 supervoxels_set_ =
false;
86 template <
typename Po
intT>
void
93 calculateConvexConnections (sv_adjacency_list_);
96 applyKconvexity (k_factor_);
101 grouping_data_valid_ =
true;
104 mergeSmallSegments ();
107 PCL_WARN (
"[pcl::LCCPSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
111 template <
typename Po
intT>
void
114 if (grouping_data_valid_)
117 for (
auto &voxel : labeled_cloud_arg)
119 voxel.label = sv_label_to_seg_label_map_[voxel.label];
124 PCL_WARN (
"[pcl::LCCPSegmentation::relabelCloud] WARNING: Call function segment first. Nothing has been done. \n");
136 template <
typename Po
intT>
void
139 seg_label_to_neighbor_set_map_.clear ();
142 std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
143 vertex_iterator_range = boost::vertices (sv_adjacency_list_);
145 std::uint32_t current_segLabel;
146 std::uint32_t neigh_segLabel;
149 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
151 const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
152 current_segLabel = sv_label_to_seg_label_map_[sv_label];
155 std::pair<AdjacencyIterator, AdjacencyIterator> neighbors = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_);
156 for (
AdjacencyIterator itr_neighbor = neighbors.first; itr_neighbor != neighbors.second; ++itr_neighbor)
158 const std::uint32_t& neigh_label = sv_adjacency_list_[*itr_neighbor];
159 neigh_segLabel = sv_label_to_seg_label_map_[neigh_label];
161 if (current_segLabel != neigh_segLabel)
163 seg_label_to_neighbor_set_map_[current_segLabel].insert (neigh_segLabel);
169 template <
typename Po
intT>
void
172 if (min_segment_size_ == 0)
175 computeSegmentAdjacency ();
177 std::set<std::uint32_t> filteredSegLabels;
179 std::uint32_t largest_neigh_size = 0;
180 std::uint32_t largest_neigh_seg_label = 0;
181 std::uint32_t current_seg_label;
183 std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
184 vertex_iterator_range = boost::vertices (sv_adjacency_list_);
186 bool continue_filtering =
true;
188 while (continue_filtering)
190 continue_filtering =
false;
191 unsigned int nr_filtered = 0;
194 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
196 const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
197 current_seg_label = sv_label_to_seg_label_map_[sv_label];
198 largest_neigh_seg_label = current_seg_label;
199 largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size ();
201 const std::uint32_t& nr_neighbors = seg_label_to_neighbor_set_map_[current_seg_label].size ();
202 if (nr_neighbors == 0)
205 if (seg_label_to_sv_list_map_[current_seg_label].size () <= min_segment_size_)
207 continue_filtering =
true;
211 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)
213 if (seg_label_to_sv_list_map_[*neighbors_itr].size () >= largest_neigh_size)
215 largest_neigh_seg_label = *neighbors_itr;
216 largest_neigh_size = seg_label_to_sv_list_map_[*neighbors_itr].size ();
221 if (largest_neigh_seg_label != current_seg_label)
223 if (filteredSegLabels.count (largest_neigh_seg_label) > 0)
226 sv_label_to_seg_label_map_[sv_label] = largest_neigh_seg_label;
227 filteredSegLabels.insert (current_seg_label);
230 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)
232 seg_label_to_sv_list_map_[largest_neigh_seg_label].insert (*sv_ID_itr);
239 for (
const unsigned int &filteredSegLabel : filteredSegLabels)
241 seg_label_to_sv_list_map_.erase (filteredSegLabel);
247 computeSegmentAdjacency ();
251 template <
typename Po
intT>
void
253 const std::multimap<std::uint32_t, std::uint32_t>& label_adjaceny_arg)
259 sv_label_to_supervoxel_map_ = supervoxel_clusters_arg;
262 std::map<std::uint32_t, VertexID> label_ID_map;
265 for (
typename std::map<std::uint32_t,
typename pcl::Supervoxel<PointT>::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin ();
266 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
268 const std::uint32_t& sv_label = svlabel_itr->first;
269 VertexID node_id = boost::add_vertex (sv_adjacency_list_);
270 sv_adjacency_list_[node_id] = sv_label;
271 label_ID_map[sv_label] = node_id;
275 for (
const auto &sv_neighbors_itr : label_adjaceny_arg)
277 const std::uint32_t& sv_label = sv_neighbors_itr.first;
278 const std::uint32_t& neighbor_label = sv_neighbors_itr.second;
280 VertexID u = label_ID_map[sv_label];
281 VertexID v = label_ID_map[neighbor_label];
283 boost::add_edge (u, v, sv_adjacency_list_);
288 seg_label_to_sv_list_map_.clear ();
289 for (
typename std::map<std::uint32_t,
typename pcl::Supervoxel<PointT>::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin ();
290 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
292 const std::uint32_t& sv_label = svlabel_itr->first;
293 processed_[sv_label] =
false;
294 sv_label_to_seg_label_map_[sv_label] = 0;
301 template <
typename Po
intT>
void
305 seg_label_to_sv_list_map_.clear ();
306 for (
typename std::map<std::uint32_t,
typename pcl::Supervoxel<PointT>::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin ();
307 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
309 const std::uint32_t& sv_label = svlabel_itr->first;
310 processed_[sv_label] =
false;
311 sv_label_to_seg_label_map_[sv_label] = 0;
316 std::pair< VertexIterator, VertexIterator> vertex_iterator_range;
317 vertex_iterator_range = boost::vertices (sv_adjacency_list_);
320 unsigned int segment_label = 1;
321 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
323 const VertexID sv_vertex_id = *sv_itr;
324 const std::uint32_t& sv_label = sv_adjacency_list_[sv_vertex_id];
325 if (!processed_[sv_label])
328 recursiveSegmentGrowing (sv_vertex_id, segment_label);
334 template <
typename Po
intT>
void
336 const unsigned int segment_label)
338 const std::uint32_t& sv_label = sv_adjacency_list_[query_point_id];
340 processed_[sv_label] =
true;
343 sv_label_to_seg_label_map_[sv_label] = segment_label;
344 seg_label_to_sv_list_map_[segment_label].insert (sv_label);
347 std::pair<OutEdgeIterator, OutEdgeIterator> out_edge_iterator_range;
348 out_edge_iterator_range = boost::out_edges (query_point_id, sv_adjacency_list_);
349 for (
OutEdgeIterator out_Edge_itr = out_edge_iterator_range.first; out_Edge_itr != out_edge_iterator_range.second; ++out_Edge_itr)
351 const VertexID neighbor_ID = boost::target (*out_Edge_itr, sv_adjacency_list_);
352 const std::uint32_t& neighbor_label = sv_adjacency_list_[neighbor_ID];
354 if (!processed_[neighbor_label])
356 if (sv_adjacency_list_[*out_Edge_itr].is_valid)
358 recursiveSegmentGrowing (neighbor_ID, segment_label);
364 template <
typename Po
intT>
void
370 unsigned int kcount = 0;
373 boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_);
375 std::pair<OutEdgeIterator, OutEdgeIterator> source_neighbors_range;
376 std::pair<OutEdgeIterator, OutEdgeIterator> target_neighbors_range;
379 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
383 bool is_convex = sv_adjacency_list_[*edge_itr].is_convex;
389 const VertexID source = boost::source (*edge_itr, sv_adjacency_list_);
390 const VertexID target = boost::target (*edge_itr, sv_adjacency_list_);
392 source_neighbors_range = boost::out_edges (source, sv_adjacency_list_);
393 target_neighbors_range = boost::out_edges (target, sv_adjacency_list_);
396 for (
OutEdgeIterator source_neighbors_itr = source_neighbors_range.first; source_neighbors_itr != source_neighbors_range.second; ++source_neighbors_itr)
398 VertexID source_neighbor_ID = boost::target (*source_neighbors_itr, sv_adjacency_list_);
400 for (
OutEdgeIterator target_neighbors_itr = target_neighbors_range.first; target_neighbors_itr != target_neighbors_range.second; ++target_neighbors_itr)
402 VertexID target_neighbor_ID = boost::target (*target_neighbors_itr, sv_adjacency_list_);
403 if (source_neighbor_ID == target_neighbor_ID)
405 EdgeID src_edge = boost::edge (source, source_neighbor_ID, sv_adjacency_list_).first;
406 EdgeID tar_edge = boost::edge (target, source_neighbor_ID, sv_adjacency_list_).first;
408 bool src_is_convex = (sv_adjacency_list_)[src_edge].is_convex;
409 bool tar_is_convex = (sv_adjacency_list_)[tar_edge].is_convex;
411 if (src_is_convex && tar_is_convex)
424 (sv_adjacency_list_)[*edge_itr].is_valid =
false;
429 template <
typename Po
intT>
void
434 boost::tie (edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg);
436 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
440 std::uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)];
441 std::uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
443 float normal_difference;
444 bool is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference);
445 adjacency_list_arg[*edge_itr].is_convex = is_convex;
446 adjacency_list_arg[*edge_itr].is_valid = is_convex;
447 adjacency_list_arg[*edge_itr].normal_difference = normal_difference;
451 template <
typename Po
intT>
bool
453 const std::uint32_t target_label_arg,
459 const Eigen::Vector3f& source_centroid = sv_source->
centroid_.getVector3fMap ();
460 const Eigen::Vector3f& target_centroid = sv_target->
centroid_.getVector3fMap ();
462 const Eigen::Vector3f& source_normal = sv_source->
normal_.getNormalVector3fMap (). normalized ();
463 const Eigen::Vector3f& target_normal = sv_target->
normal_.getNormalVector3fMap (). normalized ();
466 if (concavity_tolerance_threshold_ < 0)
471 bool is_convex =
true;
472 bool is_smooth =
true;
474 normal_angle =
getAngle3D (source_normal, target_normal,
true);
476 Eigen::Vector3f vec_t_to_s, vec_s_to_t;
478 vec_t_to_s = source_centroid - target_centroid;
479 vec_s_to_t = -vec_t_to_s;
481 Eigen::Vector3f ncross;
482 ncross = source_normal.cross (target_normal);
485 if (use_smoothness_check_)
487 float expected_distance = ncross.norm () * seed_resolution_;
488 float dot_p_1 = vec_t_to_s.dot (source_normal);
489 float dot_p_2 = vec_s_to_t.dot (target_normal);
490 float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2);
491 const float dist_smoothing = smoothness_threshold_ * voxel_resolution_;
493 if (point_dist > (expected_distance + dist_smoothing))
501 float intersection_angle =
getAngle3D (ncross, vec_t_to_s,
true);
502 float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle;
504 float intersect_thresh = 60. * 1. / (1. + std::exp (-0.25 * (normal_angle - 25.)));
505 if (min_intersect_angle < intersect_thresh && use_sanity_check_)
520 is_convex &= (normal_angle < concavity_tolerance_threshold_);
522 return (is_convex && is_smooth);
525 #endif // PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_