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_)
118 for (; voxel_itr != labeled_cloud_arg.
end (); ++voxel_itr)
120 voxel_itr->label = sv_label_to_seg_label_map_[voxel_itr->label];
125 PCL_WARN (
"[pcl::LCCPSegmentation::relabelCloud] WARNING: Call function segment first. Nothing has been done. \n");
137 template <
typename Po
intT>
void
140 seg_label_to_neighbor_set_map_.clear ();
143 std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
144 vertex_iterator_range = boost::vertices (sv_adjacency_list_);
146 uint32_t current_segLabel;
147 uint32_t neigh_segLabel;
150 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
152 const uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
153 current_segLabel = sv_label_to_seg_label_map_[sv_label];
156 std::pair<AdjacencyIterator, AdjacencyIterator> neighbors = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_);
157 for (
AdjacencyIterator itr_neighbor = neighbors.first; itr_neighbor != neighbors.second; ++itr_neighbor)
159 const uint32_t& neigh_label = sv_adjacency_list_[*itr_neighbor];
160 neigh_segLabel = sv_label_to_seg_label_map_[neigh_label];
162 if (current_segLabel != neigh_segLabel)
164 seg_label_to_neighbor_set_map_[current_segLabel].insert (neigh_segLabel);
170 template <
typename Po
intT>
void
173 if (min_segment_size_ == 0)
176 computeSegmentAdjacency ();
178 std::set<uint32_t> filteredSegLabels;
180 uint32_t largest_neigh_size = 0;
181 uint32_t largest_neigh_seg_label = 0;
182 uint32_t current_seg_label;
184 std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
185 vertex_iterator_range = boost::vertices (sv_adjacency_list_);
187 bool continue_filtering =
true;
189 while (continue_filtering)
191 continue_filtering =
false;
192 unsigned int nr_filtered = 0;
195 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
197 const uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
198 current_seg_label = sv_label_to_seg_label_map_[sv_label];
199 largest_neigh_seg_label = current_seg_label;
200 largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size ();
202 const uint32_t& nr_neighbors = seg_label_to_neighbor_set_map_[current_seg_label].size ();
203 if (nr_neighbors == 0)
206 if (seg_label_to_sv_list_map_[current_seg_label].size () <= min_segment_size_)
208 continue_filtering =
true;
212 std::set<uint32_t>::const_iterator neighbors_itr = seg_label_to_neighbor_set_map_[current_seg_label].begin ();
213 for (; neighbors_itr != seg_label_to_neighbor_set_map_[current_seg_label].end (); ++neighbors_itr)
215 if (seg_label_to_sv_list_map_[*neighbors_itr].size () >= largest_neigh_size)
217 largest_neigh_seg_label = *neighbors_itr;
218 largest_neigh_size = seg_label_to_sv_list_map_[*neighbors_itr].size ();
223 if (largest_neigh_seg_label != current_seg_label)
225 if (filteredSegLabels.count (largest_neigh_seg_label) > 0)
228 sv_label_to_seg_label_map_[sv_label] = largest_neigh_seg_label;
229 filteredSegLabels.insert (current_seg_label);
232 std::set<uint32_t>::iterator sv_ID_itr = seg_label_to_sv_list_map_[current_seg_label].begin ();
233 sv_ID_itr = seg_label_to_sv_list_map_[current_seg_label].begin ();
234 for (; sv_ID_itr != seg_label_to_sv_list_map_[current_seg_label].end (); ++sv_ID_itr)
236 seg_label_to_sv_list_map_[largest_neigh_seg_label].insert (*sv_ID_itr);
243 std::set<uint32_t>::iterator filtered_ID_itr = filteredSegLabels.begin ();
244 for (; filtered_ID_itr != filteredSegLabels.end (); ++filtered_ID_itr)
246 seg_label_to_sv_list_map_.erase (*filtered_ID_itr);
252 computeSegmentAdjacency ();
256 template <
typename Po
intT>
void
258 const std::multimap<uint32_t, uint32_t>& label_adjaceny_arg)
264 sv_label_to_supervoxel_map_ = supervoxel_clusters_arg;
267 std::map<uint32_t, VertexID> label_ID_map;
271 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
273 const uint32_t& sv_label = svlabel_itr->first;
274 VertexID node_id = boost::add_vertex (sv_adjacency_list_);
275 sv_adjacency_list_[node_id] = sv_label;
276 label_ID_map[sv_label] = node_id;
280 for (std::multimap<uint32_t, uint32_t>::const_iterator sv_neighbors_itr = label_adjaceny_arg.begin (); sv_neighbors_itr != label_adjaceny_arg.end ();
283 const uint32_t& sv_label = sv_neighbors_itr->first;
284 const uint32_t& neighbor_label = sv_neighbors_itr->second;
286 VertexID u = label_ID_map[sv_label];
287 VertexID v = label_ID_map[neighbor_label];
289 boost::add_edge (u, v, sv_adjacency_list_);
294 seg_label_to_sv_list_map_.clear ();
296 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
298 const uint32_t& sv_label = svlabel_itr->first;
299 processed_[sv_label] =
false;
300 sv_label_to_seg_label_map_[sv_label] = 0;
307 template <
typename Po
intT>
void
311 seg_label_to_sv_list_map_.clear ();
313 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
315 const uint32_t& sv_label = svlabel_itr->first;
316 processed_[sv_label] =
false;
317 sv_label_to_seg_label_map_[sv_label] = 0;
322 std::pair< VertexIterator, VertexIterator> vertex_iterator_range;
323 vertex_iterator_range = boost::vertices (sv_adjacency_list_);
326 unsigned int segment_label = 1;
327 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
329 const VertexID sv_vertex_id = *sv_itr;
330 const uint32_t& sv_label = sv_adjacency_list_[sv_vertex_id];
331 if (!processed_[sv_label])
334 recursiveSegmentGrowing (sv_vertex_id, segment_label);
340 template <
typename Po
intT>
void
342 const unsigned int segment_label)
344 const uint32_t& sv_label = sv_adjacency_list_[query_point_id];
346 processed_[sv_label] =
true;
349 sv_label_to_seg_label_map_[sv_label] = segment_label;
350 seg_label_to_sv_list_map_[segment_label].insert (sv_label);
353 std::pair<OutEdgeIterator, OutEdgeIterator> out_edge_iterator_range;
354 out_edge_iterator_range = boost::out_edges (query_point_id, sv_adjacency_list_);
355 for (
OutEdgeIterator out_Edge_itr = out_edge_iterator_range.first; out_Edge_itr != out_edge_iterator_range.second; ++out_Edge_itr)
357 const VertexID neighbor_ID = boost::target (*out_Edge_itr, sv_adjacency_list_);
358 const uint32_t& neighbor_label = sv_adjacency_list_[neighbor_ID];
360 if (!processed_[neighbor_label])
362 if (sv_adjacency_list_[*out_Edge_itr].is_valid)
364 recursiveSegmentGrowing (neighbor_ID, segment_label);
370 template <
typename Po
intT>
void
377 unsigned int kcount = 0;
380 boost::tie (edge_itr, edge_itr_end) = boost::edges (sv_adjacency_list_);
382 std::pair<OutEdgeIterator, OutEdgeIterator> source_neighbors_range;
383 std::pair<OutEdgeIterator, OutEdgeIterator> target_neighbors_range;
386 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
390 is_convex = sv_adjacency_list_[*edge_itr].is_convex;
396 const VertexID source = boost::source (*edge_itr, sv_adjacency_list_);
397 const VertexID target = boost::target (*edge_itr, sv_adjacency_list_);
399 source_neighbors_range = boost::out_edges (source, sv_adjacency_list_);
400 target_neighbors_range = boost::out_edges (target, sv_adjacency_list_);
403 for (
OutEdgeIterator source_neighbors_itr = source_neighbors_range.first; source_neighbors_itr != source_neighbors_range.second; ++source_neighbors_itr)
405 VertexID source_neighbor_ID = boost::target (*source_neighbors_itr, sv_adjacency_list_);
407 for (
OutEdgeIterator target_neighbors_itr = target_neighbors_range.first; target_neighbors_itr != target_neighbors_range.second; ++target_neighbors_itr)
409 VertexID target_neighbor_ID = boost::target (*target_neighbors_itr, sv_adjacency_list_);
410 if (source_neighbor_ID == target_neighbor_ID)
412 EdgeID src_edge = boost::edge (source, source_neighbor_ID, sv_adjacency_list_).first;
413 EdgeID tar_edge = boost::edge (target, source_neighbor_ID, sv_adjacency_list_).first;
415 bool src_is_convex = (sv_adjacency_list_)[src_edge].is_convex;
416 bool tar_is_convex = (sv_adjacency_list_)[tar_edge].is_convex;
418 if (src_is_convex && tar_is_convex)
431 (sv_adjacency_list_)[*edge_itr].is_valid =
false;
436 template <
typename Po
intT>
void
442 boost::tie (edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg);
444 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
448 uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)];
449 uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
451 float normal_difference;
452 is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference);
453 adjacency_list_arg[*edge_itr].is_convex = is_convex;
454 adjacency_list_arg[*edge_itr].is_valid = is_convex;
455 adjacency_list_arg[*edge_itr].normal_difference = normal_difference;
459 template <
typename Po
intT>
bool
461 const uint32_t target_label_arg,
467 const Eigen::Vector3f& source_centroid = sv_source->
centroid_.getVector3fMap ();
468 const Eigen::Vector3f& target_centroid = sv_target->
centroid_.getVector3fMap ();
470 const Eigen::Vector3f& source_normal = sv_source->
normal_.getNormalVector3fMap (). normalized ();
471 const Eigen::Vector3f& target_normal = sv_target->
normal_.getNormalVector3fMap (). normalized ();
474 if (concavity_tolerance_threshold_ < 0)
479 bool is_convex =
true;
480 bool is_smooth =
true;
482 normal_angle =
getAngle3D (source_normal, target_normal,
true);
484 Eigen::Vector3f vec_t_to_s, vec_s_to_t;
486 vec_t_to_s = source_centroid - target_centroid;
487 vec_s_to_t = -vec_t_to_s;
489 Eigen::Vector3f ncross;
490 ncross = source_normal.cross (target_normal);
493 if (use_smoothness_check_)
495 float expected_distance = ncross.norm () * seed_resolution_;
496 float dot_p_1 = vec_t_to_s.dot (source_normal);
497 float dot_p_2 = vec_s_to_t.dot (target_normal);
498 float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2);
499 const float dist_smoothing = smoothness_threshold_ * voxel_resolution_;
501 if (point_dist > (expected_distance + dist_smoothing))
509 float intersection_angle =
getAngle3D (ncross, vec_t_to_s,
true);
510 float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle;
512 float intersect_thresh = 60. * 1. / (1. + exp (-0.25 * (normal_angle - 25.)));
513 if (min_intersect_angle < intersect_thresh && use_sanity_check_)
528 is_convex &= (normal_angle < concavity_tolerance_threshold_);
530 return (is_convex && is_smooth);
533 #endif // PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_