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