38#ifndef PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
39#define PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_
41#include <pcl/segmentation/lccp_segmentation.h>
53template <
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),
68template <
typename Po
intT>
71template <
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;
84template <
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");
109template <
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");
134template <
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);
165template <
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;
180 unsigned int nr_filtered = 0;
184 for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr)
186 const std::uint32_t& sv_label = sv_adjacency_list_[*sv_itr];
187 std::uint32_t current_seg_label = sv_label_to_seg_label_map_[sv_label];
188 std::uint32_t largest_neigh_seg_label = current_seg_label;
189 std::uint32_t largest_neigh_size = seg_label_to_sv_list_map_[current_seg_label].size ();
191 const std::uint32_t& nr_neighbors = seg_label_to_neighbor_set_map_[current_seg_label].size ();
192 if (nr_neighbors == 0)
195 if (seg_label_to_sv_list_map_[current_seg_label].size () <= min_segment_size_)
197 continue_filtering =
true;
201 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)
203 if (seg_label_to_sv_list_map_[*neighbors_itr].size () >= largest_neigh_size)
205 largest_neigh_seg_label = *neighbors_itr;
206 largest_neigh_size = seg_label_to_sv_list_map_[*neighbors_itr].size ();
211 if (largest_neigh_seg_label != current_seg_label)
213 if (filteredSegLabels.count (largest_neigh_seg_label) > 0)
216 sv_label_to_seg_label_map_[sv_label] = largest_neigh_seg_label;
217 filteredSegLabels.insert (current_seg_label);
220 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)
222 seg_label_to_sv_list_map_[largest_neigh_seg_label].insert (*sv_ID_itr);
229 for (
const unsigned int &filteredSegLabel : filteredSegLabels)
231 seg_label_to_sv_list_map_.erase (filteredSegLabel);
237 computeSegmentAdjacency ();
241template <
typename Po
intT>
void
243 const std::multimap<std::uint32_t, std::uint32_t>& label_adjaceny_arg)
249 sv_label_to_supervoxel_map_ = supervoxel_clusters_arg;
252 std::map<std::uint32_t, VertexID> label_ID_map;
255 for (
auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
256 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
258 const std::uint32_t& sv_label = svlabel_itr->first;
259 VertexID node_id = boost::add_vertex (sv_adjacency_list_);
260 sv_adjacency_list_[node_id] = sv_label;
261 label_ID_map[sv_label] = node_id;
265 for (
const auto &sv_neighbors_itr : label_adjaceny_arg)
267 const std::uint32_t& sv_label = sv_neighbors_itr.first;
268 const std::uint32_t& neighbor_label = sv_neighbors_itr.second;
270 VertexID u = label_ID_map[sv_label];
271 VertexID v = label_ID_map[neighbor_label];
273 boost::add_edge (u, v, sv_adjacency_list_);
278 seg_label_to_sv_list_map_.clear ();
279 for (
auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
280 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
282 const std::uint32_t& sv_label = svlabel_itr->first;
283 processed_[sv_label] =
false;
284 sv_label_to_seg_label_map_[sv_label] = 0;
291template <
typename Po
intT>
void
295 seg_label_to_sv_list_map_.clear ();
296 for (
auto svlabel_itr = sv_label_to_supervoxel_map_.begin ();
297 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
299 const std::uint32_t& sv_label = svlabel_itr->first;
300 processed_[sv_label] =
false;
301 sv_label_to_seg_label_map_[sv_label] = 0;
308 unsigned int segment_label = 1;
309 for (std::tie(sv_itr, sv_itr_end) = boost::vertices (sv_adjacency_list_); sv_itr != sv_itr_end; ++sv_itr)
311 const VertexID sv_vertex_id = *sv_itr;
312 const std::uint32_t& sv_label = sv_adjacency_list_[sv_vertex_id];
313 if (!processed_[sv_label])
316 recursiveSegmentGrowing (sv_vertex_id, segment_label);
322template <
typename Po
intT>
void
324 const unsigned int segment_label)
326 const std::uint32_t& sv_label = sv_adjacency_list_[query_point_id];
328 processed_[sv_label] =
true;
331 sv_label_to_seg_label_map_[sv_label] = segment_label;
332 seg_label_to_sv_list_map_[segment_label].insert (sv_label);
337 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)
339 const VertexID neighbor_ID = boost::target (*out_Edge_itr, sv_adjacency_list_);
340 const std::uint32_t& neighbor_label = sv_adjacency_list_[neighbor_ID];
342 if (!processed_[neighbor_label])
344 if (sv_adjacency_list_[*out_Edge_itr].is_valid)
346 recursiveSegmentGrowing (neighbor_ID, segment_label);
352template <
typename Po
intT>
void
360 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)
364 bool is_convex = sv_adjacency_list_[*edge_itr].is_convex;
368 unsigned int kcount = 0;
370 const VertexID source = boost::source (*edge_itr, sv_adjacency_list_);
371 const VertexID target = boost::target (*edge_itr, sv_adjacency_list_);
375 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)
377 VertexID source_neighbor_ID = boost::target (*source_neighbors_itr, sv_adjacency_list_);
380 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)
382 VertexID target_neighbor_ID = boost::target (*target_neighbors_itr, sv_adjacency_list_);
383 if (source_neighbor_ID == target_neighbor_ID)
385 EdgeID src_edge = boost::edge (source, source_neighbor_ID, sv_adjacency_list_).first;
386 EdgeID tar_edge = boost::edge (target, source_neighbor_ID, sv_adjacency_list_).first;
388 bool src_is_convex = (sv_adjacency_list_)[src_edge].is_convex;
389 bool tar_is_convex = (sv_adjacency_list_)[tar_edge].is_convex;
391 if (src_is_convex && tar_is_convex)
404 (sv_adjacency_list_)[*edge_itr].is_valid =
false;
409template <
typename Po
intT>
void
414 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)
418 std::uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)];
419 std::uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
421 float normal_difference;
422 bool is_convex = connIsConvex (source_sv_label, target_sv_label, normal_difference);
423 adjacency_list_arg[*edge_itr].is_convex = is_convex;
424 adjacency_list_arg[*edge_itr].is_valid = is_convex;
425 adjacency_list_arg[*edge_itr].normal_difference = normal_difference;
429template <
typename Po
intT>
bool
431 const std::uint32_t target_label_arg,
437 const Eigen::Vector3f& source_centroid = sv_source->
centroid_.getVector3fMap ();
438 const Eigen::Vector3f& target_centroid = sv_target->
centroid_.getVector3fMap ();
440 const Eigen::Vector3f& source_normal = sv_source->
normal_.getNormalVector3fMap (). normalized ();
441 const Eigen::Vector3f& target_normal = sv_target->
normal_.getNormalVector3fMap (). normalized ();
444 if (concavity_tolerance_threshold_ < 0)
449 bool is_convex =
true;
450 bool is_smooth =
true;
452 normal_angle =
getAngle3D (source_normal, target_normal,
true);
454 Eigen::Vector3f vec_t_to_s, vec_s_to_t;
456 vec_t_to_s = source_centroid - target_centroid;
457 vec_s_to_t = -vec_t_to_s;
459 Eigen::Vector3f ncross;
460 ncross = source_normal.cross (target_normal);
463 if (use_smoothness_check_)
465 float expected_distance = ncross.norm () * seed_resolution_;
466 float dot_p_1 = vec_t_to_s.dot (source_normal);
467 float dot_p_2 = vec_s_to_t.dot (target_normal);
468 float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2);
469 const float dist_smoothing = smoothness_threshold_ * voxel_resolution_;
471 if (point_dist > (expected_distance + dist_smoothing))
479 float intersection_angle =
getAngle3D (ncross, vec_t_to_s,
true);
480 float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle;
482 float intersect_thresh = 60. * 1. / (1. + std::exp (-0.25 * (normal_angle - 25.)));
483 if (min_intersect_angle < intersect_thresh && use_sanity_check_)
498 is_convex &= (normal_angle < concavity_tolerance_threshold_);
500 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.