39 #ifndef PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_ 40 #define PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_ 42 #include <pcl/segmentation/boost.h> 43 #include <pcl/segmentation/min_cut_segmentation.h> 44 #include <pcl/search/search.h> 45 #include <pcl/search/kdtree.h> 50 template <
typename Po
intT>
52 inverse_sigma_ (16.0),
53 binary_potentials_are_valid_ (false),
56 unary_potentials_are_valid_ (false),
59 number_of_neighbours_ (14),
60 graph_is_valid_ (false),
61 foreground_points_ (0),
62 background_points_ (0),
73 template <
typename Po
intT>
76 foreground_points_.clear ();
77 background_points_.clear ();
80 edge_marker_.clear ();
84 template <
typename Po
intT>
void 88 graph_is_valid_ =
false;
89 unary_potentials_are_valid_ =
false;
90 binary_potentials_are_valid_ =
false;
94 template <
typename Po
intT>
double 97 return (pow (1.0 / inverse_sigma_, 0.5));
101 template <
typename Po
intT>
void 104 if (sigma > epsilon_)
106 inverse_sigma_ = 1.0 / (sigma * sigma);
107 binary_potentials_are_valid_ =
false;
112 template <
typename Po
intT>
double 115 return (pow (radius_, 0.5));
119 template <
typename Po
intT>
void 122 if (radius > epsilon_)
124 radius_ = radius * radius;
125 unary_potentials_are_valid_ =
false;
130 template <
typename Po
intT>
double 133 return (source_weight_);
137 template <
typename Po
intT>
void 140 if (weight > epsilon_)
142 source_weight_ = weight;
143 unary_potentials_are_valid_ =
false;
155 template <
typename Po
intT>
void 162 template <
typename Po
intT>
unsigned int 165 return (number_of_neighbours_);
169 template <
typename Po
intT>
void 172 if (number_of_neighbours_ != neighbour_number && neighbour_number != 0)
174 number_of_neighbours_ = neighbour_number;
175 graph_is_valid_ =
false;
176 unary_potentials_are_valid_ =
false;
177 binary_potentials_are_valid_ =
false;
182 template <
typename Po
intT> std::vector<PointT, Eigen::aligned_allocator<PointT> >
185 return (foreground_points_);
189 template <
typename Po
intT>
void 192 foreground_points_.clear ();
193 foreground_points_.insert(
194 foreground_points_.end(), foreground_points->
cbegin(), foreground_points->
cend());
196 unary_potentials_are_valid_ =
false;
200 template <
typename Po
intT> std::vector<PointT, Eigen::aligned_allocator<PointT> >
203 return (background_points_);
207 template <
typename Po
intT>
void 210 background_points_.clear ();
211 background_points_.insert(
212 background_points_.end(), background_points->
cbegin(), background_points->
cend());
214 unary_potentials_are_valid_ =
false;
218 template <
typename Po
intT>
void 223 bool segmentation_is_possible = initCompute ();
224 if ( !segmentation_is_possible )
230 if ( graph_is_valid_ && unary_potentials_are_valid_ && binary_potentials_are_valid_ )
232 clusters.reserve (clusters_.size ());
233 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
240 if ( !graph_is_valid_ )
242 bool success = buildGraph ();
248 graph_is_valid_ =
true;
249 unary_potentials_are_valid_ =
true;
250 binary_potentials_are_valid_ =
true;
253 if ( !unary_potentials_are_valid_ )
255 bool success = recalculateUnaryPotentials ();
261 unary_potentials_are_valid_ =
true;
264 if ( !binary_potentials_are_valid_ )
266 bool success = recalculateBinaryPotentials ();
272 binary_potentials_are_valid_ =
true;
276 ResidualCapacityMap residual_capacity = boost::get (boost::edge_residual_capacity, *graph_);
278 max_flow_ = boost::boykov_kolmogorov_max_flow (*graph_, source_, sink_);
280 assembleLabels (residual_capacity);
282 clusters.reserve (clusters_.size ());
283 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
289 template <
typename Po
intT>
double 303 template <
typename Po
intT>
bool 306 const auto number_of_points = input_->size ();
307 const auto number_of_indices = indices_->size ();
309 if (input_->points.empty () || number_of_points == 0 || foreground_points_.empty () == true )
315 graph_.reset (
new mGraph);
318 *capacity_ = boost::get (boost::edge_capacity, *graph_);
321 *reverse_edges_ = boost::get (boost::edge_reverse, *graph_);
325 vertices_.resize (number_of_points + 2, vertex_descriptor);
327 std::set<int> out_edges_marker;
328 edge_marker_.clear ();
329 edge_marker_.resize (number_of_points + 2, out_edges_marker);
331 for (std::size_t i_point = 0; i_point < number_of_points + 2; i_point++)
332 vertices_[i_point] = boost::add_vertex (*graph_);
334 source_ = vertices_[number_of_points];
335 sink_ = vertices_[number_of_points + 1];
337 for (std::size_t i_point = 0; i_point < number_of_indices; i_point++)
339 index_t point_index = (*indices_)[i_point];
340 double source_weight = 0.0;
341 double sink_weight = 0.0;
342 calculateUnaryPotential (point_index, source_weight, sink_weight);
343 addEdge (static_cast<int> (source_), point_index, source_weight);
344 addEdge (point_index, static_cast<int> (sink_), sink_weight);
347 std::vector<int> neighbours;
348 std::vector<float> distances;
349 search_->setInputCloud (input_, indices_);
350 for (std::size_t i_point = 0; i_point < number_of_indices; i_point++)
352 index_t point_index = (*indices_)[i_point];
353 search_->nearestKSearch (i_point, number_of_neighbours_, neighbours, distances);
354 for (std::size_t i_nghbr = 1; i_nghbr < neighbours.size (); i_nghbr++)
356 double weight = calculateBinaryPotential (point_index, neighbours[i_nghbr]);
357 addEdge (point_index, neighbours[i_nghbr], weight);
358 addEdge (neighbours[i_nghbr], point_index, weight);
368 template <
typename Po
intT>
void 371 double min_dist_to_foreground = std::numeric_limits<double>::max ();
374 double initial_point[] = {0.0, 0.0};
376 initial_point[0] = (*input_)[point].x;
377 initial_point[1] = (*input_)[point].y;
379 for (std::size_t i_point = 0; i_point < foreground_points_.size (); i_point++)
382 dist += (foreground_points_[i_point].x - initial_point[0]) * (foreground_points_[i_point].x - initial_point[0]);
383 dist += (foreground_points_[i_point].y - initial_point[1]) * (foreground_points_[i_point].y - initial_point[1]);
384 if (min_dist_to_foreground > dist)
386 min_dist_to_foreground = dist;
390 sink_weight = pow (min_dist_to_foreground / radius_, 0.5);
392 source_weight = source_weight_;
424 template <
typename Po
intT>
bool 427 std::set<int>::iterator iter_out = edge_marker_[source].find (target);
428 if ( iter_out != edge_marker_[source].end () )
433 bool edge_was_added, reverse_edge_was_added;
435 boost::tie (edge, edge_was_added) = boost::add_edge ( vertices_[source], vertices_[target], *graph_ );
436 boost::tie (reverse_edge, reverse_edge_was_added) = boost::add_edge ( vertices_[target], vertices_[source], *graph_ );
437 if ( !edge_was_added || !reverse_edge_was_added )
440 (*capacity_)[edge] = weight;
441 (*capacity_)[reverse_edge] = 0.0;
442 (*reverse_edges_)[edge] = reverse_edge;
443 (*reverse_edges_)[reverse_edge] = edge;
444 edge_marker_[source].insert (target);
450 template <
typename Po
intT>
double 455 distance += ((*input_)[source].x - (*input_)[target].x) * ((*input_)[source].x - (*input_)[target].x);
456 distance += ((*input_)[source].y - (*input_)[target].y) * ((*input_)[source].y - (*input_)[target].y);
457 distance += ((*input_)[source].z - (*input_)[target].z) * ((*input_)[source].z - (*input_)[target].z);
465 template <
typename Po
intT>
bool 470 std::pair<EdgeDescriptor, bool> sink_edge;
472 for (boost::tie (src_edge_iter, src_edge_end) = boost::out_edges (source_, *graph_); src_edge_iter != src_edge_end; src_edge_iter++)
474 double source_weight = 0.0;
475 double sink_weight = 0.0;
476 sink_edge.second =
false;
477 calculateUnaryPotential (static_cast<int> (boost::target (*src_edge_iter, *graph_)), source_weight, sink_weight);
478 sink_edge = boost::lookup_edge (boost::target (*src_edge_iter, *graph_), sink_, *graph_);
479 if (!sink_edge.second)
482 (*capacity_)[*src_edge_iter] = source_weight;
483 (*capacity_)[sink_edge.first] = sink_weight;
490 template <
typename Po
intT>
bool 493 int number_of_points =
static_cast<int> (indices_->size ());
500 std::vector< std::set<VertexDescriptor> > edge_marker;
501 std::set<VertexDescriptor> out_edges_marker;
502 edge_marker.clear ();
503 edge_marker.resize (number_of_points + 2, out_edges_marker);
505 for (boost::tie (vertex_iter, vertex_end) = boost::vertices (*graph_); vertex_iter != vertex_end; vertex_iter++)
508 if (source_vertex == source_ || source_vertex == sink_)
510 for (boost::tie (edge_iter, edge_end) = boost::out_edges (source_vertex, *graph_); edge_iter != edge_end; edge_iter++)
514 if ((*capacity_)[reverse_edge] != 0.0)
519 std::set<VertexDescriptor>::iterator iter_out = edge_marker[
static_cast<int> (source_vertex)].find (target_vertex);
520 if ( iter_out != edge_marker[static_cast<int> (source_vertex)].end () )
523 if (target_vertex != source_ && target_vertex != sink_)
526 double weight = calculateBinaryPotential (static_cast<int> (target_vertex), static_cast<int> (source_vertex));
527 (*capacity_)[*edge_iter] = weight;
528 edge_marker[
static_cast<int> (source_vertex)].insert (target_vertex);
537 template <
typename Po
intT>
void 540 std::vector<int> labels;
541 labels.resize (input_->size (), 0);
542 int number_of_indices =
static_cast<int> (indices_->size ());
543 for (
int i_point = 0; i_point < number_of_indices; i_point++)
544 labels[(*indices_)[i_point]] = 1;
549 clusters_.resize (2, segment);
552 for ( boost::tie (edge_iter, edge_end) = boost::out_edges (source_, *graph_); edge_iter != edge_end; edge_iter++ )
554 if (labels[edge_iter->m_target] == 1)
556 if (residual_capacity[*edge_iter] > epsilon_)
557 clusters_[1].
indices.push_back (static_cast<int> (edge_iter->m_target));
559 clusters_[0].indices.push_back (static_cast<int> (edge_iter->m_target));
570 if (!clusters_.empty ())
572 int num_of_pts_in_first_cluster =
static_cast<int> (clusters_[0].indices.size ());
573 int num_of_pts_in_second_cluster =
static_cast<int> (clusters_[1].indices.size ());
574 int number_of_points = num_of_pts_in_first_cluster + num_of_pts_in_second_cluster;
576 unsigned char foreground_color[3] = {255, 255, 255};
577 unsigned char background_color[3] = {255, 0, 0};
578 colored_cloud->
width = number_of_points;
579 colored_cloud->
height = 1;
580 colored_cloud->
is_dense = input_->is_dense;
583 for (
int i_point = 0; i_point < num_of_pts_in_first_cluster; i_point++)
585 index_t point_index = clusters_[0].indices[i_point];
586 point.x = *((*input_)[point_index].data);
587 point.y = *((*input_)[point_index].data + 1);
588 point.z = *((*input_)[point_index].data + 2);
589 point.r = background_color[0];
590 point.g = background_color[1];
591 point.b = background_color[2];
592 colored_cloud->
points.push_back (point);
595 for (
int i_point = 0; i_point < num_of_pts_in_second_cluster; i_point++)
597 index_t point_index = clusters_[1].indices[i_point];
598 point.x = *((*input_)[point_index].data);
599 point.y = *((*input_)[point_index].data + 1);
600 point.z = *((*input_)[point_index].data + 2);
601 point.r = foreground_color[0];
602 point.g = foreground_color[1];
603 point.b = foreground_color[2];
604 colored_cloud->
points.push_back (point);
608 return (colored_cloud);
611 #define PCL_INSTANTIATE_MinCutSegmentation(T) template class pcl::MinCutSegmentation<T>; 613 #endif // PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_ double calculateBinaryPotential(int source, int target) const
Returns the binary potential(smooth cost) for the given indices of points.
typename KdTree::Ptr KdTreePtr
std::vector< PointT, Eigen::aligned_allocator< PointT > > getForegroundPoints() const
Returns the points that must belong to foreground.
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
unsigned int getNumberOfNeighbours() const
Returns the number of neighbours to find.
double getMaxFlow() const
Returns that flow value that was calculated during the segmentation.
void setBackgroundPoints(typename pcl::PointCloud< PointT >::Ptr background_points)
Allows to specify points which are known to be the points of the background.
boost::graph_traits< mGraph >::out_edge_iterator OutEdgeIterator
double getRadius() const
Returns radius to the background.
~MinCutSegmentation()
Destructor that frees memory.
boost::property_map< mGraph, boost::edge_capacity_t >::type CapacityMap
bool recalculateBinaryPotentials()
This method recalculates binary potentials(smooth cost) if some changes were made, instead of creating new graph.
void setForegroundPoints(typename pcl::PointCloud< PointT >::Ptr foreground_points)
Allows to specify points which are known to be the points of the object.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
void setRadius(double radius)
Allows to set the radius to the background.
boost::graph_traits< mGraph >::edge_descriptor EdgeDescriptor
Traits::vertex_descriptor VertexDescriptor
void setInputCloud(const PointCloudConstPtr &cloud) override
This method simply sets the input point cloud.
shared_ptr< mGraph > mGraphPtr
std::uint32_t width
The point cloud width (if organized as an image-structure).
bool recalculateUnaryPotentials()
This method recalculates unary potentials(data cost) if some changes were made, instead of creating n...
std::vector< PointT, Eigen::aligned_allocator< PointT > > getBackgroundPoints() const
Returns the points that must belong to background.
void assembleLabels(ResidualCapacityMap &residual_capacity)
This method analyzes the residual network and assigns a label to every point in the cloud...
void setSourceWeight(double weight)
Allows to set weight for source edges.
void setSearchMethod(const KdTreePtr &tree)
Allows to set search method for finding KNN.
float distance(const PointT &p1, const PointT &p2)
boost::property_map< mGraph, boost::edge_reverse_t >::type ReverseEdgeMap
void setSigma(double sigma)
Allows to set the normalization value for the binary potentials as described in the article...
std::uint32_t height
The point cloud height (if organized as an image-structure).
boost::property_map< mGraph, boost::edge_residual_capacity_t >::type ResidualCapacityMap
const_iterator cbegin() const noexcept
bool addEdge(int source, int target, double weight)
This method simply adds the edge from the source point to the target point with a given weight...
bool buildGraph()
This method simply builds the graph that will be used during the segmentation.
double getSigma() const
Returns normalization value for binary potentials.
MinCutSegmentation()
Constructor that sets default values for member variables.
pcl::PointCloud< pcl::PointXYZRGB >::Ptr getColoredCloud()
Returns the colored cloud.
mGraphPtr getGraph() const
Returns the graph that was build for finding the minimum cut.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
boost::graph_traits< mGraph >::vertex_iterator VertexIterator
void setNumberOfNeighbours(unsigned int neighbour_number)
Allows to set the number of neighbours to find.
void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
A point structure representing Euclidean xyz coordinates, and the RGB color.
boost::adjacency_list< boost::vecS, boost::vecS, boost::directedS, boost::property< boost::vertex_name_t, std::string, boost::property< boost::vertex_index_t, long, boost::property< boost::vertex_color_t, boost::default_color_type, boost::property< boost::vertex_distance_t, long, boost::property< boost::vertex_predecessor_t, Traits::edge_descriptor > > > > >, boost::property< boost::edge_capacity_t, double, boost::property< boost::edge_residual_capacity_t, double, boost::property< boost::edge_reverse_t, Traits::edge_descriptor > > > > mGraph
void calculateUnaryPotential(int point, double &source_weight, double &sink_weight) const
Returns unary potential(data cost) for the given point index.
const_iterator cend() const noexcept
typename PointCloud::ConstPtr PointCloudConstPtr
KdTreePtr getSearchMethod() const
Returns search method that is used for finding KNN.
double getSourceWeight() const
Returns weight that every edge from the source point has.