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 ();
141 std::uint32_t current_segLabel;
142 std::uint32_t neigh_segLabel;
147 for(std::tie(sv_itr, sv_itr_end) = boost::vertices(sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr)
149 const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
150 current_segLabel = sv_label_to_seg_label_map_[sv_label];
154 for (std::tie(itr_neighbor, itr_neighbor_end) = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_); itr_neighbor != itr_neighbor_end; ++itr_neighbor)
156 const std::uint32_t& neigh_label = sv_adjacency_list_[*itr_neighbor];
157 neigh_segLabel = sv_label_to_seg_label_map_[neigh_label];
159 if (current_segLabel != neigh_segLabel)
161 seg_label_to_neighbor_set_map_[current_segLabel].insert (neigh_segLabel);
167 template <
typename Po
intT>
void
170 if (min_segment_size_ == 0)
173 computeSegmentAdjacency ();
175 std::set<std::uint32_t> filteredSegLabels;
177 bool continue_filtering =
true;
179 while (continue_filtering)
181 continue_filtering =
false;
182 unsigned int nr_filtered = 0;
186 for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr)
188 const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
189 std::uint32_t current_seg_label = sv_label_to_seg_label_map_[sv_label];
190 std::uint32_t largest_neigh_seg_label = current_seg_label;
191 std::uint32_t largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size ();
193 const std::uint32_t& nr_neighbors = seg_label_to_neighbor_set_map_[current_seg_label].size ();
194 if (nr_neighbors == 0)
197 if (seg_label_to_sv_list_map_[current_seg_label].size () <= min_segment_size_)
199 continue_filtering =
true;
203 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)
205 if (seg_label_to_sv_list_map_[*neighbors_itr].size () >= largest_neigh_size)
207 largest_neigh_seg_label = *neighbors_itr;
208 largest_neigh_size = seg_label_to_sv_list_map_[*neighbors_itr].size ();
213 if (largest_neigh_seg_label != current_seg_label)
215 if (filteredSegLabels.count (largest_neigh_seg_label) > 0)
218 sv_label_to_seg_label_map_[sv_label] = largest_neigh_seg_label;
219 filteredSegLabels.insert (current_seg_label);
222 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)
224 seg_label_to_sv_list_map_[largest_neigh_seg_label].insert (*sv_ID_itr);
231 for (
const unsigned int &filteredSegLabel : filteredSegLabels)
233 seg_label_to_sv_list_map_.erase (filteredSegLabel);
239 computeSegmentAdjacency ();
243 template <
typename Po
intT>
void
245 const std::multimap<std::uint32_t, std::uint32_t>& label_adjaceny_arg)
251 sv_label_to_supervoxel_map_ = supervoxel_clusters_arg;
254 std::map<std::uint32_t, VertexID> label_ID_map;
257 for (
typename std::map<std::uint32_t,
typename pcl::Supervoxel<PointT>::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin ();
258 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
260 const std::uint32_t& sv_label = svlabel_itr->first;
261 VertexID node_id = boost::add_vertex (sv_adjacency_list_);
262 sv_adjacency_list_[node_id] = sv_label;
263 label_ID_map[sv_label] = node_id;
267 for (
const auto &sv_neighbors_itr : label_adjaceny_arg)
269 const std::uint32_t& sv_label = sv_neighbors_itr.first;
270 const std::uint32_t& neighbor_label = sv_neighbors_itr.second;
272 VertexID u = label_ID_map[sv_label];
273 VertexID v = label_ID_map[neighbor_label];
275 boost::add_edge (u, v, sv_adjacency_list_);
280 seg_label_to_sv_list_map_.clear ();
281 for (
typename std::map<std::uint32_t,
typename pcl::Supervoxel<PointT>::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin ();
282 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
284 const std::uint32_t& sv_label = svlabel_itr->first;
285 processed_[sv_label] =
false;
286 sv_label_to_seg_label_map_[sv_label] = 0;
293 template <
typename Po
intT>
void
297 seg_label_to_sv_list_map_.clear ();
298 for (
typename std::map<std::uint32_t,
typename pcl::Supervoxel<PointT>::Ptr>::iterator svlabel_itr = sv_label_to_supervoxel_map_.begin ();
299 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
301 const std::uint32_t& sv_label = svlabel_itr->first;
302 processed_[sv_label] =
false;
303 sv_label_to_seg_label_map_[sv_label] = 0;
310 unsigned int segment_label = 1;
311 for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr)
313 const VertexID sv_vertex_id = *sv_itr;
314 const std::uint32_t& sv_label = sv_adjacency_list_[sv_vertex_id];
315 if (!processed_[sv_label])
318 recursiveSegmentGrowing (sv_vertex_id, segment_label);
324 template <
typename Po
intT>
void
326 const unsigned int segment_label)
328 const std::uint32_t& sv_label = sv_adjacency_list_[query_point_id];
330 processed_[sv_label] =
true;
333 sv_label_to_seg_label_map_[sv_label] = segment_label;
334 seg_label_to_sv_list_map_[segment_label].insert (sv_label);
339 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)
341 const VertexID neighbor_ID = boost::target (*out_Edge_itr, sv_adjacency_list_);
342 const std::uint32_t& neighbor_label = sv_adjacency_list_[neighbor_ID];
344 if (!processed_[neighbor_label])
346 if (sv_adjacency_list_[*out_Edge_itr].is_valid)
348 recursiveSegmentGrowing (neighbor_ID, segment_label);
354 template <
typename Po
intT>
void
362 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)
366 bool is_convex = sv_adjacency_list_[*edge_itr].is_convex;
370 unsigned int kcount = 0;
372 const VertexID source = boost::source (*edge_itr, sv_adjacency_list_);
373 const VertexID target = boost::target (*edge_itr, sv_adjacency_list_);
377 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)
379 VertexID source_neighbor_ID = boost::target (*source_neighbors_itr, sv_adjacency_list_);
382 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)
384 VertexID target_neighbor_ID = boost::target (*target_neighbors_itr, sv_adjacency_list_);
385 if (source_neighbor_ID == target_neighbor_ID)
387 EdgeID src_edge = boost::edge (source, source_neighbor_ID, sv_adjacency_list_).first;
388 EdgeID tar_edge = boost::edge (target, source_neighbor_ID, sv_adjacency_list_).first;
390 bool src_is_convex = (sv_adjacency_list_)[src_edge].is_convex;
391 bool tar_is_convex = (sv_adjacency_list_)[tar_edge].is_convex;
393 if (src_is_convex && tar_is_convex)
406 (sv_adjacency_list_)[*edge_itr].is_valid =
false;
411 template <
typename Po
intT>
void
416 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)
420 std::uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)];
421 std::uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
423 float normal_difference;
424 bool is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference);
425 adjacency_list_arg[*edge_itr].is_convex = is_convex;
426 adjacency_list_arg[*edge_itr].is_valid = is_convex;
427 adjacency_list_arg[*edge_itr].normal_difference = normal_difference;
431 template <
typename Po
intT>
bool
433 const std::uint32_t target_label_arg,
439 const Eigen::Vector3f& source_centroid = sv_source->
centroid_.getVector3fMap ();
440 const Eigen::Vector3f& target_centroid = sv_target->
centroid_.getVector3fMap ();
442 const Eigen::Vector3f& source_normal = sv_source->
normal_.getNormalVector3fMap (). normalized ();
443 const Eigen::Vector3f& target_normal = sv_target->
normal_.getNormalVector3fMap (). normalized ();
446 if (concavity_tolerance_threshold_ < 0)
451 bool is_convex =
true;
452 bool is_smooth =
true;
454 normal_angle =
getAngle3D (source_normal, target_normal,
true);
456 Eigen::Vector3f vec_t_to_s, vec_s_to_t;
458 vec_t_to_s = source_centroid - target_centroid;
459 vec_s_to_t = -vec_t_to_s;
461 Eigen::Vector3f ncross;
462 ncross = source_normal.cross (target_normal);
465 if (use_smoothness_check_)
467 float expected_distance = ncross.norm () * seed_resolution_;
468 float dot_p_1 = vec_t_to_s.dot (source_normal);
469 float dot_p_2 = vec_s_to_t.dot (target_normal);
470 float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2);
471 const float dist_smoothing = smoothness_threshold_ * voxel_resolution_;
473 if (point_dist > (expected_distance + dist_smoothing))
481 float intersection_angle =
getAngle3D (ncross, vec_t_to_s,
true);
482 float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle;
484 float intersect_thresh = 60. * 1. / (1. + std::exp (-0.25 * (normal_angle - 25.)));
485 if (min_intersect_angle < intersect_thresh && use_sanity_check_)
500 is_convex &= (normal_angle < concavity_tolerance_threshold_);
502 return (is_convex && is_smooth);
505 #endif // PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_