Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
grabcut_segmentation.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 
38 #ifndef PCL_SEGMENTATION_IMPL_GRABCUT_HPP
39 #define PCL_SEGMENTATION_IMPL_GRABCUT_HPP
40 
41 #include <pcl/search/organized.h>
42 #include <pcl/search/kdtree.h>
43 #include <pcl/common/distances.h>
44 
45 namespace pcl
46 {
47  template <>
50  {
51  return ((c1.r-c2.r)*(c1.r-c2.r)+(c1.g-c2.g)*(c1.g-c2.g)+(c1.b-c2.b)*(c1.b-c2.b));
52  }
53 }
54 
55 template <typename PointT>
57 {
58  r = static_cast<float> (p.r) / 255.0;
59  g = static_cast<float> (p.g) / 255.0;
60  b = static_cast<float> (p.b) / 255.0;
61 }
62 
63 template <typename PointT>
64 pcl::segmentation::grabcut::Color::operator PointT () const
65 {
66  PointT p;
67  p.r = static_cast<uint32_t> (r * 255);
68  p.g = static_cast<uint32_t> (g * 255);
69  p.b = static_cast<uint32_t> (b * 255);
70  return (p);
71 }
72 
73 template <typename PointT> void
75 {
76  input_ = cloud;
77 }
78 
79 template <typename PointT> bool
81 {
82  using namespace pcl::segmentation::grabcut;
84  {
85  PCL_ERROR ("[pcl::GrabCut::initCompute ()] Init failed!");
86  return (false);
87  }
88 
89  std::vector<pcl::PCLPointField> in_fields_;
90  if ((pcl::getFieldIndex<PointT> (*input_, "rgb", in_fields_) == -1) &&
91  (pcl::getFieldIndex<PointT> (*input_, "rgba", in_fields_) == -1))
92  {
93  PCL_ERROR ("[pcl::GrabCut::initCompute ()] No RGB data available, aborting!");
94  return (false);
95  }
96 
97  // Initialize the working image
98  image_.reset (new Image (input_->width, input_->height));
99  for (std::size_t i = 0; i < input_->size (); ++i)
100  {
101  (*image_) [i] = Color (input_->points[i]);
102  }
103  width_ = image_->width;
104  height_ = image_->height;
105 
106  // Initialize the spatial locator
107  if (!tree_ && !input_->isOrganized ())
108  {
109  tree_.reset (new pcl::search::KdTree<PointT> (true));
110  tree_->setInputCloud (input_);
111  }
112 
113  const std::size_t indices_size = indices_->size ();
114  trimap_ = std::vector<segmentation::grabcut::TrimapValue> (indices_size, TrimapUnknown);
115  hard_segmentation_ = std::vector<segmentation::grabcut::SegmentationValue> (indices_size,
117  GMM_component_.resize (indices_size);
118  n_links_.resize (indices_size);
119 
120  // soft_segmentation_ = 0; // Not yet implemented
121  foreground_GMM_.resize (K_);
122  background_GMM_.resize (K_);
123 
124  //set some constants
125  computeL ();
126 
127  if (image_->isOrganized ())
128  {
129  computeBetaOrganized ();
130  computeNLinksOrganized ();
131  }
132  else
133  {
134  computeBetaNonOrganized ();
135  computeNLinksNonOrganized ();
136  }
137 
138  initialized_ = false;
139  return (true);
140 }
141 
142 template <typename PointT> void
143 pcl::GrabCut<PointT>::addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity)
144 {
145  graph_.addEdge (v1, v2, capacity, rev_capacity);
146 }
147 
148 template <typename PointT> void
149 pcl::GrabCut<PointT>::setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity)
150 {
151  graph_.addSourceEdge (v, source_capacity);
152  graph_.addTargetEdge (v, sink_capacity);
153 }
154 
155 template <typename PointT> void
157 {
158  using namespace pcl::segmentation::grabcut;
159  if (!initCompute ())
160  return;
161 
162  std::fill (trimap_.begin (), trimap_.end (), TrimapBackground);
163  std::fill (hard_segmentation_.begin (), hard_segmentation_.end (), SegmentationBackground);
164  for (std::vector<int>::const_iterator idx = indices->indices.begin (); idx != indices->indices.end (); ++idx)
165  {
166  trimap_[*idx] = TrimapUnknown;
167  hard_segmentation_[*idx] = SegmentationForeground;
168  }
169 
170  if (!initialized_)
171  {
172  fitGMMs ();
173  initialized_ = true;
174  }
175 }
176 
177 template <typename PointT> void
179 {
180  // Step 3: Build GMMs using Orchard-Bouman clustering algorithm
181  buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
182 
183  // Initialize the graph for graphcut (do this here so that the T-Link debugging image will be initialized)
184  initGraph ();
185 }
186 
187 template <typename PointT> int
189 {
190  // Steps 4 and 5: Learn new GMMs from current segmentation
191  learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
192 
193  // Step 6: Run GraphCut and update segmentation
194  initGraph ();
195 
196  float flow = graph_.solve ();
197 
198  int changed = updateHardSegmentation ();
199  PCL_INFO ("%d pixels changed segmentation (max flow = %f)\n", changed, flow);
200 
201  return (changed);
202 }
203 
204 template <typename PointT> void
206 {
207  std::size_t changed = indices_->size ();
208 
209  while (changed)
210  changed = refineOnce ();
211 }
212 
213 template <typename PointT> int
215 {
216  using namespace pcl::segmentation::grabcut;
217 
218  int changed = 0;
219 
220  const int number_of_indices = static_cast<int> (indices_->size ());
221  for (int i_point = 0; i_point < number_of_indices; ++i_point)
222  {
223  SegmentationValue old_value = hard_segmentation_ [i_point];
224 
225  if (trimap_ [i_point] == TrimapBackground)
226  hard_segmentation_ [i_point] = SegmentationBackground;
227  else
228  if (trimap_ [i_point] == TrimapForeground)
229  hard_segmentation_ [i_point] = SegmentationForeground;
230  else // TrimapUnknown
231  {
232  if (isSource (graph_nodes_[i_point]))
233  hard_segmentation_ [i_point] = SegmentationForeground;
234  else
235  hard_segmentation_ [i_point] = SegmentationBackground;
236  }
237 
238  if (old_value != hard_segmentation_ [i_point])
239  ++changed;
240  }
241  return (changed);
242 }
243 
244 template <typename PointT> void
246 {
247  using namespace pcl::segmentation::grabcut;
248  std::vector<int>::const_iterator idx = indices->indices.begin ();
249  for (; idx != indices->indices.end (); ++idx)
250  trimap_[*idx] = t;
251 
252  // Immediately set the hard segmentation as well so that the display will update.
253  if (t == TrimapForeground)
254  for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx)
255  hard_segmentation_[*idx] = SegmentationForeground;
256  else
257  if (t == TrimapBackground)
258  for (idx = indices->indices.begin (); idx != indices->indices.end (); ++idx)
259  hard_segmentation_[*idx] = SegmentationBackground;
260 }
261 
262 template <typename PointT> void
264 {
265  using namespace pcl::segmentation::grabcut;
266  const int number_of_indices = static_cast<int> (indices_->size ());
267  // Set up the graph (it can only be used once, so we have to recreate it each time the graph is updated)
268  graph_.clear ();
269  graph_nodes_.clear ();
270  graph_nodes_.resize (indices_->size ());
271  int start = graph_.addNodes (indices_->size ());
272  for (size_t idx = 0; idx < indices_->size (); ++idx)
273  {
274  graph_nodes_[idx] = start;
275  ++start;
276  }
277 
278  // Set T-Link weights
279  for (int i_point = 0; i_point < number_of_indices; ++i_point)
280  {
281  int point_index = (*indices_) [i_point];
282  float back, fore;
283 
284  switch (trimap_[point_index])
285  {
286  case TrimapUnknown :
287  {
288  fore = static_cast<float> (-log (background_GMM_.probabilityDensity (image_->points[point_index])));
289  back = static_cast<float> (-log (foreground_GMM_.probabilityDensity (image_->points[point_index])));
290  break;
291  }
292  case TrimapBackground :
293  {
294  fore = 0;
295  back = L_;
296  break;
297  }
298  default :
299  {
300  fore = L_;
301  back = 0;
302  }
303  }
304 
305  setTerminalWeights (graph_nodes_[i_point], fore, back);
306  }
307 
308  // Set N-Link weights from precomputed values
309  for (int i_point = 0; i_point < number_of_indices; ++i_point)
310  {
311  const NLinks &n_link = n_links_[i_point];
312  if (n_link.nb_links > 0)
313  {
314  int point_index = (*indices_) [i_point];
315  std::vector<int>::const_iterator indices_it = n_link.indices.begin ();
316  std::vector<float>::const_iterator weights_it = n_link.weights.begin ();
317  for (; indices_it != n_link.indices.end (); ++indices_it, ++weights_it)
318  {
319  if ((*indices_it != point_index) && (*indices_it > -1))
320  {
321  addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it);
322  }
323  }
324  }
325  }
326 }
327 
328 template <typename PointT> void
330 {
331  const int number_of_indices = static_cast<int> (indices_->size ());
332  for (int i_point = 0; i_point < number_of_indices; ++i_point)
333  {
334  NLinks &n_link = n_links_[i_point];
335  if (n_link.nb_links > 0)
336  {
337  int point_index = (*indices_) [i_point];
338  std::vector<int>::const_iterator indices_it = n_link.indices.begin ();
339  std::vector<float>::const_iterator dists_it = n_link.dists.begin ();
340  std::vector<float>::iterator weights_it = n_link.weights.begin ();
341  for (; indices_it != n_link.indices.end (); ++indices_it, ++dists_it, ++weights_it)
342  {
343  if (*indices_it != point_index)
344  {
345  // We saved the color distance previously at the computeBeta stage for optimization purpose
346  float color_distance = *weights_it;
347  // Set the real weight
348  *weights_it = static_cast<float> (lambda_ * exp (-beta_ * color_distance) / sqrt (*dists_it));
349  }
350  }
351  }
352  }
353 }
354 
355 template <typename PointT> void
357 {
358  for( unsigned int y = 0; y < image_->height; ++y )
359  {
360  for( unsigned int x = 0; x < image_->width; ++x )
361  {
362  // We saved the color and euclidean distance previously at the computeBeta stage for
363  // optimization purpose but here we compute the real weight
364  std::size_t point_index = y * input_->width + x;
365  NLinks &links = n_links_[point_index];
366 
367  if( x > 0 && y < image_->height-1 )
368  links.weights[0] = lambda_ * exp (-beta_ * links.weights[0]) / links.dists[0];
369 
370  if( y < image_->height-1 )
371  links.weights[1] = lambda_ * exp (-beta_ * links.weights[1]) / links.dists[1];
372 
373  if( x < image_->width-1 && y < image_->height-1 )
374  links.weights[2] = lambda_ * exp (-beta_ * links.weights[2]) / links.dists[2];
375 
376  if( x < image_->width-1 )
377  links.weights[3] = lambda_ * exp (-beta_ * links.weights[3]) / links.dists[3];
378  }
379  }
380 }
381 
382 template <typename PointT> void
384 {
385  float result = 0;
386  std::size_t edges = 0;
387 
388  const int number_of_indices = static_cast<int> (indices_->size ());
389 
390  for (int i_point = 0; i_point < number_of_indices; i_point++)
391  {
392  int point_index = (*indices_)[i_point];
393  const PointT& point = input_->points [point_index];
394  if (pcl::isFinite (point))
395  {
396  NLinks &links = n_links_[i_point];
397  int found = tree_->nearestKSearch (point, nb_neighbours_, links.indices, links.dists);
398  if (found > 1)
399  {
400  links.nb_links = found - 1;
401  links.weights.reserve (links.nb_links);
402  for (std::vector<int>::const_iterator nn_it = links.indices.begin (); nn_it != links.indices.end (); ++nn_it)
403  {
404  if (*nn_it != point_index)
405  {
406  float color_distance = squaredEuclideanDistance (image_->points[point_index], image_->points[*nn_it]);
407  links.weights.push_back (color_distance);
408  result+= color_distance;
409  ++edges;
410  }
411  else
412  links.weights.push_back (0.f);
413  }
414  }
415  }
416  }
417 
418  beta_ = 1e5 / (2*result / edges);
419 }
420 
421 template <typename PointT> void
423 {
424  float result = 0;
425  std::size_t edges = 0;
426 
427  for (unsigned int y = 0; y < input_->height; ++y)
428  {
429  for (unsigned int x = 0; x < input_->width; ++x)
430  {
431  std::size_t point_index = y * input_->width + x;
432  NLinks &links = n_links_[point_index];
433  links.nb_links = 4;
434  links.weights.resize (links.nb_links, 0);
435  links.dists.resize (links.nb_links, 0);
436  links.indices.resize (links.nb_links, -1);
437 
438  if (x > 0 && y < input_->height-1)
439  {
440  std::size_t upleft = (y+1) * input_->width + x - 1;
441  links.indices[0] = upleft;
442  links.dists[0] = sqrt (2.f);
443  float color_dist = squaredEuclideanDistance (image_->points[point_index],
444  image_->points[upleft]);
445  links.weights[0] = color_dist;
446  result+= color_dist;
447  edges++;
448  }
449 
450  if (y < input_->height-1)
451  {
452  std::size_t up = (y+1) * input_->width + x;
453  links.indices[1] = up;
454  links.dists[1] = 1;
455  float color_dist = squaredEuclideanDistance (image_->points[point_index],
456  image_->points[up]);
457  links.weights[1] = color_dist;
458  result+= color_dist;
459  edges++;
460  }
461 
462  if (x < input_->width-1 && y < input_->height-1)
463  {
464  std::size_t upright = (y+1) * input_->width + x + 1;
465  links.indices[2] = upright;
466  links.dists[2] = sqrt (2.f);
467  float color_dist = squaredEuclideanDistance (image_->points[point_index],
468  image_->points [upright]);
469  links.weights[2] = color_dist;
470  result+= color_dist;
471  edges++;
472  }
473 
474  if (x < input_->width-1)
475  {
476  std::size_t right = y * input_->width + x + 1;
477  links.indices[3] = right;
478  links.dists[3] = 1;
479  float color_dist = squaredEuclideanDistance (image_->points[point_index],
480  image_->points[right]);
481  links.weights[3] = color_dist;
482  result+= color_dist;
483  edges++;
484  }
485  }
486  }
487 
488  beta_ = 1e5 / (2*result / edges);
489 }
490 
491 template <typename PointT> void
493 {
494  L_ = 8*lambda_ + 1;
495 }
496 
497 template <typename PointT> void
498 pcl::GrabCut<PointT>::extract (std::vector<pcl::PointIndices>& clusters)
499 {
500  using namespace pcl::segmentation::grabcut;
501  clusters.clear ();
502  clusters.resize (2);
503  clusters[0].indices.reserve (indices_->size ());
504  clusters[1].indices.reserve (indices_->size ());
505  refine ();
506  assert (hard_segmentation_.size () == indices_->size ());
507  const int indices_size = static_cast<int> (indices_->size ());
508  for (int i = 0; i < indices_size; ++i)
509  if (hard_segmentation_[i] == SegmentationForeground)
510  clusters[1].indices.push_back (i);
511  else
512  clusters[0].indices.push_back (i);
513 }
514 
515 #endif
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:54
virtual void fitGMMs()
Fit Gaussian Multi Models.
PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:73
void setTrimap(const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t)
Edit Trimap.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
void computeNLinksOrganized()
Compute NLinks from image.
void initGraph()
Build the graph for GraphCut.
void addEdge(vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity)
Add an edge to the graph, graph must be oriented so we add the edge and its reverse.
void computeBetaNonOrganized()
Compute beta from cloud.
void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition: distances.h:174
void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
SegmentationValue
Grabcut derived hard segmentation values.
boost::shared_ptr< PointIndices const > PointIndicesConstPtr
Definition: pcl_base.h:76
PCL base class.
Definition: pcl_base.h:68
void computeBetaOrganized()
Compute beta from image.
pcl::segmentation::grabcut::BoykovKolmogorov::vertex_descriptor vertex_descriptor
PCL_EXPORTS void learnGMMs(const Image &image, const std::vector< int > &indices, const std::vector< SegmentationValue > &hard_segmentation, std::vector< std::size_t > &components, GMM &background_GMM, GMM &foreground_GMM)
Iteratively learn GMMs using GrabCut updating algorithm.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual int refineOnce()
void computeL()
Compute L parameter from given lambda.
PCL_EXPORTS void buildGMMs(const Image &image, const std::vector< int > &indices, const std::vector< SegmentationValue > &hardSegmentation, std::vector< std::size_t > &components, GMM &background_GMM, GMM &foreground_GMM)
Build the initial GMMs using the Orchard and Bouman color clustering algorithm.
Structure to save RGB colors into floats.
A point structure representing Euclidean xyz coordinates, and the RGB color.
TrimapValue
User supplied Trimap values.
void setTerminalWeights(vertex_descriptor v, float source_capacity, float sink_capacity)
Set the weights of SOURCE –> v and v –> SINK.
virtual void refine()
Run Grabcut refinement on the hard segmentation.
void computeNLinksNonOrganized()
Compute NLinks from cloud.
void setBackgroundPointsIndices(int x1, int y1, int x2, int y2)
Set background indices, foreground indices = indices \ background indices.