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>
51 pcl::MinCutSegmentation<PointT>::MinCutSegmentation () :
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),
76 template <
typename Po
intT>
77 pcl::MinCutSegmentation<PointT>::~MinCutSegmentation ()
85 if (reverse_edges_ != 0)
86 reverse_edges_.reset ();
88 foreground_points_.clear ();
89 background_points_.clear ();
92 edge_marker_.clear ();
96 template <
typename Po
intT>
void 97 pcl::MinCutSegmentation<PointT>::setInputCloud (
const PointCloudConstPtr &cloud)
100 graph_is_valid_ =
false;
101 unary_potentials_are_valid_ =
false;
102 binary_potentials_are_valid_ =
false;
106 template <
typename Po
intT>
double 107 pcl::MinCutSegmentation<PointT>::getSigma ()
const 109 return (pow (1.0 / inverse_sigma_, 0.5));
113 template <
typename Po
intT>
void 114 pcl::MinCutSegmentation<PointT>::setSigma (
double sigma)
116 if (sigma > epsilon_)
118 inverse_sigma_ = 1.0 / (sigma * sigma);
119 binary_potentials_are_valid_ =
false;
124 template <
typename Po
intT>
double 125 pcl::MinCutSegmentation<PointT>::getRadius ()
const 127 return (pow (radius_, 0.5));
131 template <
typename Po
intT>
void 132 pcl::MinCutSegmentation<PointT>::setRadius (
double radius)
134 if (radius > epsilon_)
136 radius_ = radius * radius;
137 unary_potentials_are_valid_ =
false;
142 template <
typename Po
intT>
double 143 pcl::MinCutSegmentation<PointT>::getSourceWeight ()
const 145 return (source_weight_);
149 template <
typename Po
intT>
void 150 pcl::MinCutSegmentation<PointT>::setSourceWeight (
double weight)
152 if (weight > epsilon_)
154 source_weight_ = weight;
155 unary_potentials_are_valid_ =
false;
160 template <
typename Po
intT>
typename pcl::MinCutSegmentation<PointT>::KdTreePtr
161 pcl::MinCutSegmentation<PointT>::getSearchMethod ()
const 167 template <
typename Po
intT>
void 168 pcl::MinCutSegmentation<PointT>::setSearchMethod (
const KdTreePtr& tree)
177 template <
typename Po
intT>
unsigned int 178 pcl::MinCutSegmentation<PointT>::getNumberOfNeighbours ()
const 180 return (number_of_neighbours_);
184 template <
typename Po
intT>
void 185 pcl::MinCutSegmentation<PointT>::setNumberOfNeighbours (
unsigned int neighbour_number)
187 if (number_of_neighbours_ != neighbour_number && neighbour_number != 0)
189 number_of_neighbours_ = neighbour_number;
190 graph_is_valid_ =
false;
191 unary_potentials_are_valid_ =
false;
192 binary_potentials_are_valid_ =
false;
197 template <
typename Po
intT> std::vector<PointT, Eigen::aligned_allocator<PointT> >
198 pcl::MinCutSegmentation<PointT>::getForegroundPoints ()
const 200 return (foreground_points_);
204 template <
typename Po
intT>
void 207 foreground_points_.clear ();
208 foreground_points_.reserve (foreground_points->
points.size ());
209 for (
size_t i_point = 0; i_point < foreground_points->
points.size (); i_point++)
210 foreground_points_.push_back (foreground_points->
points[i_point]);
212 unary_potentials_are_valid_ =
false;
216 template <
typename Po
intT> std::vector<PointT, Eigen::aligned_allocator<PointT> >
217 pcl::MinCutSegmentation<PointT>::getBackgroundPoints ()
const 219 return (background_points_);
223 template <
typename Po
intT>
void 226 background_points_.clear ();
227 background_points_.reserve (background_points->
points.size ());
228 for (
size_t i_point = 0; i_point < background_points->
points.size (); i_point++)
229 background_points_.push_back (background_points->
points[i_point]);
231 unary_potentials_are_valid_ =
false;
235 template <
typename Po
intT>
void 236 pcl::MinCutSegmentation<PointT>::extract (std::vector <pcl::PointIndices>& clusters)
240 bool segmentation_is_possible = initCompute ();
241 if ( !segmentation_is_possible )
247 if ( graph_is_valid_ && unary_potentials_are_valid_ && binary_potentials_are_valid_ )
249 clusters.reserve (clusters_.size ());
250 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
258 if ( !graph_is_valid_ )
260 success = buildGraph ();
261 if (success ==
false)
266 graph_is_valid_ =
true;
267 unary_potentials_are_valid_ =
true;
268 binary_potentials_are_valid_ =
true;
271 if ( !unary_potentials_are_valid_ )
273 success = recalculateUnaryPotentials ();
274 if (success ==
false)
279 unary_potentials_are_valid_ =
true;
282 if ( !binary_potentials_are_valid_ )
284 success = recalculateBinaryPotentials ();
285 if (success ==
false)
290 binary_potentials_are_valid_ =
true;
294 ResidualCapacityMap residual_capacity = boost::get (boost::edge_residual_capacity, *graph_);
296 max_flow_ = boost::boykov_kolmogorov_max_flow (*graph_, source_, sink_);
298 assembleLabels (residual_capacity);
300 clusters.reserve (clusters_.size ());
301 std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
307 template <
typename Po
intT>
double 308 pcl::MinCutSegmentation<PointT>::getMaxFlow ()
const 314 template <
typename Po
intT>
typename boost::shared_ptr<typename pcl::MinCutSegmentation<PointT>::mGraph>
315 pcl::MinCutSegmentation<PointT>::getGraph ()
const 321 template <
typename Po
intT>
bool 322 pcl::MinCutSegmentation<PointT>::buildGraph ()
324 int number_of_points = static_cast<int> (input_->points.size ());
325 int number_of_indices = static_cast<int> (indices_->size ());
327 if (input_->points.size () == 0 || number_of_points == 0 || foreground_points_.empty () == true )
334 graph_ = boost::shared_ptr< mGraph > (
new mGraph ());
337 capacity_ = boost::shared_ptr<CapacityMap> (
new CapacityMap ());
338 *capacity_ = boost::get (boost::edge_capacity, *graph_);
340 reverse_edges_.reset ();
341 reverse_edges_ = boost::shared_ptr<ReverseEdgeMap> (
new ReverseEdgeMap ());
342 *reverse_edges_ = boost::get (boost::edge_reverse, *graph_);
344 VertexDescriptor vertex_descriptor(0);
346 vertices_.resize (number_of_points + 2, vertex_descriptor);
348 std::set<int> out_edges_marker;
349 edge_marker_.clear ();
350 edge_marker_.resize (number_of_points + 2, out_edges_marker);
352 for (
int i_point = 0; i_point < number_of_points + 2; i_point++)
353 vertices_[i_point] = boost::add_vertex (*graph_);
355 source_ = vertices_[number_of_points];
356 sink_ = vertices_[number_of_points + 1];
358 for (
int i_point = 0; i_point < number_of_indices; i_point++)
360 int point_index = (*indices_)[i_point];
361 double source_weight = 0.0;
362 double sink_weight = 0.0;
363 calculateUnaryPotential (point_index, source_weight, sink_weight);
364 addEdge (static_cast<int> (source_), point_index, source_weight);
365 addEdge (point_index, static_cast<int> (sink_), sink_weight);
368 std::vector<int> neighbours;
369 std::vector<float> distances;
370 search_->setInputCloud (input_, indices_);
371 for (
int i_point = 0; i_point < number_of_indices; i_point++)
373 int point_index = (*indices_)[i_point];
374 search_->nearestKSearch (i_point, number_of_neighbours_, neighbours, distances);
375 for (
size_t i_nghbr = 1; i_nghbr < neighbours.size (); i_nghbr++)
377 double weight = calculateBinaryPotential (point_index, neighbours[i_nghbr]);
378 addEdge (point_index, neighbours[i_nghbr], weight);
379 addEdge (neighbours[i_nghbr], point_index, weight);
389 template <
typename Po
intT>
void 390 pcl::MinCutSegmentation<PointT>::calculateUnaryPotential (
int point,
double& source_weight,
double& sink_weight)
const 392 double min_dist_to_foreground = std::numeric_limits<double>::max ();
394 double closest_foreground_point[2];
395 closest_foreground_point[0] = closest_foreground_point[1] = 0;
397 double initial_point[] = {0.0, 0.0};
399 initial_point[0] = input_->points[point].x;
400 initial_point[1] = input_->points[point].y;
402 for (
size_t i_point = 0; i_point < foreground_points_.size (); i_point++)
405 dist += (foreground_points_[i_point].x - initial_point[0]) * (foreground_points_[i_point].x - initial_point[0]);
406 dist += (foreground_points_[i_point].y - initial_point[1]) * (foreground_points_[i_point].y - initial_point[1]);
407 if (min_dist_to_foreground > dist)
409 min_dist_to_foreground = dist;
410 closest_foreground_point[0] = foreground_points_[i_point].x;
411 closest_foreground_point[1] = foreground_points_[i_point].y;
415 sink_weight = pow (min_dist_to_foreground / radius_, 0.5);
417 source_weight = source_weight_;
449 template <
typename Po
intT>
bool 450 pcl::MinCutSegmentation<PointT>::addEdge (
int source,
int target,
double weight)
452 std::set<int>::iterator iter_out = edge_marker_[source].find (target);
453 if ( iter_out != edge_marker_[source].end () )
457 EdgeDescriptor reverse_edge;
458 bool edge_was_added, reverse_edge_was_added;
460 boost::tie (edge, edge_was_added) = boost::add_edge ( vertices_[source], vertices_[target], *graph_ );
461 boost::tie (reverse_edge, reverse_edge_was_added) = boost::add_edge ( vertices_[target], vertices_[source], *graph_ );
462 if ( !edge_was_added || !reverse_edge_was_added )
465 (*capacity_)[edge] = weight;
466 (*capacity_)[reverse_edge] = 0.0;
467 (*reverse_edges_)[edge] = reverse_edge;
468 (*reverse_edges_)[reverse_edge] = edge;
469 edge_marker_[source].insert (target);
475 template <
typename Po
intT>
double 476 pcl::MinCutSegmentation<PointT>::calculateBinaryPotential (
int source,
int target)
const 480 distance += (input_->points[source].x - input_->points[target].x) * (input_->points[source].x - input_->points[target].x);
481 distance += (input_->points[source].y - input_->points[target].y) * (input_->points[source].y - input_->points[target].y);
482 distance += (input_->points[source].z - input_->points[target].z) * (input_->points[source].z - input_->points[target].z);
490 template <
typename Po
intT>
bool 491 pcl::MinCutSegmentation<PointT>::recalculateUnaryPotentials ()
493 OutEdgeIterator src_edge_iter;
494 OutEdgeIterator src_edge_end;
495 std::pair<EdgeDescriptor, bool> sink_edge;
497 for (boost::tie (src_edge_iter, src_edge_end) = boost::out_edges (source_, *graph_); src_edge_iter != src_edge_end; src_edge_iter++)
499 double source_weight = 0.0;
500 double sink_weight = 0.0;
501 sink_edge.second =
false;
502 calculateUnaryPotential (static_cast<int> (boost::target (*src_edge_iter, *graph_)), source_weight, sink_weight);
503 sink_edge = boost::lookup_edge (boost::target (*src_edge_iter, *graph_), sink_, *graph_);
504 if (!sink_edge.second)
507 (*capacity_)[*src_edge_iter] = source_weight;
508 (*capacity_)[sink_edge.first] = sink_weight;
515 template <
typename Po
intT>
bool 516 pcl::MinCutSegmentation<PointT>::recalculateBinaryPotentials ()
518 int number_of_points = static_cast<int> (indices_->size ());
520 VertexIterator vertex_iter;
521 VertexIterator vertex_end;
522 OutEdgeIterator edge_iter;
523 OutEdgeIterator edge_end;
525 std::vector< std::set<VertexDescriptor> > edge_marker;
526 std::set<VertexDescriptor> out_edges_marker;
527 edge_marker.clear ();
528 edge_marker.resize (number_of_points + 2, out_edges_marker);
530 for (boost::tie (vertex_iter, vertex_end) = boost::vertices (*graph_); vertex_iter != vertex_end; vertex_iter++)
532 VertexDescriptor source_vertex = *vertex_iter;
533 if (source_vertex == source_ || source_vertex == sink_)
535 for (boost::tie (edge_iter, edge_end) = boost::out_edges (source_vertex, *graph_); edge_iter != edge_end; edge_iter++)
538 EdgeDescriptor reverse_edge = (*reverse_edges_)[*edge_iter];
539 if ((*capacity_)[reverse_edge] != 0.0)
543 VertexDescriptor target_vertex = boost::target (*edge_iter, *graph_);
544 std::set<VertexDescriptor>::iterator iter_out = edge_marker[static_cast<int> (source_vertex)].find (target_vertex);
545 if ( iter_out != edge_marker[static_cast<int> (source_vertex)].end () )
548 if (target_vertex != source_ && target_vertex != sink_)
551 double weight = calculateBinaryPotential (static_cast<int> (target_vertex), static_cast<int> (source_vertex));
552 (*capacity_)[*edge_iter] = weight;
553 edge_marker[static_cast<int> (source_vertex)].insert (target_vertex);
562 template <
typename Po
intT>
void 563 pcl::MinCutSegmentation<PointT>::assembleLabels (ResidualCapacityMap& residual_capacity)
565 std::vector<int> labels;
566 labels.resize (input_->points.size (), 0);
567 int number_of_indices = static_cast<int> (indices_->size ());
568 for (
int i_point = 0; i_point < number_of_indices; i_point++)
569 labels[(*indices_)[i_point]] = 1;
574 clusters_.resize (2, segment);
576 OutEdgeIterator edge_iter, edge_end;
577 for ( boost::tie (edge_iter, edge_end) = boost::out_edges (source_, *graph_); edge_iter != edge_end; edge_iter++ )
579 if (labels[edge_iter->m_target] == 1)
581 if (residual_capacity[*edge_iter] > epsilon_)
582 clusters_[1].
indices.push_back (static_cast<int> (edge_iter->m_target));
584 clusters_[0].indices.push_back (static_cast<int> (edge_iter->m_target));
591 pcl::MinCutSegmentation<PointT>::getColoredCloud ()
595 if (!clusters_.empty ())
597 int num_of_pts_in_first_cluster = static_cast<int> (clusters_[0].indices.size ());
598 int num_of_pts_in_second_cluster = static_cast<int> (clusters_[1].indices.size ());
599 int number_of_points = num_of_pts_in_first_cluster + num_of_pts_in_second_cluster;
601 unsigned char foreground_color[3] = {255, 255, 255};
602 unsigned char background_color[3] = {255, 0, 0};
603 colored_cloud->
width = number_of_points;
604 colored_cloud->
height = 1;
605 colored_cloud->
is_dense = input_->is_dense;
609 for (
int i_point = 0; i_point < num_of_pts_in_first_cluster; i_point++)
611 point_index = clusters_[0].indices[i_point];
612 point.x = *(input_->points[point_index].data);
613 point.y = *(input_->points[point_index].data + 1);
614 point.z = *(input_->points[point_index].data + 2);
615 point.r = background_color[0];
616 point.g = background_color[1];
617 point.b = background_color[2];
618 colored_cloud->
points.push_back (point);
621 for (
int i_point = 0; i_point < num_of_pts_in_second_cluster; i_point++)
623 point_index = clusters_[1].indices[i_point];
624 point.x = *(input_->points[point_index].data);
625 point.y = *(input_->points[point_index].data + 1);
626 point.z = *(input_->points[point_index].data + 2);
627 point.r = foreground_color[0];
628 point.g = foreground_color[1];
629 point.b = foreground_color[2];
630 colored_cloud->
points.push_back (point);
634 return (colored_cloud);
637 #define PCL_INSTANTIATE_MinCutSegmentation(T) template class pcl::MinCutSegmentation<T>; 639 #endif // PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
std::vector< int > indices
uint32_t height
The point cloud height (if organized as an image-structure).
boost::shared_ptr< PointCloud< PointT > > Ptr
uint32_t width
The point cloud width (if organized as an image-structure).
float distance(const PointT &p1, const PointT &p2)
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
A point structure representing Euclidean xyz coordinates, and the RGB color.