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