Point Cloud Library (PCL)  1.9.1-dev
grabcut_segmentation.h
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  * $Id$
37  *
38  */
39 
40 #pragma once
41 
42 #include <pcl/point_cloud.h>
43 #include <pcl/pcl_base.h>
44 #include <pcl/pcl_macros.h>
45 #include <pcl/point_types.h>
46 #include <pcl/segmentation/boost.h>
47 #include <pcl/search/search.h>
48 
49 namespace pcl
50 {
51  namespace segmentation
52  {
53  namespace grabcut
54  {
55  /** boost implementation of Boykov and Kolmogorov's maxflow algorithm doesn't support
56  * negative flows which makes it inappropriate for this context.
57  * This implementation of Boykov and Kolmogorov's maxflow algorithm by Stephen Gould
58  * <stephen.gould@anu.edu.au> in DARWIN under BSD does the trick however solwer than original
59  * implementation.
60  */
61  class PCL_EXPORTS BoykovKolmogorov
62  {
63  public:
64  using vertex_descriptor = int;
65  using edge_capacity_type = double;
66 
67  /// construct a maxflow/mincut problem with estimated max_nodes
68  BoykovKolmogorov (std::size_t max_nodes = 0);
69  /// destructor
70  virtual ~BoykovKolmogorov () {}
71  /// get number of nodes in the graph
72  size_t
73  numNodes () const { return nodes_.size (); }
74  /// reset all edge capacities to zero (but don't free the graph)
75  void
76  reset ();
77  /// clear the graph and internal datastructures
78  void
79  clear ();
80  /// add nodes to the graph (returns the id of the first node added)
81  int
82  addNodes (std::size_t n = 1);
83  /// add constant flow to graph
84  void
85  addConstant (double c) { flow_value_ += c; }
86  /// add edge from s to nodeId
87  void
88  addSourceEdge (int u, double cap);
89  /// add edge from nodeId to t
90  void
91  addTargetEdge (int u, double cap);
92  /// add edge from u to v and edge from v to u
93  /// (requires cap_uv + cap_vu >= 0)
94  void
95  addEdge (int u, int v, double cap_uv, double cap_vu = 0.0);
96  /// solve the max-flow problem and return the flow
97  double
98  solve ();
99  /// return true if \p u is in the s-set after calling \ref solve.
100  bool
101  inSourceTree (int u) const { return (cut_[u] == SOURCE); }
102  /// return true if \p u is in the t-set after calling \ref solve
103  bool
104  inSinkTree (int u) const { return (cut_[u] == TARGET); }
105  /// returns the residual capacity for an edge (use -1 for terminal (-1,-1) is the current flow
106  double
107  operator() (int u, int v) const;
108 
109  double
110  getSourceEdgeCapacity (int u) const;
111 
112  double
113  getTargetEdgeCapacity (int u) const;
114 
115  protected:
116  /// tree states
117  enum nodestate { FREE = 0x00, SOURCE = 0x01, TARGET = 0x02 };
118  /// capacitated edge
119  using capacitated_edge = std::map<int, double>;
120  /// edge pair
121  using edge_pair = std::pair<capacitated_edge::iterator, capacitated_edge::iterator>;
122  /// pre-augment s-u-t and s-u-v-t paths
123  void
124  preAugmentPaths ();
125  /// initialize trees from source and target
126  void
127  initializeTrees ();
128  /// expand trees until a path is found (or no path (-1, -1))
129  std::pair<int, int>
130  expandTrees ();
131  /// augment the path found by expandTrees; return orphaned subtrees
132  void
133  augmentPath (const std::pair<int, int>& path, std::deque<int>& orphans);
134  /// adopt orphaned subtrees
135  void
136  adoptOrphans (std::deque<int>& orphans);
137  /// clear active set
138  void clearActive ();
139  /// \return true if active set is empty
140  inline bool
141  isActiveSetEmpty () const { return (active_head_ == TERMINAL); }
142  /// active if head or previous node is not the terminal
143  inline bool
144  isActive (int u) const { return ((u == active_head_) || (active_list_[u].first != TERMINAL)); }
145  /// mark vertex as active
146  void
147  markActive (int u);
148  /// mark vertex as inactive
149  void
150  markInactive (int u);
151  /// edges leaving the source
152  std::vector<double> source_edges_;
153  /// edges entering the target
154  std::vector<double> target_edges_;
155  /// nodes and their outgoing internal edges
156  std::vector<capacitated_edge> nodes_;
157  /// current flow value (includes constant)
158  double flow_value_;
159  /// identifies which side of the cut a node falls
160  std::vector<unsigned char> cut_;
161 
162  private:
163  /// parents_ flag for terminal state
164  static const int TERMINAL; // -1
165  /// search tree (also uses cut_)
166  std::vector<std::pair<int, edge_pair> > parents_;
167  /// doubly-linked list (prev, next)
168  std::vector<std::pair<int, int> > active_list_;
169  int active_head_, active_tail_;
170  };
171 
172  /**\brief Structure to save RGB colors into floats */
173  struct Color
174  {
175  Color () : r (0), g (0), b (0) {}
176  Color (float _r, float _g, float _b) : r(_r), g(_g), b(_b) {}
177  Color (const pcl::RGB& color) : r (color.r), g (color.g), b (color.b) {}
178 
179  template<typename PointT>
180  Color (const PointT& p);
181 
182  template<typename PointT>
183  operator PointT () const;
184 
185  float r, g, b;
186  };
187  /// An Image is a point cloud of Color
189  /** \brief Compute squared distance between two colors
190  * \param[in] c1 first color
191  * \param[in] c2 second color
192  * \return the squared distance measure in RGB space
193  */
194  float
195  colorDistance (const Color& c1, const Color& c2);
196  /// User supplied Trimap values
198  /// Grabcut derived hard segmentation values
200  /// Gaussian structure
201  struct Gaussian
202  {
203  Gaussian () {}
204  /// mean of the gaussian
206  /// covariance matrix of the gaussian
207  Eigen::Matrix3f covariance;
208  /// determinant of the covariance matrix
209  float determinant;
210  /// inverse of the covariance matrix
211  Eigen::Matrix3f inverse;
212  /// weighting of this gaussian in the GMM.
213  float pi;
214  /// highest eigenvalue of covariance matrix
215  float eigenvalue;
216  /// eigenvector corresponding to the highest eigenvector
217  Eigen::Vector3f eigenvector;
218  };
219 
220  class PCL_EXPORTS GMM
221  {
222  public:
223  /// Initialize GMM with ddesired number of gaussians.
224  GMM () : gaussians_ (0) {}
225  /// Initialize GMM with ddesired number of gaussians.
226  GMM (std::size_t K) : gaussians_ (K) {}
227  /// Destructor
228  ~GMM () {}
229  /// \return K
230  std::size_t
231  getK () const { return gaussians_.size (); }
232  /// resize gaussians
233  void
234  resize (std::size_t K) { gaussians_.resize (K); }
235  /// \return a reference to the gaussian at a given position
236  Gaussian&
237  operator[] (std::size_t pos) { return (gaussians_[pos]); }
238  /// \return a const reference to the gaussian at a given position
239  const Gaussian&
240  operator[] (std::size_t pos) const { return (gaussians_[pos]); }
241  /// \brief \return the computed probability density of a color in this GMM
242  float
243  probabilityDensity (const Color &c);
244  /// \brief \return the computed probability density of a color in just one Gaussian
245  float
246  probabilityDensity(std::size_t i, const Color &c);
247 
248  private:
249  /// array of gaussians
250  std::vector<Gaussian> gaussians_;
251  };
252 
253  /** Helper class that fits a single Gaussian to color samples */
255  {
256  public:
257  GaussianFitter (float epsilon = 0.0001)
258  : sum_ (Eigen::Vector3f::Zero ())
259  , accumulator_ (Eigen::Matrix3f::Zero ())
260  , count_ (0)
261  , epsilon_ (epsilon)
262  { }
263 
264  /// Add a color sample
265  void
266  add (const Color &c);
267  /// Build the gaussian out of all the added color samples
268  void
269  fit (Gaussian& g, std::size_t total_count, bool compute_eigens = false) const;
270  /// \return epsilon
271  float
272  getEpsilon () { return (epsilon_); }
273  /** set epsilon which will be added to the covariance matrix diagonal which avoids singular
274  * covariance matrix
275  * \param[in] epsilon user defined epsilon
276  */
277  void
278  setEpsilon (float epsilon) { epsilon_ = epsilon; }
279 
280  private:
281  /// sum of r,g, and b
282  Eigen::Vector3f sum_;
283  /// matrix of products (i.e. r*r, r*g, r*b), some values are duplicated.
284  Eigen::Matrix3f accumulator_;
285  /// count of color samples added to the gaussian
286  uint32_t count_;
287  /// small value to add to covariance matrix diagonal to avoid singular values
288  float epsilon_;
290  };
291 
292  /** Build the initial GMMs using the Orchard and Bouman color clustering algorithm */
293  PCL_EXPORTS void
294  buildGMMs (const Image &image,
295  const std::vector<int>& indices,
296  const std::vector<SegmentationValue> &hardSegmentation,
297  std::vector<std::size_t> &components,
298  GMM &background_GMM, GMM &foreground_GMM);
299  /** Iteratively learn GMMs using GrabCut updating algorithm */
300  PCL_EXPORTS void
301  learnGMMs (const Image& image,
302  const std::vector<int>& indices,
303  const std::vector<SegmentationValue>& hard_segmentation,
304  std::vector<std::size_t>& components,
305  GMM& background_GMM, GMM& foreground_GMM);
306  }
307  };
308 
309  /** \brief Implementation of the GrabCut segmentation in
310  * "GrabCut — Interactive Foreground Extraction using Iterated Graph Cuts" by
311  * Carsten Rother, Vladimir Kolmogorov and Andrew Blake.
312  *
313  * \author Justin Talbot, jtalbot@stanford.edu placed in Public Domain, 2010
314  * \author Nizar Sallem port to PCL and adaptation of original code.
315  * \ingroup segmentation
316  */
317  template <typename PointT>
318  class GrabCut : public pcl::PCLBase<PointT>
319  {
320  public:
322  using KdTreePtr = typename KdTree::Ptr;
328 
329  /// Constructor
330  GrabCut (uint32_t K = 5, float lambda = 50.f)
331  : K_ (K)
332  , lambda_ (lambda)
333  , nb_neighbours_ (9)
334  , initialized_ (false)
335  {}
336  /// Destructor
337  ~GrabCut () {};
338  // /// Set input cloud
339  void
340  setInputCloud (const PointCloudConstPtr& cloud) override;
341  /// Set background points, foreground points = points \ background points
342  void
343  setBackgroundPoints (const PointCloudConstPtr& background_points);
344  /// Set background indices, foreground indices = indices \ background indices
345  void
346  setBackgroundPointsIndices (int x1, int y1, int x2, int y2);
347  /// Set background indices, foreground indices = indices \ background indices
348  void
349  setBackgroundPointsIndices (const PointIndicesConstPtr& indices);
350  /// Run Grabcut refinement on the hard segmentation
351  virtual void
352  refine ();
353  /// \return the number of pixels that have changed from foreground to background or vice versa
354  virtual int
355  refineOnce ();
356  /// \return lambda
357  float
358  getLambda () { return (lambda_); }
359  /** Set lambda parameter to user given value. Suggested value by the authors is 50
360  * \param[in] lambda
361  */
362  void
363  setLambda (float lambda) { lambda_ = lambda; }
364  /// \return the number of components in the GMM
365  uint32_t
366  getK () { return (K_); }
367  /** Set K parameter to user given value. Suggested value by the authors is 5
368  * \param[in] K the number of components used in GMM
369  */
370  void
371  setK (uint32_t K) { K_ = K; }
372  /** \brief Provide a pointer to the search object.
373  * \param tree a pointer to the spatial search object.
374  */
375  inline void
376  setSearchMethod (const KdTreePtr &tree) { tree_ = tree; }
377  /** \brief Get a pointer to the search method used. */
378  inline KdTreePtr
379  getSearchMethod () { return (tree_); }
380  /** \brief Allows to set the number of neighbours to find.
381  * \param[in] nb_neighbours new number of neighbours
382  */
383  void
384  setNumberOfNeighbours (int nb_neighbours) { nb_neighbours_ = nb_neighbours; }
385  /** \brief Returns the number of neighbours to find. */
386  int
387  getNumberOfNeighbours () const { return (nb_neighbours_); }
388  /** \brief This method launches the segmentation algorithm and returns the clusters that were
389  * obtained during the segmentation. The indices of points belonging to the object will be stored
390  * in the cluster with index 1, other indices will be stored in the cluster with index 0.
391  * \param[out] clusters clusters that were obtained. Each cluster is an array of point indices.
392  */
393  void
394  extract (std::vector<pcl::PointIndices>& clusters);
395 
396  protected:
397  // Storage for N-link weights, each pixel stores links to nb_neighbours
398  struct NLinks
399  {
400  NLinks () : nb_links (0), indices (0), dists (0), weights (0) {}
401 
402  int nb_links;
403  std::vector<int> indices;
404  std::vector<float> dists;
405  std::vector<float> weights;
406  };
407  bool
408  initCompute ();
410  /// Compute beta from image
411  void
412  computeBetaOrganized ();
413  /// Compute beta from cloud
414  void
415  computeBetaNonOrganized ();
416  /// Compute L parameter from given lambda
417  void
418  computeL ();
419  /// Compute NLinks from image
420  void
421  computeNLinksOrganized ();
422  /// Compute NLinks from cloud
423  void
424  computeNLinksNonOrganized ();
425  /// Edit Trimap
426  void
427  setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t);
428  int
429  updateHardSegmentation ();
430  /// Fit Gaussian Multi Models
431  virtual void
432  fitGMMs ();
433  /// Build the graph for GraphCut
434  void
435  initGraph ();
436  /// Add an edge to the graph, graph must be oriented so we add the edge and its reverse
437  void
438  addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity);
439  /// Set the weights of SOURCE --> v and v --> SINK
440  void
441  setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity);
442  /// \return true if v is in source tree
443  inline bool
444  isSource (vertex_descriptor v) { return (graph_.inSourceTree (v)); }
445  /// image width
446  uint32_t width_;
447  /// image height
448  uint32_t height_;
449  // Variables used in formulas from the paper.
450  /// Number of GMM components
451  uint32_t K_;
452  /// lambda = 50. This value was suggested the GrabCut paper.
453  float lambda_;
454  /// beta = 1/2 * average of the squared color distances between all pairs of 8-neighboring pixels.
455  float beta_;
456  /// L = a large value to force a pixel to be foreground or background
457  float L_;
458  /// Pointer to the spatial search object.
460  /// Number of neighbours
462  /// is segmentation initialized
464  /// Precomputed N-link weights
465  std::vector<NLinks> n_links_;
466  /// Converted input
468  std::vector<segmentation::grabcut::TrimapValue> trimap_;
469  std::vector<std::size_t> GMM_component_;
470  std::vector<segmentation::grabcut::SegmentationValue> hard_segmentation_;
471  // Not yet implemented (this would be interpreted as alpha)
472  std::vector<float> soft_segmentation_;
474  // Graph part
475  /// Graph for Graphcut
477  /// Graph nodes
478  std::vector<vertex_descriptor> graph_nodes_;
479  };
480 }
481 
482 #include <pcl/segmentation/impl/grabcut_segmentation.hpp>
Color(float _r, float _g, float _b)
float L_
L = a large value to force a pixel to be foreground or background.
int nb_neighbours_
Number of neighbours.
std::vector< segmentation::grabcut::SegmentationValue > hard_segmentation_
bool isSource(vertex_descriptor v)
GMM(std::size_t K)
Initialize GMM with ddesired number of gaussians.
float beta_
beta = 1/2 * average of the squared color distances between all pairs of 8-neighboring pixels...
std::vector< std::size_t > GMM_component_
void setLambda(float lambda)
Set lambda parameter to user given value.
Helper class that fits a single Gaussian to color samples.
std::vector< unsigned char > cut_
identifies which side of the cut a node falls
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
std::vector< float > soft_segmentation_
std::pair< capacitated_edge::iterator, capacitated_edge::iterator > edge_pair
edge pair
Eigen::Matrix3f inverse
inverse of the covariance matrix
void addConstant(double c)
add constant flow to graph
typename KdTree::Ptr KdTreePtr
size_t numNodes() const
get number of nodes in the graph
GMM()
Initialize GMM with ddesired number of gaussians.
void resize(std::size_t K)
resize gaussians
~GrabCut()
Destructor.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
Definition: pcl_macros.h:344
Definition: bfgs.h:9
float colorDistance(const Color &c1, const Color &c2)
Compute squared distance between two colors.
uint32_t width_
image width
Eigen::Matrix3f covariance
covariance matrix of the gaussian
std::vector< vertex_descriptor > graph_nodes_
Graph nodes.
std::map< int, double > capacitated_edge
capacitated edge
A structure representing RGB color information.
std::vector< capacitated_edge > nodes_
nodes and their outgoing internal edges
float lambda_
lambda = 50. This value was suggested the GrabCut paper.
SegmentationValue
Grabcut derived hard segmentation values.
void setNumberOfNeighbours(int nb_neighbours)
Allows to set the number of neighbours to find.
std::vector< double > target_edges_
edges entering the target
KdTreePtr tree_
Pointer to the spatial search object.
segmentation::grabcut::Image::Ptr image_
Converted input.
std::vector< NLinks > n_links_
Precomputed N-link weights.
boost::shared_ptr< KdTree< PointT > > Ptr
Definition: kdtree.h:70
uint32_t K_
Number of GMM components.
Eigen::Vector3f eigenvector
eigenvector corresponding to the highest eigenvector
float pi
weighting of this gaussian in the GMM.
std::vector< double > source_edges_
edges leaving the source
PCL base class.
Definition: pcl_base.h:69
typename PointCloud::Ptr PointCloudPtr
Definition: pcl_base.h:73
uint32_t height_
image height
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:429
bool inSinkTree(int u) const
return true if u is in the t-set after calling solve
boost::shared_ptr< PointIndices const > PointIndicesConstPtr
Definition: pcl_base.h:77
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.
std::vector< segmentation::grabcut::TrimapValue > trimap_
KdTreePtr getSearchMethod()
Get a pointer to the search method used.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
bool inSourceTree(int u) const
return true if u is in the s-set after calling solve.
boost implementation of Boykov and Kolmogorov&#39;s maxflow algorithm doesn&#39;t support negative flows whic...
void setK(uint32_t K)
Set K parameter to user given value.
Definition: norms.h:54
pcl::segmentation::grabcut::BoykovKolmogorov graph_
Graph for Graphcut.
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.
Implementation of the GrabCut segmentation in "GrabCut — Interactive Foreground Extraction using Ite...
int getNumberOfNeighbours() const
Returns the number of neighbours to find.
Structure to save RGB colors into floats.
void setEpsilon(float epsilon)
set epsilon which will be added to the covariance matrix diagonal which avoids singular covariance ma...
A point structure representing Euclidean xyz coordinates, and the RGB color.
TrimapValue
User supplied Trimap values.
GrabCut(uint32_t K=5, float lambda=50.f)
Constructor.
segmentation::grabcut::GMM foreground_GMM_
bool initialized_
is segmentation initialized
double flow_value_
current flow value (includes constant)
float eigenvalue
highest eigenvalue of covariance matrix
float determinant
determinant of the covariance matrix
bool isActive(int u) const
active if head or previous node is not the terminal
typename PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:74
pcl::segmentation::grabcut::BoykovKolmogorov::vertex_descriptor vertex_descriptor
Generic search class.
Definition: search.h:73
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.