Point Cloud Library (PCL)  1.9.1
min_cut_segmentation.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  *
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the copyright holder(s) nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * $Id:$
36  *
37  */
38 
39 #ifndef PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_
40 #define PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_
41 
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>
46 #include <stdlib.h>
47 #include <cmath>
48 
49 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
50 template <typename PointT>
51 pcl::MinCutSegmentation<PointT>::MinCutSegmentation () :
52  inverse_sigma_ (16.0),
53  binary_potentials_are_valid_ (false),
54  epsilon_ (0.0001),
55  radius_ (16.0),
56  unary_potentials_are_valid_ (false),
57  source_weight_ (0.8),
58  search_ (),
59  number_of_neighbours_ (14),
60  graph_is_valid_ (false),
61  foreground_points_ (0),
62  background_points_ (0),
63  clusters_ (0),
64  graph_ (),
65  capacity_ (),
66  reverse_edges_ (),
67  vertices_ (0),
68  edge_marker_ (0),
69  source_ (),/////////////////////////////////
70  sink_ (),///////////////////////////////////
71  max_flow_ (0.0)
72 {
73 }
74 
75 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
76 template <typename PointT>
77 pcl::MinCutSegmentation<PointT>::~MinCutSegmentation ()
78 {
79  if (search_ != 0)
80  search_.reset ();
81  if (graph_ != 0)
82  graph_.reset ();
83  if (capacity_ != 0)
84  capacity_.reset ();
85  if (reverse_edges_ != 0)
86  reverse_edges_.reset ();
87 
88  foreground_points_.clear ();
89  background_points_.clear ();
90  clusters_.clear ();
91  vertices_.clear ();
92  edge_marker_.clear ();
93 }
94 
95 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
96 template <typename PointT> void
97 pcl::MinCutSegmentation<PointT>::setInputCloud (const PointCloudConstPtr &cloud)
98 {
99  input_ = cloud;
100  graph_is_valid_ = false;
101  unary_potentials_are_valid_ = false;
102  binary_potentials_are_valid_ = false;
103 }
104 
105 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
106 template <typename PointT> double
107 pcl::MinCutSegmentation<PointT>::getSigma () const
108 {
109  return (pow (1.0 / inverse_sigma_, 0.5));
110 }
111 
112 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
113 template <typename PointT> void
114 pcl::MinCutSegmentation<PointT>::setSigma (double sigma)
115 {
116  if (sigma > epsilon_)
117  {
118  inverse_sigma_ = 1.0 / (sigma * sigma);
119  binary_potentials_are_valid_ = false;
120  }
121 }
122 
123 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
124 template <typename PointT> double
125 pcl::MinCutSegmentation<PointT>::getRadius () const
126 {
127  return (pow (radius_, 0.5));
128 }
129 
130 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
131 template <typename PointT> void
132 pcl::MinCutSegmentation<PointT>::setRadius (double radius)
133 {
134  if (radius > epsilon_)
135  {
136  radius_ = radius * radius;
137  unary_potentials_are_valid_ = false;
138  }
139 }
140 
141 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
142 template <typename PointT> double
143 pcl::MinCutSegmentation<PointT>::getSourceWeight () const
144 {
145  return (source_weight_);
146 }
147 
148 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
149 template <typename PointT> void
150 pcl::MinCutSegmentation<PointT>::setSourceWeight (double weight)
151 {
152  if (weight > epsilon_)
153  {
154  source_weight_ = weight;
155  unary_potentials_are_valid_ = false;
156  }
157 }
158 
159 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
160 template <typename PointT> typename pcl::MinCutSegmentation<PointT>::KdTreePtr
161 pcl::MinCutSegmentation<PointT>::getSearchMethod () const
162 {
163  return (search_);
164 }
165 
166 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
167 template <typename PointT> void
168 pcl::MinCutSegmentation<PointT>::setSearchMethod (const KdTreePtr& tree)
169 {
170  if (search_ != 0)
171  search_.reset ();
172 
173  search_ = tree;
174 }
175 
176 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
177 template <typename PointT> unsigned int
178 pcl::MinCutSegmentation<PointT>::getNumberOfNeighbours () const
179 {
180  return (number_of_neighbours_);
181 }
182 
183 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
184 template <typename PointT> void
185 pcl::MinCutSegmentation<PointT>::setNumberOfNeighbours (unsigned int neighbour_number)
186 {
187  if (number_of_neighbours_ != neighbour_number && neighbour_number != 0)
188  {
189  number_of_neighbours_ = neighbour_number;
190  graph_is_valid_ = false;
191  unary_potentials_are_valid_ = false;
192  binary_potentials_are_valid_ = false;
193  }
194 }
195 
196 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
197 template <typename PointT> std::vector<PointT, Eigen::aligned_allocator<PointT> >
198 pcl::MinCutSegmentation<PointT>::getForegroundPoints () const
199 {
200  return (foreground_points_);
201 }
202 
203 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
204 template <typename PointT> void
205 pcl::MinCutSegmentation<PointT>::setForegroundPoints (typename pcl::PointCloud<PointT>::Ptr foreground_points)
206 {
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]);
211 
212  unary_potentials_are_valid_ = false;
213 }
214 
215 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
216 template <typename PointT> std::vector<PointT, Eigen::aligned_allocator<PointT> >
217 pcl::MinCutSegmentation<PointT>::getBackgroundPoints () const
218 {
219  return (background_points_);
220 }
221 
222 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
223 template <typename PointT> void
224 pcl::MinCutSegmentation<PointT>::setBackgroundPoints (typename pcl::PointCloud<PointT>::Ptr background_points)
225 {
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]);
230 
231  unary_potentials_are_valid_ = false;
232 }
233 
234 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
235 template <typename PointT> void
236 pcl::MinCutSegmentation<PointT>::extract (std::vector <pcl::PointIndices>& clusters)
237 {
238  clusters.clear ();
239 
240  bool segmentation_is_possible = initCompute ();
241  if ( !segmentation_is_possible )
242  {
243  deinitCompute ();
244  return;
245  }
246 
247  if ( graph_is_valid_ && unary_potentials_are_valid_ && binary_potentials_are_valid_ )
248  {
249  clusters.reserve (clusters_.size ());
250  std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
251  deinitCompute ();
252  return;
253  }
254 
255  clusters_.clear ();
256  bool success = true;
257 
258  if ( !graph_is_valid_ )
259  {
260  success = buildGraph ();
261  if (success == false)
262  {
263  deinitCompute ();
264  return;
265  }
266  graph_is_valid_ = true;
267  unary_potentials_are_valid_ = true;
268  binary_potentials_are_valid_ = true;
269  }
270 
271  if ( !unary_potentials_are_valid_ )
272  {
273  success = recalculateUnaryPotentials ();
274  if (success == false)
275  {
276  deinitCompute ();
277  return;
278  }
279  unary_potentials_are_valid_ = true;
280  }
281 
282  if ( !binary_potentials_are_valid_ )
283  {
284  success = recalculateBinaryPotentials ();
285  if (success == false)
286  {
287  deinitCompute ();
288  return;
289  }
290  binary_potentials_are_valid_ = true;
291  }
292 
293  //IndexMap index_map = boost::get (boost::vertex_index, *graph_);
294  ResidualCapacityMap residual_capacity = boost::get (boost::edge_residual_capacity, *graph_);
295 
296  max_flow_ = boost::boykov_kolmogorov_max_flow (*graph_, source_, sink_);
297 
298  assembleLabels (residual_capacity);
299 
300  clusters.reserve (clusters_.size ());
301  std::copy (clusters_.begin (), clusters_.end (), std::back_inserter (clusters));
302 
303  deinitCompute ();
304 }
305 
306 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
307 template <typename PointT> double
308 pcl::MinCutSegmentation<PointT>::getMaxFlow () const
309 {
310  return (max_flow_);
311 }
312 
313 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
314 template <typename PointT> typename boost::shared_ptr<typename pcl::MinCutSegmentation<PointT>::mGraph>
315 pcl::MinCutSegmentation<PointT>::getGraph () const
316 {
317  return (graph_);
318 }
319 
320 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
321 template <typename PointT> bool
322 pcl::MinCutSegmentation<PointT>::buildGraph ()
323 {
324  int number_of_points = static_cast<int> (input_->points.size ());
325  int number_of_indices = static_cast<int> (indices_->size ());
326 
327  if (input_->points.size () == 0 || number_of_points == 0 || foreground_points_.empty () == true )
328  return (false);
329 
330  if (search_ == 0)
331  search_ = boost::shared_ptr<pcl::search::Search<PointT> > (new pcl::search::KdTree<PointT>);
332 
333  graph_.reset ();
334  graph_ = boost::shared_ptr< mGraph > (new mGraph ());
335 
336  capacity_.reset ();
337  capacity_ = boost::shared_ptr<CapacityMap> (new CapacityMap ());
338  *capacity_ = boost::get (boost::edge_capacity, *graph_);
339 
340  reverse_edges_.reset ();
341  reverse_edges_ = boost::shared_ptr<ReverseEdgeMap> (new ReverseEdgeMap ());
342  *reverse_edges_ = boost::get (boost::edge_reverse, *graph_);
343 
344  VertexDescriptor vertex_descriptor(0);
345  vertices_.clear ();
346  vertices_.resize (number_of_points + 2, vertex_descriptor);
347 
348  std::set<int> out_edges_marker;
349  edge_marker_.clear ();
350  edge_marker_.resize (number_of_points + 2, out_edges_marker);
351 
352  for (int i_point = 0; i_point < number_of_points + 2; i_point++)
353  vertices_[i_point] = boost::add_vertex (*graph_);
354 
355  source_ = vertices_[number_of_points];
356  sink_ = vertices_[number_of_points + 1];
357 
358  for (int i_point = 0; i_point < number_of_indices; i_point++)
359  {
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);
366  }
367 
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++)
372  {
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++)
376  {
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);
380  }
381  neighbours.clear ();
382  distances.clear ();
383  }
384 
385  return (true);
386 }
387 
388 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
389 template <typename PointT> void
390 pcl::MinCutSegmentation<PointT>::calculateUnaryPotential (int point, double& source_weight, double& sink_weight) const
391 {
392  double min_dist_to_foreground = std::numeric_limits<double>::max ();
393  //double min_dist_to_background = std::numeric_limits<double>::max ();
394  double closest_foreground_point[2];
395  closest_foreground_point[0] = closest_foreground_point[1] = 0;
396  //double closest_background_point[] = {0.0, 0.0};
397  double initial_point[] = {0.0, 0.0};
398 
399  initial_point[0] = input_->points[point].x;
400  initial_point[1] = input_->points[point].y;
401 
402  for (size_t i_point = 0; i_point < foreground_points_.size (); i_point++)
403  {
404  double dist = 0.0;
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)
408  {
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;
412  }
413  }
414 
415  sink_weight = pow (min_dist_to_foreground / radius_, 0.5);
416 
417  source_weight = source_weight_;
418  return;
419 /*
420  if (background_points_.size () == 0)
421  return;
422 
423  for (int i_point = 0; i_point < background_points_.size (); i_point++)
424  {
425  double dist = 0.0;
426  dist += (background_points_[i_point].x - initial_point[0]) * (background_points_[i_point].x - initial_point[0]);
427  dist += (background_points_[i_point].y - initial_point[1]) * (background_points_[i_point].y - initial_point[1]);
428  if (min_dist_to_background > dist)
429  {
430  min_dist_to_background = dist;
431  closest_background_point[0] = background_points_[i_point].x;
432  closest_background_point[1] = background_points_[i_point].y;
433  }
434  }
435 
436  if (min_dist_to_background <= epsilon_)
437  {
438  source_weight = 0.0;
439  sink_weight = 1.0;
440  return;
441  }
442 
443  source_weight = 1.0 / (1.0 + pow (min_dist_to_background / min_dist_to_foreground, 0.5));
444  sink_weight = 1 - source_weight;
445 */
446 }
447 
448 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
449 template <typename PointT> bool
450 pcl::MinCutSegmentation<PointT>::addEdge (int source, int target, double weight)
451 {
452  std::set<int>::iterator iter_out = edge_marker_[source].find (target);
453  if ( iter_out != edge_marker_[source].end () )
454  return (false);
455 
456  EdgeDescriptor edge;
457  EdgeDescriptor reverse_edge;
458  bool edge_was_added, reverse_edge_was_added;
459 
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 )
463  return (false);
464 
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);
470 
471  return (true);
472 }
473 
474 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
475 template <typename PointT> double
476 pcl::MinCutSegmentation<PointT>::calculateBinaryPotential (int source, int target) const
477 {
478  double weight = 0.0;
479  double distance = 0.0;
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);
483  distance *= inverse_sigma_;
484  weight = exp (-distance);
485 
486  return (weight);
487 }
488 
489 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
490 template <typename PointT> bool
491 pcl::MinCutSegmentation<PointT>::recalculateUnaryPotentials ()
492 {
493  OutEdgeIterator src_edge_iter;
494  OutEdgeIterator src_edge_end;
495  std::pair<EdgeDescriptor, bool> sink_edge;
496 
497  for (boost::tie (src_edge_iter, src_edge_end) = boost::out_edges (source_, *graph_); src_edge_iter != src_edge_end; src_edge_iter++)
498  {
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)
505  return (false);
506 
507  (*capacity_)[*src_edge_iter] = source_weight;
508  (*capacity_)[sink_edge.first] = sink_weight;
509  }
510 
511  return (true);
512 }
513 
514 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
515 template <typename PointT> bool
516 pcl::MinCutSegmentation<PointT>::recalculateBinaryPotentials ()
517 {
518  int number_of_points = static_cast<int> (indices_->size ());
519 
520  VertexIterator vertex_iter;
521  VertexIterator vertex_end;
522  OutEdgeIterator edge_iter;
523  OutEdgeIterator edge_end;
524 
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);
529 
530  for (boost::tie (vertex_iter, vertex_end) = boost::vertices (*graph_); vertex_iter != vertex_end; vertex_iter++)
531  {
532  VertexDescriptor source_vertex = *vertex_iter;
533  if (source_vertex == source_ || source_vertex == sink_)
534  continue;
535  for (boost::tie (edge_iter, edge_end) = boost::out_edges (source_vertex, *graph_); edge_iter != edge_end; edge_iter++)
536  {
537  //If this is not the edge of the graph, but the reverse fictitious edge that is needed for the algorithm then continue
538  EdgeDescriptor reverse_edge = (*reverse_edges_)[*edge_iter];
539  if ((*capacity_)[reverse_edge] != 0.0)
540  continue;
541 
542  //If we already changed weight for this edge then continue
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 () )
546  continue;
547 
548  if (target_vertex != source_ && target_vertex != sink_)
549  {
550  //Change weight and remember that this edges were updated
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);
554  }
555  }
556  }
557 
558  return (true);
559 }
560 
561 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
562 template <typename PointT> void
563 pcl::MinCutSegmentation<PointT>::assembleLabels (ResidualCapacityMap& residual_capacity)
564 {
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;
570 
571  clusters_.clear ();
572 
573  pcl::PointIndices segment;
574  clusters_.resize (2, segment);
575 
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++ )
578  {
579  if (labels[edge_iter->m_target] == 1)
580  {
581  if (residual_capacity[*edge_iter] > epsilon_)
582  clusters_[1].indices.push_back (static_cast<int> (edge_iter->m_target));
583  else
584  clusters_[0].indices.push_back (static_cast<int> (edge_iter->m_target));
585  }
586  }
587 }
588 
589 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
590 template <typename PointT> pcl::PointCloud<pcl::PointXYZRGB>::Ptr
591 pcl::MinCutSegmentation<PointT>::getColoredCloud ()
592 {
594 
595  if (!clusters_.empty ())
596  {
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;
600  colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared ();
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;
606 
607  pcl::PointXYZRGB point;
608  int point_index = 0;
609  for (int i_point = 0; i_point < num_of_pts_in_first_cluster; i_point++)
610  {
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);
619  }
620 
621  for (int i_point = 0; i_point < num_of_pts_in_second_cluster; i_point++)
622  {
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);
631  }
632  }
633 
634  return (colored_cloud);
635 }
636 
637 #define PCL_INSTANTIATE_MinCutSegmentation(T) template class pcl::MinCutSegmentation<T>;
638 
639 #endif // PCL_SEGMENTATION_MIN_CUT_SEGMENTATION_HPP_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
std::vector< int > indices
Definition: PointIndices.h:19
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:61
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:418
A point structure representing Euclidean xyz coordinates, and the RGB color.