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