Point Cloud Library (PCL)  1.11.0
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_->points[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_->points[point_index])));
294  back = static_cast<float> (-std::log (foreground_GMM_.probabilityDensity (image_->points[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_->points[point_index], image_->points[*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_->points[point_index],
447  image_->points[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_->points[point_index],
459  image_->points[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_->points[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_->points[point_index],
483  image_->points[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 
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
virtual void fitGMMs()
Fit Gaussian Multi Models.
void setTrimap(const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t)
Edit Trimap.
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:178
SegmentationValue
Grabcut derived hard segmentation values.
PCL base class.
Definition: pcl_base.h:69
void computeBetaOrganized()
Compute beta from image.
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.
PointIndices::ConstPtr PointIndicesConstPtr
Definition: pcl_base.h:77
void computeNLinksNonOrganized()
Compute NLinks from cloud.
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
void setBackgroundPointsIndices(int x1, int y1, int x2, int y2)
Set background indices, foreground indices = indices \ background indices.
pcl::segmentation::grabcut::BoykovKolmogorov::vertex_descriptor vertex_descriptor