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);
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.