38 #ifndef PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_ 39 #define PCL_SEGMENTATION_IMPL_LCCP_SEGMENTATION_HPP_ 41 #include <pcl/segmentation/lccp_segmentation.h> 52 template <
typename Po
intT>
54 concavity_tolerance_threshold_ (10),
55 grouping_data_valid_ (false),
56 supervoxels_set_ (false),
57 use_smoothness_check_ (false),
58 smoothness_threshold_ (0.1),
59 use_sanity_check_ (false),
61 voxel_resolution_ (0),
67 template <
typename Po
intT>
72 template <
typename Po
intT>
void 85 template <
typename Po
intT>
void 106 PCL_WARN (
"[pcl::LCCPSegmentation::segment] WARNING: Call function setInputSupervoxels first. Nothing has been done. \n");
110 template <
typename Po
intT>
void 117 for (; voxel_itr != labeled_cloud_arg.
end (); ++voxel_itr)
124 PCL_WARN (
"[pcl::LCCPSegmentation::relabelCloud] WARNING: Call function segment first. Nothing has been done. \n");
136 template <
typename Po
intT>
void 142 std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
145 uint32_t current_segLabel;
146 uint32_t neigh_segLabel;
149 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
155 std::pair<AdjacencyIterator, AdjacencyIterator> neighbors = boost::adjacent_vertices (*sv_itr,
sv_adjacency_list_);
156 for (
AdjacencyIterator itr_neighbor = neighbors.first; itr_neighbor != neighbors.second; ++itr_neighbor)
161 if (current_segLabel != neigh_segLabel)
169 template <
typename Po
intT>
void 177 std::set<uint32_t> filteredSegLabels;
179 uint32_t largest_neigh_size = 0;
180 uint32_t largest_neigh_seg_label = 0;
181 uint32_t current_seg_label;
183 std::pair<VertexIterator, VertexIterator> vertex_iterator_range;
186 bool continue_filtering =
true;
188 while (continue_filtering)
190 continue_filtering =
false;
191 unsigned int nr_filtered = 0;
194 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
198 largest_neigh_seg_label = current_seg_label;
202 if (nr_neighbors == 0)
207 continue_filtering =
true;
216 largest_neigh_seg_label = *neighbors_itr;
222 if (largest_neigh_seg_label != current_seg_label)
224 if (filteredSegLabels.count (largest_neigh_seg_label) > 0)
228 filteredSegLabels.insert (current_seg_label);
242 std::set<uint32_t>::iterator filtered_ID_itr = filteredSegLabels.begin ();
243 for (; filtered_ID_itr != filteredSegLabels.end (); ++filtered_ID_itr)
255 template <
typename Po
intT>
void 257 const std::multimap<uint32_t, uint32_t>& label_adjaceny_arg)
266 std::map<uint32_t, VertexID> label_ID_map;
272 const uint32_t& sv_label = svlabel_itr->first;
275 label_ID_map[sv_label] = node_id;
279 for (std::multimap<uint32_t, uint32_t>::const_iterator sv_neighbors_itr = label_adjaceny_arg.begin (); sv_neighbors_itr != label_adjaceny_arg.end ();
282 const uint32_t& sv_label = sv_neighbors_itr->first;
283 const uint32_t& neighbor_label = sv_neighbors_itr->second;
285 VertexID u = label_ID_map[sv_label];
286 VertexID v = label_ID_map[neighbor_label];
297 const uint32_t& sv_label = svlabel_itr->first;
306 template <
typename Po
intT>
void 314 const uint32_t& sv_label = svlabel_itr->first;
321 std::pair< VertexIterator, VertexIterator> vertex_iterator_range;
325 unsigned int segment_label = 1;
326 for (
VertexIterator sv_itr = vertex_iterator_range.first; sv_itr != vertex_iterator_range.second; ++sv_itr)
328 const VertexID sv_vertex_id = *sv_itr;
339 template <
typename Po
intT>
void 341 const unsigned int segment_label)
352 std::pair<OutEdgeIterator, OutEdgeIterator> out_edge_iterator_range;
354 for (
OutEdgeIterator out_Edge_itr = out_edge_iterator_range.first; out_Edge_itr != out_edge_iterator_range.second; ++out_Edge_itr)
369 template <
typename Po
intT>
void 376 unsigned int kcount = 0;
381 std::pair<OutEdgeIterator, OutEdgeIterator> source_neighbors_range;
382 std::pair<OutEdgeIterator, OutEdgeIterator> target_neighbors_range;
385 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
402 for (
OutEdgeIterator source_neighbors_itr = source_neighbors_range.first; source_neighbors_itr != source_neighbors_range.second; ++source_neighbors_itr)
406 for (
OutEdgeIterator target_neighbors_itr = target_neighbors_range.first; target_neighbors_itr != target_neighbors_range.second; ++target_neighbors_itr)
409 if (source_neighbor_ID == target_neighbor_ID)
417 if (src_is_convex && tar_is_convex)
435 template <
typename Po
intT>
void 441 boost::tie (edge_itr, edge_itr_end) = boost::edges (adjacency_list_arg);
443 for (next_edge = edge_itr; edge_itr != edge_itr_end; edge_itr = next_edge)
447 uint32_t source_sv_label = adjacency_list_arg[boost::source (*edge_itr, adjacency_list_arg)];
448 uint32_t target_sv_label = adjacency_list_arg[boost::target (*edge_itr, adjacency_list_arg)];
450 float normal_difference;
451 is_convex =
connIsConvex (source_sv_label, target_sv_label, normal_difference);
452 adjacency_list_arg[*edge_itr].is_convex = is_convex;
453 adjacency_list_arg[*edge_itr].is_valid = is_convex;
454 adjacency_list_arg[*edge_itr].normal_difference = normal_difference;
458 template <
typename Po
intT>
bool 460 const uint32_t target_label_arg,
466 const Eigen::Vector3f& source_centroid = sv_source->
centroid_.getVector3fMap ();
467 const Eigen::Vector3f& target_centroid = sv_target->
centroid_.getVector3fMap ();
469 const Eigen::Vector3f& source_normal = sv_source->
normal_.getNormalVector3fMap (). normalized ();
470 const Eigen::Vector3f& target_normal = sv_target->
normal_.getNormalVector3fMap (). normalized ();
478 bool is_convex =
true;
479 bool is_smooth =
true;
481 normal_angle =
getAngle3D (source_normal, target_normal,
true);
483 Eigen::Vector3f vec_t_to_s, vec_s_to_t;
485 vec_t_to_s = source_centroid - target_centroid;
486 vec_s_to_t = -vec_t_to_s;
488 Eigen::Vector3f ncross;
489 ncross = source_normal.cross (target_normal);
495 float dot_p_1 = vec_t_to_s.dot (source_normal);
496 float dot_p_2 = vec_s_to_t.dot (target_normal);
497 float point_dist = (std::fabs (dot_p_1) < std::fabs (dot_p_2)) ? std::fabs (dot_p_1) : std::fabs (dot_p_2);
500 if (point_dist > (expected_distance + dist_smoothing))
508 float intersection_angle =
getAngle3D (ncross, vec_t_to_s,
true);
509 float min_intersect_angle = (intersection_angle < 90.) ? intersection_angle : 180. - intersection_angle;
511 float intersect_thresh = 60. * 1. / (1. + exp (-0.25 * (normal_angle - 25.)));
529 return (is_convex && is_smooth);
532 #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>=""> >
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.