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_