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>
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_.begin (), clusters_.end (), 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_.begin (), clusters_.end (), 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 (std::size_t i_point = 0; i_point < number_of_indices; i_point++)
338 index_t point_index = (*indices_)[i_point];
339 double source_weight = 0.0;
340 double sink_weight = 0.0;
341 calculateUnaryPotential (point_index, source_weight, sink_weight);
342 addEdge (
static_cast<int> (source_), point_index, source_weight);
343 addEdge (point_index,
static_cast<int> (sink_), sink_weight);
346 std::vector<int> neighbours;
347 std::vector<float> distances;
348 search_->setInputCloud (input_, indices_);
349 for (std::size_t i_point = 0; i_point < number_of_indices; i_point++)
351 index_t point_index = (*indices_)[i_point];
352 search_->nearestKSearch (i_point, number_of_neighbours_, neighbours, distances);
353 for (std::size_t i_nghbr = 1; i_nghbr < neighbours.size (); i_nghbr++)
355 double weight = calculateBinaryPotential (point_index, neighbours[i_nghbr]);
356 addEdge (point_index, neighbours[i_nghbr], weight);
357 addEdge (neighbours[i_nghbr], point_index, weight);
367 template <
typename Po
intT>
void
370 double min_dist_to_foreground = std::numeric_limits<double>::max ();
373 double initial_point[] = {0.0, 0.0};
375 initial_point[0] = (*input_)[point].x;
376 initial_point[1] = (*input_)[point].y;
378 for (std::size_t i_point = 0; i_point < foreground_points_.size (); i_point++)
381 dist += (foreground_points_[i_point].x - initial_point[0]) * (foreground_points_[i_point].x - initial_point[0]);
382 dist += (foreground_points_[i_point].y - initial_point[1]) * (foreground_points_[i_point].y - initial_point[1]);
383 if (min_dist_to_foreground > dist)
385 min_dist_to_foreground = dist;
389 sink_weight = pow (min_dist_to_foreground / radius_, 0.5);
391 source_weight = source_weight_;
423 template <
typename Po
intT>
bool
426 std::set<int>::iterator iter_out = edge_marker_[source].find (target);
427 if ( iter_out != edge_marker_[source].end () )
432 bool edge_was_added, reverse_edge_was_added;
434 boost::tie (edge, edge_was_added) = boost::add_edge ( vertices_[source], vertices_[target], *graph_ );
435 boost::tie (reverse_edge, reverse_edge_was_added) = boost::add_edge ( vertices_[target], vertices_[source], *graph_ );
436 if ( !edge_was_added || !reverse_edge_was_added )
439 (*capacity_)[edge] = weight;
440 (*capacity_)[reverse_edge] = 0.0;
441 (*reverse_edges_)[edge] = reverse_edge;
442 (*reverse_edges_)[reverse_edge] = edge;
443 edge_marker_[source].insert (target);
449 template <
typename Po
intT>
double
454 distance += ((*input_)[source].x - (*input_)[target].x) * ((*input_)[source].x - (*input_)[target].x);
455 distance += ((*input_)[source].y - (*input_)[target].y) * ((*input_)[source].y - (*input_)[target].y);
456 distance += ((*input_)[source].z - (*input_)[target].z) * ((*input_)[source].z - (*input_)[target].z);
464 template <
typename Po
intT>
bool
469 std::pair<EdgeDescriptor, bool> sink_edge;
471 for (boost::tie (src_edge_iter, src_edge_end) = boost::out_edges (source_, *graph_); src_edge_iter != src_edge_end; src_edge_iter++)
473 double source_weight = 0.0;
474 double sink_weight = 0.0;
475 sink_edge.second =
false;
476 calculateUnaryPotential (
static_cast<int> (boost::target (*src_edge_iter, *graph_)), source_weight, sink_weight);
477 sink_edge = boost::lookup_edge (boost::target (*src_edge_iter, *graph_), sink_, *graph_);
478 if (!sink_edge.second)
481 (*capacity_)[*src_edge_iter] = source_weight;
482 (*capacity_)[sink_edge.first] = sink_weight;
489 template <
typename Po
intT>
bool
497 std::vector< std::set<VertexDescriptor> > edge_marker;
498 std::set<VertexDescriptor> out_edges_marker;
499 edge_marker.clear ();
500 edge_marker.resize (indices_->size () + 2, out_edges_marker);
502 for (boost::tie (vertex_iter, vertex_end) = boost::vertices (*graph_); vertex_iter != vertex_end; vertex_iter++)
505 if (source_vertex == source_ || source_vertex == sink_)
507 for (boost::tie (edge_iter, edge_end) = boost::out_edges (source_vertex, *graph_); edge_iter != edge_end; edge_iter++)
511 if ((*capacity_)[reverse_edge] != 0.0)
516 std::set<VertexDescriptor>::iterator iter_out = edge_marker[
static_cast<int> (source_vertex)].find (target_vertex);
517 if ( iter_out != edge_marker[
static_cast<int> (source_vertex)].end () )
520 if (target_vertex != source_ && target_vertex != sink_)
523 double weight = calculateBinaryPotential (
static_cast<int> (target_vertex),
static_cast<int> (source_vertex));
524 (*capacity_)[*edge_iter] = weight;
525 edge_marker[
static_cast<int> (source_vertex)].insert (target_vertex);
534 template <
typename Po
intT>
void
537 std::vector<int> labels;
538 labels.resize (input_->size (), 0);
539 int number_of_indices =
static_cast<int> (indices_->size ());
540 for (
int i_point = 0; i_point < number_of_indices; i_point++)
541 labels[(*indices_)[i_point]] = 1;
546 clusters_.resize (2, segment);
549 for ( boost::tie (edge_iter, edge_end) = boost::out_edges (source_, *graph_); edge_iter != edge_end; edge_iter++ )
551 if (labels[edge_iter->m_target] == 1)
553 if (residual_capacity[*edge_iter] > epsilon_)
554 clusters_[1].
indices.push_back (
static_cast<int> (edge_iter->m_target));
556 clusters_[0].indices.push_back (
static_cast<int> (edge_iter->m_target));
567 if (!clusters_.empty ())
569 int num_of_pts_in_first_cluster =
static_cast<int> (clusters_[0].indices.size ());
570 int num_of_pts_in_second_cluster =
static_cast<int> (clusters_[1].indices.size ());
572 unsigned char foreground_color[3] = {255, 255, 255};
573 unsigned char background_color[3] = {255, 0, 0};
574 colored_cloud->
width = (num_of_pts_in_first_cluster + num_of_pts_in_second_cluster);
575 colored_cloud->
height = 1;
576 colored_cloud->
is_dense = input_->is_dense;
579 for (
int i_point = 0; i_point < num_of_pts_in_first_cluster; i_point++)
581 index_t point_index = clusters_[0].indices[i_point];
582 point.x = *((*input_)[point_index].data);
583 point.y = *((*input_)[point_index].data + 1);
584 point.z = *((*input_)[point_index].data + 2);
585 point.r = background_color[0];
586 point.g = background_color[1];
587 point.b = background_color[2];
588 colored_cloud->
points.push_back (point);
591 for (
int i_point = 0; i_point < num_of_pts_in_second_cluster; i_point++)
593 index_t point_index = clusters_[1].indices[i_point];
594 point.x = *((*input_)[point_index].data);
595 point.y = *((*input_)[point_index].data + 1);
596 point.z = *((*input_)[point_index].data + 2);
597 point.r = foreground_color[0];
598 point.g = foreground_color[1];
599 point.b = foreground_color[2];
600 colored_cloud->
points.push_back (point);
604 return (colored_cloud);
607 #define PCL_INSTANTIATE_MinCutSegmentation(T) template class pcl::MinCutSegmentation<T>;
609 #endif // PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_