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