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 ();
147 for(std::tie(sv_itr, sv_itr_end) = boost::vertices(sv_adjacency_list_); sv_itr != sv_itr_end; ++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)
189 std::uint32_t current_seg_label = sv_label_to_seg_label_map_[sv_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;
258 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
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)
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 ();
282 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
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 ();
299 svlabel_itr != sv_label_to_supervoxel_map_.end (); ++svlabel_itr)
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 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_ typename boost::graph_traits< SupervoxelAdjacencyList >::out_edge_iterator OutEdgeIterator
void doGrouping()
Perform depth search on the graph and recursively group all supervoxels with convex connections...
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.
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. ...
pcl::PointXYZRGBA centroid_
The centroid of the supervoxel - average voxel.
pcl::Normal normal_
The normal calculated for the voxels contained in the supervoxel.
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.
typename boost::graph_traits< SupervoxelAdjacencyList >::edge_iterator EdgeIterator
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.
Define standard C methods and C++ classes that are common to all methods.
typename boost::graph_traits< SupervoxelAdjacencyList >::edge_descriptor EdgeID
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, std::uint32_t, EdgeProperties > SupervoxelAdjacencyList
virtual ~LCCPSegmentation()
void reset()
Reset internal memory.
typename boost::graph_traits< SupervoxelAdjacencyList >::vertex_descriptor VertexID
typename boost::graph_traits< SupervoxelAdjacencyList >::vertex_iterator VertexIterator
shared_ptr< Supervoxel< PointT > > Ptr
void mergeSmallSegments()
Segments smaller than min_segment_size_ are merged to the label of largest neighbor.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
void segment()
Merge supervoxels using local convexity.
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 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 >::adjacency_iterator AdjacencyIterator