Point Cloud Library (PCL)  1.7.2
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
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
412  /// Compute beta from cloud
413  void
415  /// Compute L parameter from given lambda
416  void
417  computeL ();
418  /// Compute NLinks from image
419  void
421  /// Compute NLinks from cloud
422  void
424  /// Edit Trimap
425  void
427  int
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
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.
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...
virtual void fitGMMs()
Fit Gaussian Multi Models.
PointCloud::ConstPtr PointCloudConstPtr
Definition: pcl_base.h:73
std::vector< std::size_t > GMM_component_
void setTrimap(const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t)
Edit Trimap.
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
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
std::vector< float > soft_segmentation_
Eigen::Matrix3f inverse
inverse of the covariance matrix
void addConstant(double c)
add constant flow to graph
void computeNLinksOrganized()
Compute NLinks from image.
void initGraph()
Build the graph for GraphCut.
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.
size_t numNodes() const
get number of nodes in the graph
GMM()
Initialize GMM with ddesired number of gaussians.
void computeBetaNonOrganized()
Compute beta from cloud.
void resize(std::size_t K)
resize gaussians
void extract(std::vector< pcl::PointIndices > &clusters)
This method launches the segmentation algorithm and returns the clusters that were obtained during th...
float colorDistance(const Color &c1, const Color &c2)
Compute squared distance between two colors.
uint32_t width_
image width
void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Eigen::Matrix3f covariance
covariance matrix of the gaussian
std::vector< vertex_descriptor > graph_nodes_
Graph nodes.
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.
void setBackgroundPoints(const PointCloudConstPtr &background_points)
Set background points, foreground points = points \ background points.
boost::shared_ptr< pcl::search::Search< PointT > > Ptr
Definition: search.h:81
void fit(Gaussian &g, std::size_t total_count, bool compute_eigens=false) const
Build the gaussian out of all the added color samples.
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
void computeBetaOrganized()
Compute beta from image.
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_
boost::shared_ptr< ::pcl::PointIndices const > PointIndicesConstPtr
Definition: PointIndices.h:27
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.
virtual int refineOnce()
Definition: norms.h:55
pcl::segmentation::grabcut::BoykovKolmogorov graph_
Graph for Graphcut.
void computeL()
Compute L parameter from given lambda.
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 &quot;GrabCut — Interactive Foreground Extraction using Iter...
int getNumberOfNeighbours() const
Returns the number of neighbours to find.
segmentation::grabcut::GMM background_GMM_
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_
void setTerminalWeights(vertex_descriptor v, float source_capacity, float sink_capacity)
Set the weights of SOURCE –&gt; v and v –&gt; SINK.
bool initialized_
is segmentation initialized
double flow_value_
current flow value (includes constant)
virtual void refine()
Run Grabcut refinement on the hard segmentation.
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
void computeNLinksNonOrganized()
Compute NLinks from cloud.
void add(const Color &c)
Add a color sample.
void setBackgroundPointsIndices(int x1, int y1, int x2, int y2)
Set background indices, foreground indices = indices \ background indices.
Generic search class.
Definition: search.h:74
void setSearchMethod(const KdTreePtr &tree)
Provide a pointer to the search object.