Point Cloud Library (PCL)  1.7.0
grabcut.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 #ifndef PCL_SEGMENTATION_GRABCUT
41 #define PCL_SEGMENTATION_GRABCUT
42 
43 #include <pcl/point_cloud.h>
44 #include <pcl/pcl_base.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 conext.
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  */
62  {
63  public:
64  typedef int vertex_descriptor;
65  typedef double edge_capacity_type;
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  protected:
110  /// tree states
111  typedef enum { FREE = 0x00, SOURCE = 0x01, TARGET = 0x02 } nodestate;
112  /// capacitated edge
113  typedef std::map<int, double> capacitated_edge;
114  /// edge pair
115  typedef std::pair<capacitated_edge::iterator, capacitated_edge::iterator> edge_pair;
116  /// pre-augment s-u-t and s-u-v-t paths
117  void
118  preAugmentPaths ();
119  /// initialize trees from source and target
120  void
121  initializeTrees ();
122  /// expand trees until a path is found (or no path (-1, -1))
123  std::pair<int, int>
124  expandTrees ();
125  /// augment the path found by expandTrees; return orphaned subtrees
126  void
127  augmentPath (const std::pair<int, int>& path, std::deque<int>& orphans);
128  /// adopt orphaned subtrees
129  void
130  adoptOrphans (std::deque<int>& orphans);
131  /// clear active set
132  void clearActive ();
133  /// \return true if active set is empty
134  inline bool
135  isActiveSetEmpty () const { return (active_head_ == TERMINAL); }
136  /// active if head or previous node is not the terminal
137  inline bool
138  isActive (int u) const { return ((u == active_head_) || (active_list_[u].first != TERMINAL)); }
139  /// mark vertex as active
140  void
141  markActive (int u);
142  /// mark vertex as inactive
143  void
144  markInactive (int u);
145  /// edges leaving the source
146  std::vector<double> source_edges_;
147  /// edges entering the target
148  std::vector<double> target_edges_;
149  /// nodes and their outgoing internal edges
150  std::vector<capacitated_edge> nodes_;
151  /// current flow value (includes constant)
152  double flow_value_;
153  /// identifies which side of the cut a node falls
154  std::vector<unsigned char> cut_;
155 
156  private:
157  /// parents_ flag for terminal state
158  static const int TERMINAL = -1;
159  /// search tree (also uses cut_)
160  std::vector<std::pair<int, edge_pair> > parents_;
161  /// doubly-linked list (prev, next)
162  std::vector<std::pair<int, int> > active_list_;
163  int active_head_, active_tail_;
164  };
165 
166  /**\brief Structure to save RGB colors into floats */
167  struct Color
168  {
169  Color () : r (0), g (0), b (0) {}
170  Color (float _r, float _g, float _b) : r(_r), g(_g), b(_b) {}
171  Color (const pcl::RGB& color) : r (color.r), g (color.g), b (color.b) {}
172 
173  template<typename PointT>
174  Color (const PointT& p)
175  {
176  r = static_cast<float> (p.r);
177  g = static_cast<float> (p.g);
178  b = static_cast<float> (p.b);
179  }
180 
181  template<typename PointT>
182  operator PointT () const
183  {
184  PointT p;
185  p.r = static_cast<uint32_t> (r);
186  p.g = static_cast<uint32_t> (g);
187  p.b = static_cast<uint32_t> (b);
188  return (p);
189  }
190 
191  float r, g, b;
192  };
193  /// An Image is a point cloud of Color
195  /** \brief Compute squared distance between two colors
196  * \param[in] c1 first color
197  * \param[in] c2 second color
198  * \return the squared distance measure in RGB space
199  */
200  float
201  colorDistance (const Color& c1, const Color& c2);
202  /// User supplied Trimap values
204  /// Grabcut derived hard segementation values
206  /// Gaussian structure
207  struct Gaussian
208  {
209  Gaussian () {}
210  /// mean of the gaussian
212  /// covariance matrix of the gaussian
213  Eigen::Matrix3f covariance;
214  /// determinant of the covariance matrix
215  float determinant;
216  /// inverse of the covariance matrix
217  Eigen::Matrix3f inverse;
218  /// weighting of this gaussian in the GMM.
219  float pi;
220  /// heighest eigenvalue of covariance matrix
221  float eigenvalue;
222  /// eigenvector corresponding to the heighest eigenvector
223  Eigen::Vector3f eigenvector;
224  };
225 
226  class GMM
227  {
228  public:
229  /// Initialize GMM with ddesired number of gaussians.
230  GMM () : gaussians_ (0) {}
231  /// Initialize GMM with ddesired number of gaussians.
232  GMM (std::size_t K) : gaussians_ (K) {}
233  /// Destructor
234  ~GMM () {}
235  /// \return K
236  std::size_t
237  getK () const { return gaussians_.size (); }
238  /// resize gaussians
239  void
240  resize (std::size_t K) { gaussians_.resize (K); }
241  /// \return a reference to the gaussian at a given position
242  Gaussian&
243  operator[] (std::size_t pos) { return (gaussians_[pos]); }
244  /// \return a const reference to the gaussian at a given position
245  const Gaussian&
246  operator[] (std::size_t pos) const { return (gaussians_[pos]); }
247  /// \brief \return the computed probability density of a color in this GMM
248  float
249  probabilityDensity (const Color &c);
250  /// \brief \return the computed probability density of a color in just one Gaussian
251  float
252  probabilityDensity(std::size_t i, const Color &c);
253 
254  private:
255  /// array of gaussians
256  std::vector<Gaussian> gaussians_;
257  };
258 
259  /** Helper class that fits a single Gaussian to color samples */
261  {
262  public:
263  GaussianFitter (float epsilon = 0.0001)
264  : sum_ (Eigen::Vector3f::Zero ())
265  , accumulator_ (Eigen::Matrix3f::Zero ())
266  , count_ (0)
267  , epsilon_ (epsilon)
268  { }
269 
270  /// Add a color sample
271  void
272  add (const Color &c);
273  /// Build the gaussian out of all the added color samples
274  void
275  fit (Gaussian& g, std::size_t total_count, bool compute_eigens = false) const;
276  /// \return epsilon
277  float
278  getEpsilon () { return (epsilon_); }
279  /** set epsilon which will be added to the covariance matrix diagonal which avoids singular
280  * covariance matrix
281  * \param[in] epsilon user defined epsilon
282  */
283  void
284  setEpsilon (float epsilon) { epsilon_ = epsilon; }
285 
286  private:
287  /// sum of r,g, and b
288  Eigen::Vector3f sum_;
289  /// matrix of products (i.e. r*r, r*g, r*b), some values are duplicated.
290  Eigen::Matrix3f accumulator_;
291  /// count of color samples added to the gaussian
292  uint32_t count_;
293  /// small value to add to covariance matrix diagonal to avoid singular values
294  float epsilon_;
295  };
296 
297  /** Build the initial GMMs using the Orchard and Bouman color clustering algorithm */
298  void
299  buildGMMs (const Image &image,
300  const std::vector<int>& indices,
301  const std::vector<SegmentationValue> &hardSegmentation,
302  std::vector<std::size_t> &components,
303  GMM &background_GMM, GMM &foreground_GMM);
304  /** Iteratively learn GMMs using GrabCut updating algorithm */
305  void
306  learnGMMs (const Image& image,
307  const std::vector<int>& indices,
308  const std::vector<SegmentationValue>& hard_segmentation,
309  std::vector<std::size_t>& components,
310  GMM& background_GMM, GMM& foreground_GMM);
311  }
312  };
313 
314  /** \brief Implementation of the GrabCut segmentation in
315  * "GrabCut — Interactive Foreground Extraction using Iterated Graph Cuts" by
316  * Carsten Rother, Vladimir Kolmogorov and Andrew Blake.
317  *
318  * \author Justin Talbot, jtalbot@stanford.edu placed in Public Domain, 2010
319  * \author Nizar Sallem port to PCL and adaptation of original code.
320  * \ingroup segmentation
321  */
322  template <typename PointT>
323  class GrabCut : public pcl::PCLBase<PointT>
324  {
325  public:
333 
334  /// Constructor
335  GrabCut (uint32_t K = 5, float lambda = 50.f)
336  : K_ (K)
337  , lambda_ (lambda)
338  , initialized_ (false)
339  , nb_neighbours_ (9)
340  {}
341  /// Desctructor
342  virtual ~GrabCut () {};
343  // /// Set input cloud
344  void
345  setInputCloud (const PointCloudConstPtr& cloud);
346  /// Set background points, foreground points = points \ background points
347  void
348  setBackgroundPoints (const PointCloudConstPtr& background_points);
349  /// Set background indices, foreground indices = indices \ background indices
350  void
351  setBackgroundPointsIndices (int x1, int y1, int x2, int y2);
352  /// Set background indices, foreground indices = indices \ background indices
353  void
355  /// Run Grabcut refinement on the hard segmentation
356  virtual void
357  refine ();
358  /// \return the number of pixels that have changed from foreground to background or vice versa
359  virtual int
360  refineOnce ();
361  /// \return lambda
362  float
363  getLambda () { return (lambda_); }
364  /** Set lambda parameter to user given value. Suggested value by the authors is 50
365  * \param[in] lambda
366  */
367  void
368  setLambda (float lambda) { lambda_ = lambda; }
369  /// \return the number of components in the GMM
370  uint32_t
371  getK () { return (K_); }
372  /** Set K parameter to user given value. Suggested value by the authors is 5
373  * \param[in] K the number of components used in GMM
374  */
375  void
376  setK (uint32_t K) { K_ = K; }
377  /** \brief Provide a pointer to the search object.
378  * \param tree a pointer to the spatial search object.
379  */
380  inline void
381  setSearchMethod (const KdTreePtr &tree) { tree_ = tree; }
382  /** \brief Get a pointer to the search method used. */
383  inline KdTreePtr
384  getSearchMethod () { return (tree_); }
385  /** \brief Allows to set the number of neighbours to find.
386  * \param[in] number_of_neighbours new number of neighbours
387  */
388  void
389  setNumberOfNeighbours (int nb_neighbours) { nb_neighbours_ = nb_neighbours; }
390  /** \brief Returns the number of neighbours to find. */
391  int
392  getNumberOfNeighbours () const { return (nb_neighbours_); }
393  /** \brief This method launches the segmentation algorithm and returns the clusters that were
394  * obtained during the segmentation. The indices of points belonging to the object will be stored
395  * in the cluster with index 1, other indices will be stored in the cluster with index 0.
396  * \param[out] clusters clusters that were obtained. Each cluster is an array of point indices.
397  */
398  void
399  extract (std::vector<pcl::PointIndices>& clusters);
400 
401  protected:
402  // Storage for N-link weights, each pixel stores links to nb_neighbours
403  struct NLinks
404  {
405  NLinks () : nb_links (0), indices (0), dists (0), weights (0) {}
406 
407  int nb_links;
408  std::vector<int> indices;
409  std::vector<float> dists;
410  std::vector<float> weights;
411  };
412  bool
413  initCompute ();
415  // /** Update hard segmentation after running GraphCut, \return the number of pixels that have
416  // * changed from foreground to background or vice versa.
417  // */
418  // int
419  // updateHardSegmentation ();
420  /// Compute beta from image
421  void
422  computeBeta ();
423  /// Compute L parameter from given lambda
424  void
425  computeL ();
426  /// Compute NLinks
427  void
428  computeNLinks ();
429  /// Compute NLinks at a specific rectangular location
430  float
431  computeNLink (uint32_t x1, uint32_t y1, uint32_t x2, uint32_t y2);
432  /// Edit Trimap
433  void
435  int
437  /// Fit Gaussian Multi Models
438  virtual void
439  fitGMMs ();
440  /// Build the graph for GraphCut
441  void
442  initGraph ();
443  /// Add an edge to the graph, graph must be oriented so we add the edge and its reverse
444  void
445  addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity);
446  /// Set the weights of SOURCE --> v and v --> SINK
447  void
448  setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity);
449  /// \return true if v is in source tree
450  inline bool
452  /// image width
453  uint32_t width_;
454  /// image height
455  uint32_t height_;
456  // Variables used in formulas from the paper.
457  /// Number of GMM components
458  uint32_t K_;
459  /// lambda = 50. This value was suggested the GrabCut paper.
460  float lambda_;
461  /// beta = 1/2 * average of the squared color distances between all pairs of 8-neighboring pixels.
462  float beta_;
463  /// L = a large value to force a pixel to be foreground or background
464  float L_;
465  /// Pointer to the spatial search object.
467  /// Number of neighbours
469  /// is segmentation initialized
471  /// Precomputed N-link weights
472  std::vector<NLinks> n_links_;
473  /// Converted input
475  std::vector<segmentation::grabcut::TrimapValue> trimap_;
476  std::vector<std::size_t> GMM_component_;
477  std::vector<segmentation::grabcut::SegmentationValue> hard_segmentation_;
478  // Not yet implemented (this would be interpreted as alpha)
479  std::vector<float> soft_segmentation_;
481  // Graph part
482  /// Graph for Graphcut
484  /// Graph nodes
485  std::vector<vertex_descriptor> graph_nodes_;
486  };
487 }
488 
489 #include <pcl/segmentation/impl/grabcut.hpp>
490 
491 #endif