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 86 template <
typename Po
intT>
void 107 PCL_WARN (
"[pcl::LCCPSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
111 template <
typename Po
intT>
void 118 for (; voxel_itr != labeled_cloud_arg.
end (); ++voxel_itr)
125 PCL_WARN (
"[pcl::LCCPSegmentation::relabelCloud] WARNING: Call function segment first. Nothing has been done. \n");
137 template <
typename Po
intT>
void 143 std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
146 uint32_t current_segLabel;
147 uint32_t neigh_segLabel;
150 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
156 std::pair<AdjacencyIterator, AdjacencyIterator> neighbors = boost::adjacent_vertices (*sv_itr,
sv_adjacency_list_);
157 for (
AdjacencyIterator itr_neighbor = neighbors.first; itr_neighbor != neighbors.second; ++itr_neighbor)
162 if (current_segLabel != neigh_segLabel)
170 template <
typename Po
intT>
void 178 std::set<uint32_t> filteredSegLabels;
180 uint32_t largest_neigh_size = 0;
181 uint32_t largest_neigh_seg_label = 0;
182 uint32_t current_seg_label;
184 std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
187 bool continue_filtering =
true;
189 while (continue_filtering)
191 continue_filtering =
false;
192 unsigned int nr_filtered = 0;
195 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
199 largest_neigh_seg_label = current_seg_label;
203 if (nr_neighbors == 0)
208 continue_filtering =
true;
217 largest_neigh_seg_label = *neighbors_itr;
223 if (largest_neigh_seg_label != current_seg_label)
225 if (filteredSegLabels.count (largest_neigh_seg_label) > 0)
229 filteredSegLabels.insert (current_seg_label);
243 std::set<uint32_t>::iterator filtered_ID_itr = filteredSegLabels.begin ();
244 for (; filtered_ID_itr != filteredSegLabels.end (); ++filtered_ID_itr)
256 template <
typename Po
intT>
void 258 const std::multimap<uint32_t, uint32_t>& label_adjaceny_arg)
267 std::map<uint32_t, VertexID> label_ID_map;
273 const uint32_t& sv_label = svlabel_itr->first;
276 label_ID_map[sv_label] = node_id;
280 for (std::multimap<uint32_t, uint32_t>::const_iterator sv_neighbors_itr = label_adjaceny_arg.begin (); sv_neighbors_itr != label_adjaceny_arg.end ();
283 const uint32_t& sv_label = sv_neighbors_itr->first;
284 const uint32_t& neighbor_label = sv_neighbors_itr->second;
286 VertexID u = label_ID_map[sv_label];
287 VertexID v = label_ID_map[neighbor_label];
298 const uint32_t& sv_label = svlabel_itr->first;
307 template <
typename Po
intT>
void 315 const uint32_t& sv_label = svlabel_itr->first;
322 std::pair< VertexIterator, VertexIterator> vertex_iterator_range;
326 unsigned int segment_label = 1;
327 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
329 const VertexID sv_vertex_id = *sv_itr;
340 template <
typename Po
intT>
void 342 const unsigned int segment_label)
353 std::pair<OutEdgeIterator, OutEdgeIterator> out_edge_iterator_range;
355 for (
OutEdgeIterator out_Edge_itr = out_edge_iterator_range.first; out_Edge_itr != out_edge_iterator_range.second; ++out_Edge_itr)
370 template <
typename Po
intT>
void 377 unsigned int kcount = 0;
382 std::pair<OutEdgeIterator, OutEdgeIterator> source_neighbors_range;
383 std::pair<OutEdgeIterator, OutEdgeIterator> target_neighbors_range;
386 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
403 for (
OutEdgeIterator source_neighbors_itr = source_neighbors_range.first; source_neighbors_itr != source_neighbors_range.second; ++source_neighbors_itr)
407 for (
OutEdgeIterator target_neighbors_itr = target_neighbors_range.first; target_neighbors_itr != target_neighbors_range.second; ++target_neighbors_itr)
410 if (source_neighbor_ID == target_neighbor_ID)
418 if (src_is_convex && tar_is_convex)
436 template <
typename Po
intT>
void 442 boost::tie (edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg);
444 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
448 uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)];
449 uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
451 float normal_difference;
452 is_convex =
connIsConvex (source_sv_label, target_sv_label, normal_difference);
453 adjacency_list_arg[*edge_itr].is_convex = is_convex;
454 adjacency_list_arg[*edge_itr].is_valid = is_convex;
455 adjacency_list_arg[*edge_itr].normal_difference = normal_difference;
459 template <
typename Po
intT>
bool 461 const uint32_t target_label_arg,
467 const Eigen::Vector3f& source_centroid = sv_source->
centroid_.getVector3fMap ();
468 const Eigen::Vector3f& target_centroid = sv_target->
centroid_.getVector3fMap ();
470 const Eigen::Vector3f& source_normal = sv_source->
normal_.getNormalVector3fMap (). normalized ();
471 const Eigen::Vector3f& target_normal = sv_target->
normal_.getNormalVector3fMap (). normalized ();
479 bool is_convex =
true;
480 bool is_smooth =
true;
482 normal_angle =
getAngle3D (source_normal, target_normal,
true);
484 Eigen::Vector3f vec_t_to_s, vec_s_to_t;
486 vec_t_to_s = source_centroid - target_centroid;
487 vec_s_to_t = -vec_t_to_s;
489 Eigen::Vector3f ncross;
490 ncross = source_normal.cross (target_normal);
496 float dot_p_1 = vec_t_to_s.dot (source_normal);
497 float dot_p_2 = vec_s_to_t.dot (target_normal);
498 float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2);
501 if (point_dist > (expected_distance + dist_smoothing))
509 float intersection_angle =
getAngle3D (ncross, vec_t_to_s,
true);
510 float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle;
512 float intersect_thresh = 60. * 1. / (1. + exp (-0.25 * (normal_angle - 25.)));
530 return (is_convex && is_smooth);
533 #endif // PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_ std::map< uint32_t, bool > processed_
Stores which supervoxel labels were already visited during recursive grouping.
bool use_smoothness_check_
Determines if the smoothness check is used during segmentation.
std::map< uint32_t, std::set< uint32_t > > seg_label_to_neighbor_set_map_
map < SegmentID, std::set< Neighboring segment labels> >
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. ...
bool use_sanity_check_
Determines if we use the sanity check which tries to find and invalidate singular connected patches...
boost::graph_traits< SupervoxelAdjacencyList >::vertex_iterator VertexIterator
pcl::PointXYZRGBA centroid_
The centroid of the supervoxel - average voxel.
pcl::Normal normal_
The normal calculated for the voxels contained in the supervoxel.
float voxel_resolution_
Voxel resolution used to build the supervoxels (used only for smoothness check)
uint32_t min_segment_size_
Minimum segment size.
VectorType::iterator iterator
uint32_t k_factor_
Factor used for k-convexity.
std::map< uint32_t, std::set< uint32_t > > seg_label_to_sv_list_map_
map <Segment Label, std::set <SuperVoxel labels>=""> >
Define standard C methods and C++ classes that are common to all methods.
std::map< uint32_t, typename pcl::Supervoxel< PointT >::Ptr > sv_label_to_supervoxel_map_
map from the supervoxel labels to the supervoxel objects
void prepareSegmentation(const std::map< uint32_t, typename pcl::Supervoxel< PointT >::Ptr > &supervoxel_clusters_arg, const std::multimap< uint32_t, uint32_t > &label_adjacency_arg)
Is called within setInputSupervoxels mainly to reserve required memory.
virtual ~LCCPSegmentation()
float seed_resolution_
Seed resolution of the supervoxels (used only for smoothness check)
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, uint32_t, EdgeProperties > SupervoxelAdjacencyList
boost::graph_traits< SupervoxelAdjacencyList >::edge_iterator EdgeIterator
float smoothness_threshold_
Two supervoxels are unsmooth if their plane-to-plane distance DIST > (expected_distance + smoothness_...
void reset()
Reset internal memory.
boost::graph_traits< SupervoxelAdjacencyList >::adjacency_iterator AdjacencyIterator
bool supervoxels_set_
Marks if supervoxels have been set by calling setInputSupervoxels.
boost::graph_traits< SupervoxelAdjacencyList >::vertex_descriptor VertexID
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. ...
boost::graph_traits< SupervoxelAdjacencyList >::edge_descriptor EdgeID
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.
boost::graph_traits< SupervoxelAdjacencyList >::out_edge_iterator OutEdgeIterator
std::map< uint32_t, uint32_t > sv_label_to_seg_label_map_
Storing relation between original SuperVoxel Labels and new segmantion labels.
boost::shared_ptr< Supervoxel< PointT > > Ptr
float concavity_tolerance_threshold_
*** Parameters *** ///
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...
bool grouping_data_valid_
Marks if valid grouping data (sv_adjacency_list_, sv_label_to_seg_label_map_, processed_) is avaiable...
SupervoxelAdjacencyList sv_adjacency_list_
Adjacency graph with the supervoxel labels as nodes and edges between adjacent supervoxels.
bool connIsConvex(const uint32_t source_label_arg, const uint32_t target_label_arg, float &normal_angle)
Returns true if the connection between source and target is convex.