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_.reserve (foreground_points->
points.size ());
194 for (std::size_t i_point = 0; i_point < foreground_points->
points.size (); i_point++)
195 foreground_points_.push_back (foreground_points->
points[i_point]);
197 unary_potentials_are_valid_ =
false;
201 template <
typename Po
intT> std::vector<PointT, Eigen::aligned_allocator<PointT> >
204 return (background_points_);
208 template <
typename Po
intT>
void
211 background_points_.clear ();
212 background_points_.reserve (background_points->
points.size ());
213 for (std::size_t i_point = 0; i_point < background_points->
points.size (); i_point++)
214 background_points_.push_back (background_points->
points[i_point]);
216 unary_potentials_are_valid_ =
false;
220 template <
typename Po
intT>
void
225 bool segmentation_is_possible = initCompute ();
226 if ( !segmentation_is_possible )
232 if ( graph_is_valid_ && unary_potentials_are_valid_ && binary_potentials_are_valid_ )
234 clusters.reserve (clusters_.size ());
235 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
242 if ( !graph_is_valid_ )
244 bool success = buildGraph ();
250 graph_is_valid_ =
true;
251 unary_potentials_are_valid_ =
true;
252 binary_potentials_are_valid_ =
true;
255 if ( !unary_potentials_are_valid_ )
257 bool success = recalculateUnaryPotentials ();
263 unary_potentials_are_valid_ =
true;
266 if ( !binary_potentials_are_valid_ )
268 bool success = recalculateBinaryPotentials ();
274 binary_potentials_are_valid_ =
true;
278 ResidualCapacityMap residual_capacity = boost::get (boost::edge_residual_capacity, *graph_);
280 max_flow_ = boost::boykov_kolmogorov_max_flow (*graph_, source_, sink_);
282 assembleLabels (residual_capacity);
284 clusters.reserve (clusters_.size ());
285 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
291 template <
typename Po
intT>
double
305 template <
typename Po
intT>
bool
308 int number_of_points =
static_cast<int> (input_->points.size ());
309 int number_of_indices =
static_cast<int> (indices_->size ());
311 if (input_->points.empty () || number_of_points == 0 || foreground_points_.empty () == true )
317 graph_.reset (
new mGraph);
320 *capacity_ = boost::get (boost::edge_capacity, *graph_);
323 *reverse_edges_ = boost::get (boost::edge_reverse, *graph_);
327 vertices_.resize (number_of_points + 2, vertex_descriptor);
329 std::set<int> out_edges_marker;
330 edge_marker_.clear ();
331 edge_marker_.resize (number_of_points + 2, out_edges_marker);
333 for (
int i_point = 0; i_point < number_of_points + 2; i_point++)
334 vertices_[i_point] = boost::add_vertex (*graph_);
336 source_ = vertices_[number_of_points];
337 sink_ = vertices_[number_of_points + 1];
339 for (
int i_point = 0; i_point < number_of_indices; i_point++)
341 index_t point_index = (*indices_)[i_point];
342 double source_weight = 0.0;
343 double sink_weight = 0.0;
344 calculateUnaryPotential (point_index, source_weight, sink_weight);
345 addEdge (
static_cast<int> (source_), point_index, source_weight);
346 addEdge (point_index,
static_cast<int> (sink_), sink_weight);
349 std::vector<int> neighbours;
350 std::vector<float> distances;
351 search_->setInputCloud (input_, indices_);
352 for (
int i_point = 0; i_point < number_of_indices; i_point++)
354 index_t point_index = (*indices_)[i_point];
355 search_->nearestKSearch (i_point, number_of_neighbours_, neighbours, distances);
356 for (std::size_t i_nghbr = 1; i_nghbr < neighbours.size (); i_nghbr++)
358 double weight = calculateBinaryPotential (point_index, neighbours[i_nghbr]);
359 addEdge (point_index, neighbours[i_nghbr], weight);
360 addEdge (neighbours[i_nghbr], point_index, weight);
370 template <
typename Po
intT>
void
373 double min_dist_to_foreground = std::numeric_limits<double>::max ();
376 double initial_point[] = {0.0, 0.0};
378 initial_point[0] = input_->points[point].x;
379 initial_point[1] = input_->points[point].y;
381 for (std::size_t i_point = 0; i_point < foreground_points_.size (); i_point++)
384 dist += (foreground_points_[i_point].x - initial_point[0]) * (foreground_points_[i_point].x - initial_point[0]);
385 dist += (foreground_points_[i_point].y - initial_point[1]) * (foreground_points_[i_point].y - initial_point[1]);
386 if (min_dist_to_foreground > dist)
388 min_dist_to_foreground = dist;
392 sink_weight = pow (min_dist_to_foreground / radius_, 0.5);
394 source_weight = source_weight_;
426 template <
typename Po
intT>
bool
429 std::set<int>::iterator iter_out = edge_marker_[source].find (target);
430 if ( iter_out != edge_marker_[source].end () )
435 bool edge_was_added, reverse_edge_was_added;
437 boost::tie (edge, edge_was_added) = boost::add_edge ( vertices_[source], vertices_[target], *graph_ );
438 boost::tie (reverse_edge, reverse_edge_was_added) = boost::add_edge ( vertices_[target], vertices_[source], *graph_ );
439 if ( !edge_was_added || !reverse_edge_was_added )
442 (*capacity_)[edge] = weight;
443 (*capacity_)[reverse_edge] = 0.0;
444 (*reverse_edges_)[edge] = reverse_edge;
445 (*reverse_edges_)[reverse_edge] = edge;
446 edge_marker_[source].insert (target);
452 template <
typename Po
intT>
double
457 distance += (input_->points[source].x - input_->points[target].x) * (input_->points[source].x - input_->points[target].x);
458 distance += (input_->points[source].y - input_->points[target].y) * (input_->points[source].y - input_->points[target].y);
459 distance += (input_->points[source].z - input_->points[target].z) * (input_->points[source].z - input_->points[target].z);
467 template <
typename Po
intT>
bool
472 std::pair<EdgeDescriptor, bool> sink_edge;
474 for (boost::tie (src_edge_iter, src_edge_end) = boost::out_edges (source_, *graph_); src_edge_iter != src_edge_end; src_edge_iter++)
476 double source_weight = 0.0;
477 double sink_weight = 0.0;
478 sink_edge.second =
false;
479 calculateUnaryPotential (
static_cast<int> (boost::target (*src_edge_iter, *graph_)), source_weight, sink_weight);
480 sink_edge = boost::lookup_edge (boost::target (*src_edge_iter, *graph_), sink_, *graph_);
481 if (!sink_edge.second)
484 (*capacity_)[*src_edge_iter] = source_weight;
485 (*capacity_)[sink_edge.first] = sink_weight;
492 template <
typename Po
intT>
bool
495 int number_of_points =
static_cast<int> (indices_->size ());
502 std::vector< std::set<VertexDescriptor> > edge_marker;
503 std::set<VertexDescriptor> out_edges_marker;
504 edge_marker.clear ();
505 edge_marker.resize (number_of_points + 2, out_edges_marker);
507 for (boost::tie (vertex_iter, vertex_end) = boost::vertices (*graph_); vertex_iter != vertex_end; vertex_iter++)
510 if (source_vertex == source_ || source_vertex == sink_)
512 for (boost::tie (edge_iter, edge_end) = boost::out_edges (source_vertex, *graph_); edge_iter != edge_end; edge_iter++)
516 if ((*capacity_)[reverse_edge] != 0.0)
521 std::set<VertexDescriptor>::iterator iter_out = edge_marker[
static_cast<int> (source_vertex)].find (target_vertex);
522 if ( iter_out != edge_marker[
static_cast<int> (source_vertex)].end () )
525 if (target_vertex != source_ && target_vertex != sink_)
528 double weight = calculateBinaryPotential (
static_cast<int> (target_vertex),
static_cast<int> (source_vertex));
529 (*capacity_)[*edge_iter] = weight;
530 edge_marker[
static_cast<int> (source_vertex)].insert (target_vertex);
539 template <
typename Po
intT>
void
542 std::vector<int> labels;
543 labels.resize (input_->points.size (), 0);
544 int number_of_indices =
static_cast<int> (indices_->size ());
545 for (
int i_point = 0; i_point < number_of_indices; i_point++)
546 labels[(*indices_)[i_point]] = 1;
551 clusters_.resize (2, segment);
554 for ( boost::tie (edge_iter, edge_end) = boost::out_edges (source_, *graph_); edge_iter != edge_end; edge_iter++ )
556 if (labels[edge_iter->m_target] == 1)
558 if (residual_capacity[*edge_iter] > epsilon_)
559 clusters_[1].
indices.push_back (
static_cast<int> (edge_iter->m_target));
561 clusters_[0].indices.push_back (
static_cast<int> (edge_iter->m_target));
572 if (!clusters_.empty ())
574 int num_of_pts_in_first_cluster =
static_cast<int> (clusters_[0].indices.size ());
575 int num_of_pts_in_second_cluster =
static_cast<int> (clusters_[1].indices.size ());
576 int number_of_points = num_of_pts_in_first_cluster + num_of_pts_in_second_cluster;
578 unsigned char foreground_color[3] = {255, 255, 255};
579 unsigned char background_color[3] = {255, 0, 0};
580 colored_cloud->
width = number_of_points;
581 colored_cloud->
height = 1;
582 colored_cloud->
is_dense = input_->is_dense;
585 for (
int i_point = 0; i_point < num_of_pts_in_first_cluster; i_point++)
587 index_t point_index = clusters_[0].indices[i_point];
588 point.x = *(input_->points[point_index].data);
589 point.y = *(input_->points[point_index].data + 1);
590 point.z = *(input_->points[point_index].data + 2);
591 point.r = background_color[0];
592 point.g = background_color[1];
593 point.b = background_color[2];
594 colored_cloud->
points.push_back (point);
597 for (
int i_point = 0; i_point < num_of_pts_in_second_cluster; i_point++)
599 index_t point_index = clusters_[1].indices[i_point];
600 point.x = *(input_->points[point_index].data);
601 point.y = *(input_->points[point_index].data + 1);
602 point.z = *(input_->points[point_index].data + 2);
603 point.r = foreground_color[0];
604 point.g = foreground_color[1];
605 point.b = foreground_color[2];
606 colored_cloud->
points.push_back (point);
610 return (colored_cloud);
613 #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
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.