39 #ifndef PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_
40 #define PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_
42 #include <boost/graph/boykov_kolmogorov_max_flow.hpp>
43 #include <pcl/segmentation/min_cut_segmentation.h>
44 #include <pcl/search/search.h>
45 #include <pcl/search/kdtree.h>
49 template <
typename Po
intT>
51 inverse_sigma_ (16.0),
52 binary_potentials_are_valid_ (false),
55 unary_potentials_are_valid_ (false),
58 number_of_neighbours_ (14),
59 graph_is_valid_ (false),
60 foreground_points_ (0),
61 background_points_ (0),
72 template <
typename Po
intT>
75 foreground_points_.clear ();
76 background_points_.clear ();
79 edge_marker_.clear ();
83 template <
typename Po
intT>
void
87 graph_is_valid_ =
false;
88 unary_potentials_are_valid_ =
false;
89 binary_potentials_are_valid_ =
false;
93 template <
typename Po
intT>
double
96 return (pow (1.0 / inverse_sigma_, 0.5));
100 template <
typename Po
intT>
void
103 if (sigma > epsilon_)
105 inverse_sigma_ = 1.0 / (sigma * sigma);
106 binary_potentials_are_valid_ =
false;
111 template <
typename Po
intT>
double
114 return (pow (radius_, 0.5));
118 template <
typename Po
intT>
void
121 if (radius > epsilon_)
123 radius_ = radius * radius;
124 unary_potentials_are_valid_ =
false;
129 template <
typename Po
intT>
double
132 return (source_weight_);
136 template <
typename Po
intT>
void
139 if (weight > epsilon_)
141 source_weight_ = weight;
142 unary_potentials_are_valid_ =
false;
154 template <
typename Po
intT>
void
161 template <
typename Po
intT>
unsigned int
164 return (number_of_neighbours_);
168 template <
typename Po
intT>
void
171 if (number_of_neighbours_ != neighbour_number && neighbour_number != 0)
173 number_of_neighbours_ = neighbour_number;
174 graph_is_valid_ =
false;
175 unary_potentials_are_valid_ =
false;
176 binary_potentials_are_valid_ =
false;
181 template <
typename Po
intT> std::vector<PointT, Eigen::aligned_allocator<PointT> >
184 return (foreground_points_);
188 template <
typename Po
intT>
void
191 foreground_points_.clear ();
192 foreground_points_.insert(
193 foreground_points_.end(), foreground_points->
cbegin(), foreground_points->
cend());
195 unary_potentials_are_valid_ =
false;
199 template <
typename Po
intT> std::vector<PointT, Eigen::aligned_allocator<PointT> >
202 return (background_points_);
206 template <
typename Po
intT>
void
209 background_points_.clear ();
210 background_points_.insert(
211 background_points_.end(), background_points->
cbegin(), background_points->
cend());
213 unary_potentials_are_valid_ =
false;
217 template <
typename Po
intT>
void
222 bool segmentation_is_possible = initCompute ();
223 if ( !segmentation_is_possible )
229 if ( graph_is_valid_ && unary_potentials_are_valid_ && binary_potentials_are_valid_ )
231 clusters.reserve (clusters_.size ());
232 std::copy (clusters_.cbegin (), clusters_.cend (), std::back_inserter (clusters));
239 if ( !graph_is_valid_ )
241 bool success = buildGraph ();
247 graph_is_valid_ =
true;
248 unary_potentials_are_valid_ =
true;
249 binary_potentials_are_valid_ =
true;
252 if ( !unary_potentials_are_valid_ )
254 bool success = recalculateUnaryPotentials ();
260 unary_potentials_are_valid_ =
true;
263 if ( !binary_potentials_are_valid_ )
265 bool success = recalculateBinaryPotentials ();
271 binary_potentials_are_valid_ =
true;
275 ResidualCapacityMap residual_capacity = boost::get (boost::edge_residual_capacity, *graph_);
277 max_flow_ = boost::boykov_kolmogorov_max_flow (*graph_, source_, sink_);
279 assembleLabels (residual_capacity);
281 clusters.reserve (clusters_.size ());
282 std::copy (clusters_.cbegin (), clusters_.cend (), std::back_inserter (clusters));
288 template <
typename Po
intT>
double
302 template <
typename Po
intT>
bool
305 const auto number_of_points = input_->size ();
306 const auto number_of_indices = indices_->size ();
308 if (input_->points.empty () || number_of_points == 0 || foreground_points_.empty () ==
true )
314 graph_.reset (
new mGraph);
317 *capacity_ = boost::get (boost::edge_capacity, *graph_);
320 *reverse_edges_ = boost::get (boost::edge_reverse, *graph_);
324 vertices_.resize (number_of_points + 2, vertex_descriptor);
326 std::set<int> out_edges_marker;
327 edge_marker_.clear ();
328 edge_marker_.resize (number_of_points + 2, out_edges_marker);
330 for (std::size_t i_point = 0; i_point < number_of_points + 2; i_point++)
331 vertices_[i_point] = boost::add_vertex (*graph_);
333 source_ = vertices_[number_of_points];
334 sink_ = vertices_[number_of_points + 1];
336 for (
const auto& point_index : (*indices_))
338 double source_weight = 0.0;
339 double sink_weight = 0.0;
340 calculateUnaryPotential (point_index, source_weight, sink_weight);
341 addEdge (
static_cast<int> (source_), point_index, source_weight);
342 addEdge (point_index,
static_cast<int> (sink_), sink_weight);
346 std::vector<float> distances;
347 search_->setInputCloud (input_, indices_);
348 for (std::size_t i_point = 0; i_point < number_of_indices; i_point++)
350 index_t point_index = (*indices_)[i_point];
351 search_->nearestKSearch (i_point, number_of_neighbours_, neighbours, distances);
352 for (std::size_t i_nghbr = 1; i_nghbr < neighbours.size (); i_nghbr++)
354 double weight = calculateBinaryPotential (point_index, neighbours[i_nghbr]);
355 addEdge (point_index, neighbours[i_nghbr], weight);
356 addEdge (neighbours[i_nghbr], point_index, weight);
366 template <
typename Po
intT>
void
369 double min_dist_to_foreground = std::numeric_limits<double>::max ();
372 double initial_point[] = {0.0, 0.0};
374 initial_point[0] = (*input_)[point].x;
375 initial_point[1] = (*input_)[point].y;
377 for (
const auto& fg_point : foreground_points_)
380 dist += (fg_point.x - initial_point[0]) * (fg_point.x - initial_point[0]);
381 dist += (fg_point.y - initial_point[1]) * (fg_point.y - initial_point[1]);
382 if (min_dist_to_foreground > dist)
384 min_dist_to_foreground = dist;
388 sink_weight = pow (min_dist_to_foreground / radius_, 0.5);
390 source_weight = source_weight_;
422 template <
typename Po
intT>
bool
425 auto iter_out = edge_marker_[source].find (target);
426 if ( iter_out != edge_marker_[source].end () )
431 bool edge_was_added, reverse_edge_was_added;
433 boost::tie (edge, edge_was_added) = boost::add_edge ( vertices_[source], vertices_[target], *graph_ );
434 boost::tie (reverse_edge, reverse_edge_was_added) = boost::add_edge ( vertices_[target], vertices_[source], *graph_ );
435 if ( !edge_was_added || !reverse_edge_was_added )
438 (*capacity_)[edge] = weight;
439 (*capacity_)[reverse_edge] = 0.0;
440 (*reverse_edges_)[edge] = reverse_edge;
441 (*reverse_edges_)[reverse_edge] = edge;
442 edge_marker_[source].insert (target);
448 template <
typename Po
intT>
double
453 distance += ((*input_)[source].x - (*input_)[target].x) * ((*input_)[source].x - (*input_)[target].x);
454 distance += ((*input_)[source].y - (*input_)[target].y) * ((*input_)[source].y - (*input_)[target].y);
455 distance += ((*input_)[source].z - (*input_)[target].z) * ((*input_)[source].z - (*input_)[target].z);
463 template <
typename Po
intT>
bool
468 std::pair<EdgeDescriptor, bool> sink_edge;
470 for (boost::tie (src_edge_iter, src_edge_end) = boost::out_edges (source_, *graph_); src_edge_iter != src_edge_end; ++src_edge_iter)
472 double source_weight = 0.0;
473 double sink_weight = 0.0;
474 sink_edge.second =
false;
475 calculateUnaryPotential (
static_cast<int> (boost::target (*src_edge_iter, *graph_)), source_weight, sink_weight);
476 sink_edge = boost::lookup_edge (boost::target (*src_edge_iter, *graph_), sink_, *graph_);
477 if (!sink_edge.second)
480 (*capacity_)[*src_edge_iter] = source_weight;
481 (*capacity_)[sink_edge.first] = sink_weight;
488 template <
typename Po
intT>
bool
496 std::vector< std::set<VertexDescriptor> > edge_marker;
497 std::set<VertexDescriptor> out_edges_marker;
498 edge_marker.clear ();
499 edge_marker.resize (input_->size () + 2, out_edges_marker);
501 for (boost::tie (vertex_iter, vertex_end) = boost::vertices (*graph_); vertex_iter != vertex_end; ++vertex_iter)
504 if (source_vertex == source_ || source_vertex == sink_)
506 for (boost::tie (edge_iter, edge_end) = boost::out_edges (source_vertex, *graph_); edge_iter != edge_end; ++edge_iter)
510 if ((*capacity_)[reverse_edge] != 0.0)
515 auto iter_out = edge_marker[
static_cast<int> (source_vertex)].find (target_vertex);
516 if ( iter_out != edge_marker[
static_cast<int> (source_vertex)].end () )
519 if (target_vertex != source_ && target_vertex != sink_)
522 double weight = calculateBinaryPotential (
static_cast<int> (target_vertex),
static_cast<int> (source_vertex));
523 (*capacity_)[*edge_iter] = weight;
524 edge_marker[
static_cast<int> (source_vertex)].insert (target_vertex);
533 template <
typename Po
intT>
void
536 std::vector<int> labels;
537 labels.resize (input_->size (), 0);
538 for (
const auto& i_point : (*indices_))
544 clusters_.resize (2, segment);
547 for ( boost::tie (edge_iter, edge_end) = boost::out_edges (source_, *graph_); edge_iter != edge_end; ++edge_iter )
549 if (labels[edge_iter->m_target] == 1)
551 if (residual_capacity[*edge_iter] > epsilon_)
552 clusters_[1].
indices.push_back (
static_cast<int> (edge_iter->m_target));
554 clusters_[0].indices.push_back (
static_cast<int> (edge_iter->m_target));
565 if (!clusters_.empty ())
568 unsigned char foreground_color[3] = {255, 255, 255};
569 unsigned char background_color[3] = {255, 0, 0};
570 colored_cloud->
width = (clusters_[0].indices.size () + clusters_[1].indices.size ());
571 colored_cloud->
height = 1;
572 colored_cloud->
is_dense = input_->is_dense;
575 for (
const auto& point_index : (clusters_[0].indices))
577 point.x = *((*input_)[point_index].data);
578 point.y = *((*input_)[point_index].data + 1);
579 point.z = *((*input_)[point_index].data + 2);
580 point.r = background_color[0];
581 point.g = background_color[1];
582 point.b = background_color[2];
583 colored_cloud->
points.push_back (point);
586 for (
const auto& point_index : (clusters_[1].indices))
588 point.x = *((*input_)[point_index].data);
589 point.y = *((*input_)[point_index].data + 1);
590 point.z = *((*input_)[point_index].data + 2);
591 point.r = foreground_color[0];
592 point.g = foreground_color[1];
593 point.b = foreground_color[2];
594 colored_cloud->
points.push_back (point);
598 return (colored_cloud);
601 #define PCL_INSTANTIATE_MinCutSegmentation(T) template class pcl::MinCutSegmentation<T>;
KdTreePtr getSearchMethod() const
Returns search method that is used for finding KNN.
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.
~MinCutSegmentation() override
Destructor that frees memory.
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.
IndicesAllocator<> Indices
Type used for indices in PCL.
A point structure representing Euclidean xyz coordinates, and the RGB color.