Point Cloud Library (PCL)  1.9.1-dev
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 (const int &index : indices->indices)
165  {
166  trimap_[index] = TrimapUnknown;
167  hard_segmentation_[index] = 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  for (const int &index : indices->indices)
249  trimap_[index] = t;
250 
251  // Immediately set the hard segmentation as well so that the display will update.
252  if (t == TrimapForeground)
253  for (const int &index : indices->indices)
254  hard_segmentation_[index] = SegmentationForeground;
255  else
256  if (t == TrimapBackground)
257  for (const int &index : indices->indices)
258  hard_segmentation_[index] = SegmentationBackground;
259 }
260 
261 template <typename PointT> void
263 {
264  using namespace pcl::segmentation::grabcut;
265  const int number_of_indices = static_cast<int> (indices_->size ());
266  // Set up the graph (it can only be used once, so we have to recreate it each time the graph is updated)
267  graph_.clear ();
268  graph_nodes_.clear ();
269  graph_nodes_.resize (indices_->size ());
270  int start = graph_.addNodes (indices_->size ());
271  for (size_t idx = 0; idx < indices_->size (); ++idx)
272  {
273  graph_nodes_[idx] = start;
274  ++start;
275  }
276 
277  // Set T-Link weights
278  for (int i_point = 0; i_point < number_of_indices; ++i_point)
279  {
280  int point_index = (*indices_) [i_point];
281  float back, fore;
282 
283  switch (trimap_[point_index])
284  {
285  case TrimapUnknown :
286  {
287  fore = static_cast<float> (-std::log (background_GMM_.probabilityDensity (image_->points[point_index])));
288  back = static_cast<float> (-std::log (foreground_GMM_.probabilityDensity (image_->points[point_index])));
289  break;
290  }
291  case TrimapBackground :
292  {
293  fore = 0;
294  back = L_;
295  break;
296  }
297  default :
298  {
299  fore = L_;
300  back = 0;
301  }
302  }
303 
304  setTerminalWeights (graph_nodes_[i_point], fore, back);
305  }
306 
307  // Set N-Link weights from precomputed values
308  for (int i_point = 0; i_point < number_of_indices; ++i_point)
309  {
310  const NLinks &n_link = n_links_[i_point];
311  if (n_link.nb_links > 0)
312  {
313  int point_index = (*indices_) [i_point];
314  std::vector<float>::const_iterator weights_it = n_link.weights.begin ();
315  for (auto indices_it = n_link.indices.cbegin (); indices_it != n_link.indices.cend (); ++indices_it, ++weights_it)
316  {
317  if ((*indices_it != point_index) && (*indices_it > -1))
318  {
319  addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it);
320  }
321  }
322  }
323  }
324 }
325 
326 template <typename PointT> void
328 {
329  const int number_of_indices = static_cast<int> (indices_->size ());
330  for (int i_point = 0; i_point < number_of_indices; ++i_point)
331  {
332  NLinks &n_link = n_links_[i_point];
333  if (n_link.nb_links > 0)
334  {
335  int point_index = (*indices_) [i_point];
336  auto dists_it = n_link.dists.cbegin ();
337  auto weights_it = n_link.weights.begin ();
338  for (auto indices_it = n_link.indices.cbegin (); indices_it != n_link.indices.cend (); ++indices_it, ++dists_it, ++weights_it)
339  {
340  if (*indices_it != point_index)
341  {
342  // We saved the color distance previously at the computeBeta stage for optimization purpose
343  float color_distance = *weights_it;
344  // Set the real weight
345  *weights_it = static_cast<float> (lambda_ * std::exp (-beta_ * color_distance) / sqrt (*dists_it));
346  }
347  }
348  }
349  }
350 }
351 
352 template <typename PointT> void
354 {
355  for( unsigned int y = 0; y < image_->height; ++y )
356  {
357  for( unsigned int x = 0; x < image_->width; ++x )
358  {
359  // We saved the color and euclidean distance previously at the computeBeta stage for
360  // optimization purpose but here we compute the real weight
361  std::size_t point_index = y * input_->width + x;
362  NLinks &links = n_links_[point_index];
363 
364  if( x > 0 && y < image_->height-1 )
365  links.weights[0] = lambda_ * std::exp (-beta_ * links.weights[0]) / links.dists[0];
366 
367  if( y < image_->height-1 )
368  links.weights[1] = lambda_ * std::exp (-beta_ * links.weights[1]) / links.dists[1];
369 
370  if( x < image_->width-1 && y < image_->height-1 )
371  links.weights[2] = lambda_ * std::exp (-beta_ * links.weights[2]) / links.dists[2];
372 
373  if( x < image_->width-1 )
374  links.weights[3] = lambda_ * std::exp (-beta_ * links.weights[3]) / links.dists[3];
375  }
376  }
377 }
378 
379 template <typename PointT> void
381 {
382  float result = 0;
383  std::size_t edges = 0;
384 
385  const int number_of_indices = static_cast<int> (indices_->size ());
386 
387  for (int i_point = 0; i_point < number_of_indices; i_point++)
388  {
389  int point_index = (*indices_)[i_point];
390  const PointT& point = input_->points [point_index];
391  if (pcl::isFinite (point))
392  {
393  NLinks &links = n_links_[i_point];
394  int found = tree_->nearestKSearch (point, nb_neighbours_, links.indices, links.dists);
395  if (found > 1)
396  {
397  links.nb_links = found - 1;
398  links.weights.reserve (links.nb_links);
399  for (std::vector<int>::const_iterator nn_it = links.indices.begin (); nn_it != links.indices.end (); ++nn_it)
400  {
401  if (*nn_it != point_index)
402  {
403  float color_distance = squaredEuclideanDistance (image_->points[point_index], image_->points[*nn_it]);
404  links.weights.push_back (color_distance);
405  result+= color_distance;
406  ++edges;
407  }
408  else
409  links.weights.push_back (0.f);
410  }
411  }
412  }
413  }
414 
415  beta_ = 1e5 / (2*result / edges);
416 }
417 
418 template <typename PointT> void
420 {
421  float result = 0;
422  std::size_t edges = 0;
423 
424  for (unsigned int y = 0; y < input_->height; ++y)
425  {
426  for (unsigned int x = 0; x < input_->width; ++x)
427  {
428  std::size_t point_index = y * input_->width + x;
429  NLinks &links = n_links_[point_index];
430  links.nb_links = 4;
431  links.weights.resize (links.nb_links, 0);
432  links.dists.resize (links.nb_links, 0);
433  links.indices.resize (links.nb_links, -1);
434 
435  if (x > 0 && y < input_->height-1)
436  {
437  std::size_t upleft = (y+1) * input_->width + x - 1;
438  links.indices[0] = upleft;
439  links.dists[0] = std::sqrt (2.f);
440  float color_dist = squaredEuclideanDistance (image_->points[point_index],
441  image_->points[upleft]);
442  links.weights[0] = color_dist;
443  result+= color_dist;
444  edges++;
445  }
446 
447  if (y < input_->height-1)
448  {
449  std::size_t up = (y+1) * input_->width + x;
450  links.indices[1] = up;
451  links.dists[1] = 1;
452  float color_dist = squaredEuclideanDistance (image_->points[point_index],
453  image_->points[up]);
454  links.weights[1] = color_dist;
455  result+= color_dist;
456  edges++;
457  }
458 
459  if (x < input_->width-1 && y < input_->height-1)
460  {
461  std::size_t upright = (y+1) * input_->width + x + 1;
462  links.indices[2] = upright;
463  links.dists[2] = std::sqrt (2.f);
464  float color_dist = squaredEuclideanDistance (image_->points[point_index],
465  image_->points [upright]);
466  links.weights[2] = color_dist;
467  result+= color_dist;
468  edges++;
469  }
470 
471  if (x < input_->width-1)
472  {
473  std::size_t right = y * input_->width + x + 1;
474  links.indices[3] = right;
475  links.dists[3] = 1;
476  float color_dist = squaredEuclideanDistance (image_->points[point_index],
477  image_->points[right]);
478  links.weights[3] = color_dist;
479  result+= color_dist;
480  edges++;
481  }
482  }
483  }
484 
485  beta_ = 1e5 / (2*result / edges);
486 }
487 
488 template <typename PointT> void
490 {
491  L_ = 8*lambda_ + 1;
492 }
493 
494 template <typename PointT> void
495 pcl::GrabCut<PointT>::extract (std::vector<pcl::PointIndices>& clusters)
496 {
497  using namespace pcl::segmentation::grabcut;
498  clusters.clear ();
499  clusters.resize (2);
500  clusters[0].indices.reserve (indices_->size ());
501  clusters[1].indices.reserve (indices_->size ());
502  refine ();
503  assert (hard_segmentation_.size () == indices_->size ());
504  const int indices_size = static_cast<int> (indices_->size ());
505  for (int i = 0; i < indices_size; ++i)
506  if (hard_segmentation_[i] == SegmentationForeground)
507  clusters[1].indices.push_back (i);
508  else
509  clusters[0].indices.push_back (i);
510 }
511 
512 #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:53
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:44
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
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. ...
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
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.