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>
71 template <
typename Po
intT>
void
74 sv_adjacency_list_.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;
84 template <
typename Po
intT>
void
91 calculateConvexConnections (sv_adjacency_list_);
94 applyKconvexity (k_factor_);
99 grouping_data_valid_ =
true;
102 mergeSmallSegments ();
105 PCL_WARN (
"[pcl::LCCPSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
109 template <
typename Po
intT>
void
112 if (grouping_data_valid_)
115 for (
auto &voxel : labeled_cloud_arg)
117 voxel.label = sv_label_to_seg_label_map_[voxel.label];
122 PCL_WARN (
"[pcl::LCCPSegmentation::relabelCloud] WARNING: Call function segment first. Nothing has been done. \n");
134 template <
typename Po
intT>
void
137 seg_label_to_neighbor_set_map_.clear ();
139 std::uint32_t current_segLabel;
140 std::uint32_t neigh_segLabel;
145 for(std::tie(sv_itr, sv_itr_end) = boost::vertices(sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr)
147 const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
148 current_segLabel = sv_label_to_seg_label_map_[sv_label];
152 for (std::tie(itr_neighbor, itr_neighbor_end) = boost::adjacent_vertices (*sv_itr, sv_adjacency_list_); itr_neighbor != itr_neighbor_end; ++itr_neighbor)
154 const std::uint32_t& neigh_label = sv_adjacency_list_[*itr_neighbor];
155 neigh_segLabel = sv_label_to_seg_label_map_[neigh_label];
157 if (current_segLabel != neigh_segLabel)
159 seg_label_to_neighbor_set_map_[current_segLabel].insert (neigh_segLabel);
165 template <
typename Po
intT>
void
168 if (min_segment_size_ == 0)
171 computeSegmentAdjacency ();
173 std::set<std::uint32_t> filteredSegLabels;
175 bool continue_filtering =
true;
177 while (continue_filtering)
179 continue_filtering =
false;
183 for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr)
185 const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
186 std::uint32_t current_seg_label = sv_label_to_seg_label_map_[sv_label];
187 std::uint32_t largest_neigh_seg_label = current_seg_label;
188 std::uint32_t largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size ();
190 const std::uint32_t& nr_neighbors = seg_label_to_neighbor_set_map_[current_seg_label].size ();
191 if (nr_neighbors == 0)
194 if (seg_label_to_sv_list_map_[current_seg_label].size () <= min_segment_size_)
196 continue_filtering =
true;
199 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)
201 if (seg_label_to_sv_list_map_[*neighbors_itr].size () >= largest_neigh_size)
203 largest_neigh_seg_label = *neighbors_itr;
204 largest_neigh_size = seg_label_to_sv_list_map_[*neighbors_itr].size ();
209 if (largest_neigh_seg_label != current_seg_label)
211 if (filteredSegLabels.count (largest_neigh_seg_label) > 0)
214 sv_label_to_seg_label_map_[sv_label] = largest_neigh_seg_label;
215 filteredSegLabels.insert (current_seg_label);
218 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)
220 seg_label_to_sv_list_map_[largest_neigh_seg_label].insert (*sv_ID_itr);
227 for (
const unsigned int &filteredSegLabel : filteredSegLabels)
229 seg_label_to_sv_list_map_.erase (filteredSegLabel);
235 computeSegmentAdjacency ();
239 template <
typename Po
intT>
void
241 const std::multimap<std::uint32_t, std::uint32_t>& label_adjaceny_arg)
247 sv_label_to_supervoxel_map_ = supervoxel_clusters_arg;
250 std::map<std::uint32_t, VertexID> label_ID_map;
253 for (
auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
254 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
256 const std::uint32_t& sv_label = svlabel_itr->first;
257 VertexID node_id = boost::add_vertex (sv_adjacency_list_);
258 sv_adjacency_list_[node_id] = sv_label;
259 label_ID_map[sv_label] = node_id;
263 for (
const auto &sv_neighbors_itr : label_adjaceny_arg)
265 const std::uint32_t& sv_label = sv_neighbors_itr.first;
266 const std::uint32_t& neighbor_label = sv_neighbors_itr.second;
268 VertexID u = label_ID_map[sv_label];
269 VertexID v = label_ID_map[neighbor_label];
271 boost::add_edge (u, v, sv_adjacency_list_);
276 seg_label_to_sv_list_map_.clear ();
277 for (
auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
278 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
280 const std::uint32_t& sv_label = svlabel_itr->first;
281 processed_[sv_label] =
false;
282 sv_label_to_seg_label_map_[sv_label] = 0;
289 template <
typename Po
intT>
void
293 seg_label_to_sv_list_map_.clear ();
294 for (
auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
295 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
297 const std::uint32_t& sv_label = svlabel_itr->first;
298 processed_[sv_label] =
false;
299 sv_label_to_seg_label_map_[sv_label] = 0;
306 unsigned int segment_label = 1;
307 for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr)
309 const VertexID sv_vertex_id = *sv_itr;
310 const std::uint32_t& sv_label = sv_adjacency_list_[sv_vertex_id];
311 if (!processed_[sv_label])
314 recursiveSegmentGrowing (sv_vertex_id, segment_label);
320 template <
typename Po
intT>
void
322 const unsigned int segment_label)
324 const std::uint32_t& sv_label = sv_adjacency_list_[query_point_id];
326 processed_[sv_label] =
true;
329 sv_label_to_seg_label_map_[sv_label] = segment_label;
330 seg_label_to_sv_list_map_[segment_label].insert (sv_label);
335 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)
337 const VertexID neighbor_ID = boost::target (*out_Edge_itr, sv_adjacency_list_);
338 const std::uint32_t& neighbor_label = sv_adjacency_list_[neighbor_ID];
340 if (!processed_[neighbor_label])
342 if (sv_adjacency_list_[*out_Edge_itr].is_valid)
344 recursiveSegmentGrowing (neighbor_ID, segment_label);
350 template <
typename Po
intT>
void
358 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)
362 bool is_convex = sv_adjacency_list_[*edge_itr].is_convex;
366 unsigned int kcount = 0;
368 const VertexID source = boost::source (*edge_itr, sv_adjacency_list_);
369 const VertexID target = boost::target (*edge_itr, sv_adjacency_list_);
373 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)
375 VertexID source_neighbor_ID = boost::target (*source_neighbors_itr, sv_adjacency_list_);
378 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)
380 VertexID target_neighbor_ID = boost::target (*target_neighbors_itr, sv_adjacency_list_);
381 if (source_neighbor_ID == target_neighbor_ID)
383 EdgeID src_edge = boost::edge (source, source_neighbor_ID, sv_adjacency_list_).first;
384 EdgeID tar_edge = boost::edge (target, source_neighbor_ID, sv_adjacency_list_).first;
386 bool src_is_convex = (sv_adjacency_list_)[src_edge].is_convex;
387 bool tar_is_convex = (sv_adjacency_list_)[tar_edge].is_convex;
389 if (src_is_convex && tar_is_convex)
402 (sv_adjacency_list_)[*edge_itr].is_valid =
false;
407 template <
typename Po
intT>
void
412 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)
416 std::uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)];
417 std::uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
419 float normal_difference;
420 bool is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference);
421 adjacency_list_arg[*edge_itr].is_convex = is_convex;
422 adjacency_list_arg[*edge_itr].is_valid = is_convex;
423 adjacency_list_arg[*edge_itr].normal_difference = normal_difference;
427 template <
typename Po
intT>
bool
429 const std::uint32_t target_label_arg,
435 const Eigen::Vector3f& source_centroid = sv_source->
centroid_.getVector3fMap ();
436 const Eigen::Vector3f& target_centroid = sv_target->
centroid_.getVector3fMap ();
438 const Eigen::Vector3f& source_normal = sv_source->
normal_.getNormalVector3fMap (). normalized ();
439 const Eigen::Vector3f& target_normal = sv_target->
normal_.getNormalVector3fMap (). normalized ();
442 if (concavity_tolerance_threshold_ < 0)
447 bool is_convex =
true;
448 bool is_smooth =
true;
450 normal_angle =
getAngle3D (source_normal, target_normal,
true);
452 Eigen::Vector3f vec_t_to_s, vec_s_to_t;
454 vec_t_to_s = source_centroid - target_centroid;
455 vec_s_to_t = -vec_t_to_s;
457 Eigen::Vector3f ncross;
458 ncross = source_normal.cross (target_normal);
461 if (use_smoothness_check_)
463 float expected_distance = ncross.norm () * seed_resolution_;
464 float dot_p_1 = vec_t_to_s.dot (source_normal);
465 float dot_p_2 = vec_s_to_t.dot (target_normal);
466 float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2);
467 const float dist_smoothing = smoothness_threshold_ * voxel_resolution_;
469 if (point_dist > (expected_distance + dist_smoothing))
477 float intersection_angle =
getAngle3D (ncross, vec_t_to_s,
true);
478 float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle;
480 float intersect_thresh = 60. * 1. / (1. + std::exp (-0.25 * (normal_angle - 25.)));
481 if (min_intersect_angle < intersect_thresh && use_sanity_check_)
496 is_convex &= (normal_angle < concavity_tolerance_threshold_);
498 return (is_convex && is_smooth);
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.