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>;
615 #endif // PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_