Point Cloud Library (PCL)  1.8.0
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 #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  */
61  class PCL_EXPORTS BoykovKolmogorov
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  double
110  getSourceEdgeCapacity (int u) const;
111 
112  double
113  getTargetEdgeCapacity (int u) const;
114 
115  protected:
116  /// tree states
117  typedef enum { FREE = 0x00, SOURCE = 0x01, TARGET = 0x02 } nodestate;
118  /// capacitated edge
119  typedef std::map<int, double> capacitated_edge;
120  /// edge pair
121  typedef std::pair<capacitated_edge::iterator, capacitated_edge::iterator> edge_pair;
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 segementation 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  /// heighest eigenvalue of covariance matrix
215  float eigenvalue;
216  /// eigenvector corresponding to the heighest 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_;
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:
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  /// Desctructor
336  virtual ~GrabCut () {};
337  // /// Set input cloud
338  void
339  setInputCloud (const PointCloudConstPtr& cloud);
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>
482 
483 #endif
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
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
Definition: bfgs.h:10
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:428
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 segementation 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:81
std::vector< NLinks > n_links_
Precomputed N-link weights.
uint32_t K_
Number of GMM components.
Eigen::Vector3f eigenvector
eigenvector corresponding to the heighest 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.
pcl::search::Search< PointT >::Ptr KdTreePtr
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.
virtual ~GrabCut()
Desctructor.
Definition: norms.h:55
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
heighest 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:74
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.