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));
243 if ( !graph_is_valid_ )
245 success = buildGraph ();
251 graph_is_valid_ =
true;
252 unary_potentials_are_valid_ =
true;
253 binary_potentials_are_valid_ =
true;
256 if ( !unary_potentials_are_valid_ )
258 success = recalculateUnaryPotentials ();
264 unary_potentials_are_valid_ =
true;
267 if ( !binary_potentials_are_valid_ )
269 success = recalculateBinaryPotentials ();
275 binary_potentials_are_valid_ =
true;
279 ResidualCapacityMap residual_capacity = boost::get (boost::edge_residual_capacity, *graph_);
281 max_flow_ = boost::boykov_kolmogorov_max_flow (*graph_, source_, sink_);
283 assembleLabels (residual_capacity);
285 clusters.reserve (clusters_.size ());
286 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
292 template <
typename Po
intT>
double
306 template <
typename Po
intT>
bool
309 int number_of_points =
static_cast<int> (input_->points.size ());
310 int number_of_indices =
static_cast<int> (indices_->size ());
312 if (input_->points.empty () || number_of_points == 0 || foreground_points_.empty () == true )
318 graph_.reset (
new mGraph);
321 *capacity_ = boost::get (boost::edge_capacity, *graph_);
324 *reverse_edges_ = boost::get (boost::edge_reverse, *graph_);
328 vertices_.resize (number_of_points + 2, vertex_descriptor);
330 std::set<int> out_edges_marker;
331 edge_marker_.clear ();
332 edge_marker_.resize (number_of_points + 2, out_edges_marker);
334 for (
int i_point = 0; i_point < number_of_points + 2; i_point++)
335 vertices_[i_point] = boost::add_vertex (*graph_);
337 source_ = vertices_[number_of_points];
338 sink_ = vertices_[number_of_points + 1];
340 for (
int i_point = 0; i_point < number_of_indices; i_point++)
342 int point_index = (*indices_)[i_point];
343 double source_weight = 0.0;
344 double sink_weight = 0.0;
345 calculateUnaryPotential (point_index, source_weight, sink_weight);
346 addEdge (
static_cast<int> (source_), point_index, source_weight);
347 addEdge (point_index,
static_cast<int> (sink_), sink_weight);
350 std::vector<int> neighbours;
351 std::vector<float> distances;
352 search_->setInputCloud (input_, indices_);
353 for (
int i_point = 0; i_point < number_of_indices; i_point++)
355 int point_index = (*indices_)[i_point];
356 search_->nearestKSearch (i_point, number_of_neighbours_, neighbours, distances);
357 for (std::size_t i_nghbr = 1; i_nghbr < neighbours.size (); i_nghbr++)
359 double weight = calculateBinaryPotential (point_index, neighbours[i_nghbr]);
360 addEdge (point_index, neighbours[i_nghbr], weight);
361 addEdge (neighbours[i_nghbr], point_index, weight);
371 template <
typename Po
intT>
void
374 double min_dist_to_foreground = std::numeric_limits<double>::max ();
376 double closest_foreground_point[2];
377 closest_foreground_point[0] = closest_foreground_point[1] = 0;
379 double initial_point[] = {0.0, 0.0};
381 initial_point[0] = input_->points[point].x;
382 initial_point[1] = input_->points[point].y;
384 for (std::size_t i_point = 0; i_point < foreground_points_.size (); i_point++)
387 dist += (foreground_points_[i_point].x - initial_point[0]) * (foreground_points_[i_point].x - initial_point[0]);
388 dist += (foreground_points_[i_point].y - initial_point[1]) * (foreground_points_[i_point].y - initial_point[1]);
389 if (min_dist_to_foreground > dist)
391 min_dist_to_foreground = dist;
392 closest_foreground_point[0] = foreground_points_[i_point].x;
393 closest_foreground_point[1] = foreground_points_[i_point].y;
397 sink_weight = pow (min_dist_to_foreground / radius_, 0.5);
399 source_weight = source_weight_;
431 template <
typename Po
intT>
bool
434 std::set<int>::iterator iter_out = edge_marker_[source].find (target);
435 if ( iter_out != edge_marker_[source].end () )
440 bool edge_was_added, reverse_edge_was_added;
442 boost::tie (edge, edge_was_added) = boost::add_edge ( vertices_[source], vertices_[target], *graph_ );
443 boost::tie (reverse_edge, reverse_edge_was_added) = boost::add_edge ( vertices_[target], vertices_[source], *graph_ );
444 if ( !edge_was_added || !reverse_edge_was_added )
447 (*capacity_)[edge] = weight;
448 (*capacity_)[reverse_edge] = 0.0;
449 (*reverse_edges_)[edge] = reverse_edge;
450 (*reverse_edges_)[reverse_edge] = edge;
451 edge_marker_[source].insert (target);
457 template <
typename Po
intT>
double
462 distance += (input_->points[source].x - input_->points[target].x) * (input_->points[source].x - input_->points[target].x);
463 distance += (input_->points[source].y - input_->points[target].y) * (input_->points[source].y - input_->points[target].y);
464 distance += (input_->points[source].z - input_->points[target].z) * (input_->points[source].z - input_->points[target].z);
472 template <
typename Po
intT>
bool
477 std::pair<EdgeDescriptor, bool> sink_edge;
479 for (boost::tie (src_edge_iter, src_edge_end) = boost::out_edges (source_, *graph_); src_edge_iter != src_edge_end; src_edge_iter++)
481 double source_weight = 0.0;
482 double sink_weight = 0.0;
483 sink_edge.second =
false;
484 calculateUnaryPotential (
static_cast<int> (boost::target (*src_edge_iter, *graph_)), source_weight, sink_weight);
485 sink_edge = boost::lookup_edge (boost::target (*src_edge_iter, *graph_), sink_, *graph_);
486 if (!sink_edge.second)
489 (*capacity_)[*src_edge_iter] = source_weight;
490 (*capacity_)[sink_edge.first] = sink_weight;
497 template <
typename Po
intT>
bool
500 int number_of_points =
static_cast<int> (indices_->size ());
507 std::vector< std::set<VertexDescriptor> > edge_marker;
508 std::set<VertexDescriptor> out_edges_marker;
509 edge_marker.clear ();
510 edge_marker.resize (number_of_points + 2, out_edges_marker);
512 for (boost::tie (vertex_iter, vertex_end) = boost::vertices (*graph_); vertex_iter != vertex_end; vertex_iter++)
515 if (source_vertex == source_ || source_vertex == sink_)
517 for (boost::tie (edge_iter, edge_end) = boost::out_edges (source_vertex, *graph_); edge_iter != edge_end; edge_iter++)
521 if ((*capacity_)[reverse_edge] != 0.0)
526 std::set<VertexDescriptor>::iterator iter_out = edge_marker[
static_cast<int> (source_vertex)].find (target_vertex);
527 if ( iter_out != edge_marker[
static_cast<int> (source_vertex)].end () )
530 if (target_vertex != source_ && target_vertex != sink_)
533 double weight = calculateBinaryPotential (
static_cast<int> (target_vertex),
static_cast<int> (source_vertex));
534 (*capacity_)[*edge_iter] = weight;
535 edge_marker[
static_cast<int> (source_vertex)].insert (target_vertex);
544 template <
typename Po
intT>
void
547 std::vector<int> labels;
548 labels.resize (input_->points.size (), 0);
549 int number_of_indices =
static_cast<int> (indices_->size ());
550 for (
int i_point = 0; i_point < number_of_indices; i_point++)
551 labels[(*indices_)[i_point]] = 1;
556 clusters_.resize (2, segment);
559 for ( boost::tie (edge_iter, edge_end) = boost::out_edges (source_, *graph_); edge_iter != edge_end; edge_iter++ )
561 if (labels[edge_iter->m_target] == 1)
563 if (residual_capacity[*edge_iter] > epsilon_)
564 clusters_[1].
indices.push_back (
static_cast<int> (edge_iter->m_target));
566 clusters_[0].indices.push_back (
static_cast<int> (edge_iter->m_target));
577 if (!clusters_.empty ())
579 int num_of_pts_in_first_cluster =
static_cast<int> (clusters_[0].indices.size ());
580 int num_of_pts_in_second_cluster =
static_cast<int> (clusters_[1].indices.size ());
581 int number_of_points = num_of_pts_in_first_cluster + num_of_pts_in_second_cluster;
583 unsigned char foreground_color[3] = {255, 255, 255};
584 unsigned char background_color[3] = {255, 0, 0};
585 colored_cloud->
width = number_of_points;
586 colored_cloud->
height = 1;
587 colored_cloud->
is_dense = input_->is_dense;
591 for (
int i_point = 0; i_point < num_of_pts_in_first_cluster; i_point++)
593 point_index = clusters_[0].indices[i_point];
594 point.x = *(input_->points[point_index].data);
595 point.y = *(input_->points[point_index].data + 1);
596 point.z = *(input_->points[point_index].data + 2);
597 point.r = background_color[0];
598 point.g = background_color[1];
599 point.b = background_color[2];
600 colored_cloud->
points.push_back (point);
603 for (
int i_point = 0; i_point < num_of_pts_in_second_cluster; i_point++)
605 point_index = clusters_[1].indices[i_point];
606 point.x = *(input_->points[point_index].data);
607 point.y = *(input_->points[point_index].data + 1);
608 point.z = *(input_->points[point_index].data + 2);
609 point.r = foreground_color[0];
610 point.g = foreground_color[1];
611 point.b = foreground_color[2];
612 colored_cloud->
points.push_back (point);
616 return (colored_cloud);
619 #define PCL_INSTANTIATE_MinCutSegmentation(T) template class pcl::MinCutSegmentation<T>;
621 #endif // PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_