Point Cloud Library (PCL)  1.11.1
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 #pragma once
39 
40 #include <pcl/common/distances.h>
41 #include <pcl/common/point_tests.h> // for pcl::isFinite
42 #include <pcl/search/organized.h>
43 #include <pcl/search/kdtree.h>
44 
45 
46 namespace pcl
47 {
48 
49 template <>
52 {
53  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));
54 }
55 
56 namespace segmentation
57 {
58 
59 template <typename PointT>
61 {
62  r = static_cast<float> (p.r) / 255.0;
63  g = static_cast<float> (p.g) / 255.0;
64  b = static_cast<float> (p.b) / 255.0;
65 }
66 
67 template <typename PointT>
68 grabcut::Color::operator PointT () const
69 {
70  PointT p;
71  p.r = static_cast<std::uint32_t> (r * 255);
72  p.g = static_cast<std::uint32_t> (g * 255);
73  p.b = static_cast<std::uint32_t> (b * 255);
74  return (p);
75 }
76 
77 } // namespace segmentation
78 
79 template <typename PointT> void
81 {
82  input_ = cloud;
83 }
84 
85 template <typename PointT> bool
87 {
88  using namespace pcl::segmentation::grabcut;
90  {
91  PCL_ERROR ("[pcl::GrabCut::initCompute ()] Init failed!");
92  return (false);
93  }
94 
95  std::vector<pcl::PCLPointField> in_fields_;
96  if ((pcl::getFieldIndex<PointT> ("rgb", in_fields_) == -1) &&
97  (pcl::getFieldIndex<PointT> ("rgba", in_fields_) == -1))
98  {
99  PCL_ERROR ("[pcl::GrabCut::initCompute ()] No RGB data available, aborting!");
100  return (false);
101  }
102 
103  // Initialize the working image
104  image_.reset (new Image (input_->width, input_->height));
105  for (std::size_t i = 0; i < input_->size (); ++i)
106  {
107  (*image_) [i] = Color ((*input_)[i]);
108  }
109  width_ = image_->width;
110  height_ = image_->height;
111 
112  // Initialize the spatial locator
113  if (!tree_ && !input_->isOrganized ())
114  {
115  tree_.reset (new pcl::search::KdTree<PointT> (true));
116  tree_->setInputCloud (input_);
117  }
118 
119  const std::size_t indices_size = indices_->size ();
120  trimap_ = std::vector<segmentation::grabcut::TrimapValue> (indices_size, TrimapUnknown);
121  hard_segmentation_ = std::vector<segmentation::grabcut::SegmentationValue> (indices_size,
123  GMM_component_.resize (indices_size);
124  n_links_.resize (indices_size);
125 
126  // soft_segmentation_ = 0; // Not yet implemented
127  foreground_GMM_.resize (K_);
128  background_GMM_.resize (K_);
129 
130  //set some constants
131  computeL ();
132 
133  if (image_->isOrganized ())
134  {
135  computeBetaOrganized ();
136  computeNLinksOrganized ();
137  }
138  else
139  {
140  computeBetaNonOrganized ();
141  computeNLinksNonOrganized ();
142  }
143 
144  initialized_ = false;
145  return (true);
146 }
147 
148 template <typename PointT> void
149 GrabCut<PointT>::addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity)
150 {
151  graph_.addEdge (v1, v2, capacity, rev_capacity);
152 }
153 
154 template <typename PointT> void
155 GrabCut<PointT>::setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity)
156 {
157  graph_.addSourceEdge (v, source_capacity);
158  graph_.addTargetEdge (v, sink_capacity);
159 }
160 
161 template <typename PointT> void
163 {
164  using namespace pcl::segmentation::grabcut;
165  if (!initCompute ())
166  return;
167 
168  std::fill (trimap_.begin (), trimap_.end (), TrimapBackground);
169  std::fill (hard_segmentation_.begin (), hard_segmentation_.end (), SegmentationBackground);
170  for (const int &index : indices->indices)
171  {
172  trimap_[index] = TrimapUnknown;
173  hard_segmentation_[index] = SegmentationForeground;
174  }
175 
176  if (!initialized_)
177  {
178  fitGMMs ();
179  initialized_ = true;
180  }
181 }
182 
183 template <typename PointT> void
185 {
186  // Step 3: Build GMMs using Orchard-Bouman clustering algorithm
187  buildGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
188 
189  // Initialize the graph for graphcut (do this here so that the T-Link debugging image will be initialized)
190  initGraph ();
191 }
192 
193 template <typename PointT> int
195 {
196  // Steps 4 and 5: Learn new GMMs from current segmentation
197  learnGMMs (*image_, *indices_, hard_segmentation_, GMM_component_, background_GMM_, foreground_GMM_);
198 
199  // Step 6: Run GraphCut and update segmentation
200  initGraph ();
201 
202  float flow = graph_.solve ();
203 
204  int changed = updateHardSegmentation ();
205  PCL_INFO ("%d pixels changed segmentation (max flow = %f)\n", changed, flow);
206 
207  return (changed);
208 }
209 
210 template <typename PointT> void
212 {
213  std::size_t changed = indices_->size ();
214 
215  while (changed)
216  changed = refineOnce ();
217 }
218 
219 template <typename PointT> int
221 {
222  using namespace pcl::segmentation::grabcut;
223 
224  int changed = 0;
225 
226  const int number_of_indices = static_cast<int> (indices_->size ());
227  for (int i_point = 0; i_point < number_of_indices; ++i_point)
228  {
229  SegmentationValue old_value = hard_segmentation_ [i_point];
230 
231  if (trimap_ [i_point] == TrimapBackground)
232  hard_segmentation_ [i_point] = SegmentationBackground;
233  else
234  if (trimap_ [i_point] == TrimapForeground)
235  hard_segmentation_ [i_point] = SegmentationForeground;
236  else // TrimapUnknown
237  {
238  if (isSource (graph_nodes_[i_point]))
239  hard_segmentation_ [i_point] = SegmentationForeground;
240  else
241  hard_segmentation_ [i_point] = SegmentationBackground;
242  }
243 
244  if (old_value != hard_segmentation_ [i_point])
245  ++changed;
246  }
247  return (changed);
248 }
249 
250 template <typename PointT> void
252 {
253  using namespace pcl::segmentation::grabcut;
254  for (const int &index : indices->indices)
255  trimap_[index] = t;
256 
257  // Immediately set the hard segmentation as well so that the display will update.
258  if (t == TrimapForeground)
259  for (const int &index : indices->indices)
260  hard_segmentation_[index] = SegmentationForeground;
261  else
262  if (t == TrimapBackground)
263  for (const int &index : indices->indices)
264  hard_segmentation_[index] = SegmentationBackground;
265 }
266 
267 template <typename PointT> void
269 {
270  using namespace pcl::segmentation::grabcut;
271  const int number_of_indices = static_cast<int> (indices_->size ());
272  // Set up the graph (it can only be used once, so we have to recreate it each time the graph is updated)
273  graph_.clear ();
274  graph_nodes_.clear ();
275  graph_nodes_.resize (indices_->size ());
276  int start = graph_.addNodes (indices_->size ());
277  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
278  {
279  graph_nodes_[idx] = start;
280  ++start;
281  }
282 
283  // Set T-Link weights
284  for (int i_point = 0; i_point < number_of_indices; ++i_point)
285  {
286  int point_index = (*indices_) [i_point];
287  float back, fore;
288 
289  switch (trimap_[point_index])
290  {
291  case TrimapUnknown :
292  {
293  fore = static_cast<float> (-std::log (background_GMM_.probabilityDensity ((*image_)[point_index])));
294  back = static_cast<float> (-std::log (foreground_GMM_.probabilityDensity ((*image_)[point_index])));
295  break;
296  }
297  case TrimapBackground :
298  {
299  fore = 0;
300  back = L_;
301  break;
302  }
303  default :
304  {
305  fore = L_;
306  back = 0;
307  }
308  }
309 
310  setTerminalWeights (graph_nodes_[i_point], fore, back);
311  }
312 
313  // Set N-Link weights from precomputed values
314  for (int i_point = 0; i_point < number_of_indices; ++i_point)
315  {
316  const NLinks &n_link = n_links_[i_point];
317  if (n_link.nb_links > 0)
318  {
319  int point_index = (*indices_) [i_point];
320  std::vector<float>::const_iterator weights_it = n_link.weights.begin ();
321  for (auto indices_it = n_link.indices.cbegin (); indices_it != n_link.indices.cend (); ++indices_it, ++weights_it)
322  {
323  if ((*indices_it != point_index) && (*indices_it > -1))
324  {
325  addEdge (graph_nodes_[i_point], graph_nodes_[*indices_it], *weights_it, *weights_it);
326  }
327  }
328  }
329  }
330 }
331 
332 template <typename PointT> void
334 {
335  const int number_of_indices = static_cast<int> (indices_->size ());
336  for (int i_point = 0; i_point < number_of_indices; ++i_point)
337  {
338  NLinks &n_link = n_links_[i_point];
339  if (n_link.nb_links > 0)
340  {
341  int point_index = (*indices_) [i_point];
342  auto dists_it = n_link.dists.cbegin ();
343  auto weights_it = n_link.weights.begin ();
344  for (auto indices_it = n_link.indices.cbegin (); indices_it != n_link.indices.cend (); ++indices_it, ++dists_it, ++weights_it)
345  {
346  if (*indices_it != point_index)
347  {
348  // We saved the color distance previously at the computeBeta stage for optimization purpose
349  float color_distance = *weights_it;
350  // Set the real weight
351  *weights_it = static_cast<float> (lambda_ * std::exp (-beta_ * color_distance) / sqrt (*dists_it));
352  }
353  }
354  }
355  }
356 }
357 
358 template <typename PointT> void
360 {
361  for( unsigned int y = 0; y < image_->height; ++y )
362  {
363  for( unsigned int x = 0; x < image_->width; ++x )
364  {
365  // We saved the color and euclidean distance previously at the computeBeta stage for
366  // optimization purpose but here we compute the real weight
367  std::size_t point_index = y * input_->width + x;
368  NLinks &links = n_links_[point_index];
369 
370  if( x > 0 && y < image_->height-1 )
371  links.weights[0] = lambda_ * std::exp (-beta_ * links.weights[0]) / links.dists[0];
372 
373  if( y < image_->height-1 )
374  links.weights[1] = lambda_ * std::exp (-beta_ * links.weights[1]) / links.dists[1];
375 
376  if( x < image_->width-1 && y < image_->height-1 )
377  links.weights[2] = lambda_ * std::exp (-beta_ * links.weights[2]) / links.dists[2];
378 
379  if( x < image_->width-1 )
380  links.weights[3] = lambda_ * std::exp (-beta_ * links.weights[3]) / links.dists[3];
381  }
382  }
383 }
384 
385 template <typename PointT> void
387 {
388  float result = 0;
389  std::size_t edges = 0;
390 
391  const int number_of_indices = static_cast<int> (indices_->size ());
392 
393  for (int i_point = 0; i_point < number_of_indices; i_point++)
394  {
395  int point_index = (*indices_)[i_point];
396  const PointT& point = input_->points [point_index];
397  if (pcl::isFinite (point))
398  {
399  NLinks &links = n_links_[i_point];
400  int found = tree_->nearestKSearch (point, nb_neighbours_, links.indices, links.dists);
401  if (found > 1)
402  {
403  links.nb_links = found - 1;
404  links.weights.reserve (links.nb_links);
405  for (std::vector<int>::const_iterator nn_it = links.indices.begin (); nn_it != links.indices.end (); ++nn_it)
406  {
407  if (*nn_it != point_index)
408  {
409  float color_distance = squaredEuclideanDistance ((*image_)[point_index], (*image_)[*nn_it]);
410  links.weights.push_back (color_distance);
411  result+= color_distance;
412  ++edges;
413  }
414  else
415  links.weights.push_back (0.f);
416  }
417  }
418  }
419  }
420 
421  beta_ = 1e5 / (2*result / edges);
422 }
423 
424 template <typename PointT> void
426 {
427  float result = 0;
428  std::size_t edges = 0;
429 
430  for (unsigned int y = 0; y < input_->height; ++y)
431  {
432  for (unsigned int x = 0; x < input_->width; ++x)
433  {
434  std::size_t point_index = y * input_->width + x;
435  NLinks &links = n_links_[point_index];
436  links.nb_links = 4;
437  links.weights.resize (links.nb_links, 0);
438  links.dists.resize (links.nb_links, 0);
439  links.indices.resize (links.nb_links, -1);
440 
441  if (x > 0 && y < input_->height-1)
442  {
443  std::size_t upleft = (y+1) * input_->width + x - 1;
444  links.indices[0] = upleft;
445  links.dists[0] = std::sqrt (2.f);
446  float color_dist = squaredEuclideanDistance ((*image_)[point_index],
447  (*image_)[upleft]);
448  links.weights[0] = color_dist;
449  result+= color_dist;
450  edges++;
451  }
452 
453  if (y < input_->height-1)
454  {
455  std::size_t up = (y+1) * input_->width + x;
456  links.indices[1] = up;
457  links.dists[1] = 1;
458  float color_dist = squaredEuclideanDistance ((*image_)[point_index],
459  (*image_)[up]);
460  links.weights[1] = color_dist;
461  result+= color_dist;
462  edges++;
463  }
464 
465  if (x < input_->width-1 && y < input_->height-1)
466  {
467  std::size_t upright = (y+1) * input_->width + x + 1;
468  links.indices[2] = upright;
469  links.dists[2] = std::sqrt (2.f);
470  float color_dist = squaredEuclideanDistance ((*image_)[point_index],
471  image_->points [upright]);
472  links.weights[2] = color_dist;
473  result+= color_dist;
474  edges++;
475  }
476 
477  if (x < input_->width-1)
478  {
479  std::size_t right = y * input_->width + x + 1;
480  links.indices[3] = right;
481  links.dists[3] = 1;
482  float color_dist = squaredEuclideanDistance ((*image_)[point_index],
483  (*image_)[right]);
484  links.weights[3] = color_dist;
485  result+= color_dist;
486  edges++;
487  }
488  }
489  }
490 
491  beta_ = 1e5 / (2*result / edges);
492 }
493 
494 template <typename PointT> void
496 {
497  L_ = 8*lambda_ + 1;
498 }
499 
500 template <typename PointT> void
501 GrabCut<PointT>::extract (std::vector<pcl::PointIndices>& clusters)
502 {
503  using namespace pcl::segmentation::grabcut;
504  clusters.clear ();
505  clusters.resize (2);
506  clusters[0].indices.reserve (indices_->size ());
507  clusters[1].indices.reserve (indices_->size ());
508  refine ();
509  assert (hard_segmentation_.size () == indices_->size ());
510  const int indices_size = static_cast<int> (indices_->size ());
511  for (int i = 0; i < indices_size; ++i)
512  if (hard_segmentation_[i] == SegmentationForeground)
513  clusters[1].indices.push_back (i);
514  else
515  clusters[0].indices.push_back (i);
516 }
517 
518 } // namespace pcl
519 
void computeL()
Compute L parameter from given lambda.
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.
virtual void fitGMMs()
Fit Gaussian Multi Models.
void computeNLinksNonOrganized()
Compute NLinks from cloud.
virtual void refine()
Run Grabcut refinement on the hard segmentation.
void setTrimap(const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t)
Edit Trimap.
pcl::segmentation::grabcut::BoykovKolmogorov::vertex_descriptor vertex_descriptor
void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
void initGraph()
Build the graph for GraphCut.
void computeBetaOrganized()
Compute beta from image.
void computeNLinksOrganized()
Compute NLinks from image.
void setTerminalWeights(vertex_descriptor v, float source_capacity, float sink_capacity)
Set the weights of SOURCE --> v and v --> SINK.
virtual int refineOnce()
void setBackgroundPointsIndices(int x1, int y1, int x2, int y2)
Set background indices, foreground indices = indices \ background indices.
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
void computeBetaNonOrganized()
Compute beta from cloud.
PCL base class.
Definition: pcl_base.h:73
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:77
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_base.h:80
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:181
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition: kdtree.h:62
Define standard C methods to do distance calculations.
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.
TrimapValue
User supplied Trimap values.
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.
SegmentationValue
Grabcut derived hard segmentation values.
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition: distances.h:182
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:55
std::uint32_t uint32_t
Definition: types.h:58
A point structure representing Euclidean xyz coordinates, and the RGB color.
Structure to save RGB colors into floats.