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>;
KdTreePtr getSearchMethod() const
Returns search method that is used for finding KNN.
~MinCutSegmentation()
Destructor that frees memory.
void calculateUnaryPotential(int point, double &source_weight, double &sink_weight) const
Returns unary potential(data cost) for the given point index.
void setSigma(double sigma)
Allows to set the normalization value for the binary potentials as described in the article.
double getSigma() const
Returns normalization value for binary potentials.
MinCutSegmentation()
Constructor that sets default values for member variables.
void setRadius(double radius)
Allows to set the radius to the background.
void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
double getSourceWeight() const
Returns weight that every edge from the source point has.
mGraphPtr getGraph() const
Returns the graph that was build for finding the minimum cut.
void setInputCloud(const PointCloudConstPtr &cloud) override
This method simply sets the input point cloud.
void setSourceWeight(double weight)
Allows to set weight for source edges.
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
unsigned int getNumberOfNeighbours() const
Returns the number of neighbours to find.
bool buildGraph()
This method simply builds the graph that will be used during the segmentation.
boost::property_map< mGraph, boost::edge_capacity_t >::type CapacityMap
double getMaxFlow() const
Returns that flow value that was calculated during the segmentation.
bool recalculateUnaryPotentials()
This method recalculates unary potentials(data cost) if some changes were made, instead of creating n...
boost::property_map< mGraph, boost::edge_reverse_t >::type ReverseEdgeMap
shared_ptr< mGraph > mGraphPtr
void setSearchMethod(const KdTreePtr &tree)
Allows to set search method for finding KNN.
double getRadius() const
Returns radius to the background.
double calculateBinaryPotential(int source, int target) const
Returns the binary potential(smooth cost) for the given indices of points.
bool recalculateBinaryPotentials()
This method recalculates binary potentials(smooth cost) if some changes were made,...
std::vector< PointT, Eigen::aligned_allocator< PointT > > getBackgroundPoints() const
Returns the points that must belong to background.
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.
Traits::vertex_descriptor VertexDescriptor
void setNumberOfNeighbours(unsigned int neighbour_number)
Allows to set the number of neighbours to find.
boost::graph_traits< mGraph >::vertex_iterator VertexIterator
typename KdTree::Ptr KdTreePtr
std::vector< PointT, Eigen::aligned_allocator< PointT > > getForegroundPoints() const
Returns the points that must belong to foreground.
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
boost::graph_traits< mGraph >::edge_descriptor EdgeDescriptor
boost::property_map< mGraph, boost::edge_residual_capacity_t >::type ResidualCapacityMap
void setForegroundPoints(typename pcl::PointCloud< PointT >::Ptr foreground_points)
Allows to specify points which are known to be the points of the object.
pcl::PointCloud< pcl::PointXYZRGB >::Ptr getColoredCloud()
Returns the colored cloud.
void assembleLabels(ResidualCapacityMap &residual_capacity)
This method analyzes the residual network and assigns a label to every point in the cloud.
typename PointCloud::ConstPtr PointCloudConstPtr
const_iterator cbegin() const noexcept
const_iterator cend() const noexcept
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
std::uint32_t width
The point cloud width (if organized as an image-structure).
std::uint32_t height
The point cloud height (if organized as an image-structure).
shared_ptr< PointCloud< PointT > > Ptr
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
float distance(const PointT &p1, const PointT &p2)
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.