Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2012-, Open Perception, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 * 00038 */ 00039 00040 #ifndef PCL_SEGMENTATION_GRABCUT 00041 #define PCL_SEGMENTATION_GRABCUT 00042 00043 #include <pcl/point_cloud.h> 00044 #include <pcl/pcl_base.h> 00045 #include <pcl/point_types.h> 00046 #include <pcl/segmentation/boost.h> 00047 #include <pcl/search/search.h> 00048 00049 namespace pcl 00050 { 00051 namespace segmentation 00052 { 00053 namespace grabcut 00054 { 00055 /** boost implementation of Boykov and Kolmogorov's maxflow algorithm doesn't support 00056 * negative flows which makes it inappropriate for this conext. 00057 * This implementation of Boykov and Kolmogorov's maxflow algorithm by Stephen Gould 00058 * <stephen.gould@anu.edu.au> in DARWIN under BSD does the trick however solwer than original 00059 * implementation. 00060 */ 00061 class BoykovKolmogorov 00062 { 00063 public: 00064 typedef int vertex_descriptor; 00065 typedef double edge_capacity_type; 00066 00067 /// construct a maxflow/mincut problem with estimated max_nodes 00068 BoykovKolmogorov (std::size_t max_nodes = 0); 00069 /// destructor 00070 virtual ~BoykovKolmogorov () {} 00071 /// get number of nodes in the graph 00072 size_t 00073 numNodes () const { return nodes_.size (); } 00074 /// reset all edge capacities to zero (but don't free the graph) 00075 void 00076 reset (); 00077 /// clear the graph and internal datastructures 00078 void 00079 clear (); 00080 /// add nodes to the graph (returns the id of the first node added) 00081 int 00082 addNodes (std::size_t n = 1); 00083 /// add constant flow to graph 00084 void 00085 addConstant (double c) { flow_value_ += c; } 00086 /// add edge from s to nodeId 00087 void 00088 addSourceEdge (int u, double cap); 00089 /// add edge from nodeId to t 00090 void 00091 addTargetEdge (int u, double cap); 00092 /// add edge from u to v and edge from v to u 00093 /// (requires cap_uv + cap_vu >= 0) 00094 void 00095 addEdge (int u, int v, double cap_uv, double cap_vu = 0.0); 00096 /// solve the max-flow problem and return the flow 00097 double 00098 solve (); 00099 /// return true if \p u is in the s-set after calling \ref solve. 00100 bool 00101 inSourceTree (int u) const { return (cut_[u] == SOURCE); } 00102 /// return true if \p u is in the t-set after calling \ref solve 00103 bool 00104 inSinkTree (int u) const { return (cut_[u] == TARGET); } 00105 /// returns the residual capacity for an edge (use -1 for terminal (-1,-1) is the current flow 00106 double 00107 operator() (int u, int v) const; 00108 00109 protected: 00110 /// tree states 00111 typedef enum { FREE = 0x00, SOURCE = 0x01, TARGET = 0x02 } nodestate; 00112 /// capacitated edge 00113 typedef std::map<int, double> capacitated_edge; 00114 /// edge pair 00115 typedef std::pair<capacitated_edge::iterator, capacitated_edge::iterator> edge_pair; 00116 /// pre-augment s-u-t and s-u-v-t paths 00117 void 00118 preAugmentPaths (); 00119 /// initialize trees from source and target 00120 void 00121 initializeTrees (); 00122 /// expand trees until a path is found (or no path (-1, -1)) 00123 std::pair<int, int> 00124 expandTrees (); 00125 /// augment the path found by expandTrees; return orphaned subtrees 00126 void 00127 augmentPath (const std::pair<int, int>& path, std::deque<int>& orphans); 00128 /// adopt orphaned subtrees 00129 void 00130 adoptOrphans (std::deque<int>& orphans); 00131 /// clear active set 00132 void clearActive (); 00133 /// \return true if active set is empty 00134 inline bool 00135 isActiveSetEmpty () const { return (active_head_ == TERMINAL); } 00136 /// active if head or previous node is not the terminal 00137 inline bool 00138 isActive (int u) const { return ((u == active_head_) || (active_list_[u].first != TERMINAL)); } 00139 /// mark vertex as active 00140 void 00141 markActive (int u); 00142 /// mark vertex as inactive 00143 void 00144 markInactive (int u); 00145 /// edges leaving the source 00146 std::vector<double> source_edges_; 00147 /// edges entering the target 00148 std::vector<double> target_edges_; 00149 /// nodes and their outgoing internal edges 00150 std::vector<capacitated_edge> nodes_; 00151 /// current flow value (includes constant) 00152 double flow_value_; 00153 /// identifies which side of the cut a node falls 00154 std::vector<unsigned char> cut_; 00155 00156 private: 00157 /// parents_ flag for terminal state 00158 static const int TERMINAL = -1; 00159 /// search tree (also uses cut_) 00160 std::vector<std::pair<int, edge_pair> > parents_; 00161 /// doubly-linked list (prev, next) 00162 std::vector<std::pair<int, int> > active_list_; 00163 int active_head_, active_tail_; 00164 }; 00165 00166 /**\brief Structure to save RGB colors into floats */ 00167 struct Color 00168 { 00169 Color () : r (0), g (0), b (0) {} 00170 Color (float _r, float _g, float _b) : r(_r), g(_g), b(_b) {} 00171 Color (const pcl::RGB& color) : r (color.r), g (color.g), b (color.b) {} 00172 00173 template<typename PointT> 00174 Color (const PointT& p) 00175 { 00176 r = static_cast<float> (p.r); 00177 g = static_cast<float> (p.g); 00178 b = static_cast<float> (p.b); 00179 } 00180 00181 template<typename PointT> 00182 operator PointT () const 00183 { 00184 PointT p; 00185 p.r = static_cast<uint32_t> (r); 00186 p.g = static_cast<uint32_t> (g); 00187 p.b = static_cast<uint32_t> (b); 00188 return (p); 00189 } 00190 00191 float r, g, b; 00192 }; 00193 /// An Image is a point cloud of Color 00194 typedef pcl::PointCloud<Color> Image; 00195 /** \brief Compute squared distance between two colors 00196 * \param[in] c1 first color 00197 * \param[in] c2 second color 00198 * \return the squared distance measure in RGB space 00199 */ 00200 float 00201 colorDistance (const Color& c1, const Color& c2); 00202 /// User supplied Trimap values 00203 enum TrimapValue { TrimapUnknown = -1, TrimapForeground, TrimapBackground }; 00204 /// Grabcut derived hard segementation values 00205 enum SegmentationValue { SegmentationForeground = 0, SegmentationBackground }; 00206 /// Gaussian structure 00207 struct Gaussian 00208 { 00209 Gaussian () {} 00210 /// mean of the gaussian 00211 Color mu; 00212 /// covariance matrix of the gaussian 00213 Eigen::Matrix3f covariance; 00214 /// determinant of the covariance matrix 00215 float determinant; 00216 /// inverse of the covariance matrix 00217 Eigen::Matrix3f inverse; 00218 /// weighting of this gaussian in the GMM. 00219 float pi; 00220 /// heighest eigenvalue of covariance matrix 00221 float eigenvalue; 00222 /// eigenvector corresponding to the heighest eigenvector 00223 Eigen::Vector3f eigenvector; 00224 }; 00225 00226 class GMM 00227 { 00228 public: 00229 /// Initialize GMM with ddesired number of gaussians. 00230 GMM () : gaussians_ (0) {} 00231 /// Initialize GMM with ddesired number of gaussians. 00232 GMM (std::size_t K) : gaussians_ (K) {} 00233 /// Destructor 00234 ~GMM () {} 00235 /// \return K 00236 std::size_t 00237 getK () const { return gaussians_.size (); } 00238 /// resize gaussians 00239 void 00240 resize (std::size_t K) { gaussians_.resize (K); } 00241 /// \return a reference to the gaussian at a given position 00242 Gaussian& 00243 operator[] (std::size_t pos) { return (gaussians_[pos]); } 00244 /// \return a const reference to the gaussian at a given position 00245 const Gaussian& 00246 operator[] (std::size_t pos) const { return (gaussians_[pos]); } 00247 /// \brief \return the computed probability density of a color in this GMM 00248 float 00249 probabilityDensity (const Color &c); 00250 /// \brief \return the computed probability density of a color in just one Gaussian 00251 float 00252 probabilityDensity(std::size_t i, const Color &c); 00253 00254 private: 00255 /// array of gaussians 00256 std::vector<Gaussian> gaussians_; 00257 }; 00258 00259 /** Helper class that fits a single Gaussian to color samples */ 00260 class GaussianFitter 00261 { 00262 public: 00263 GaussianFitter (float epsilon = 0.0001) 00264 : sum_ (Eigen::Vector3f::Zero ()) 00265 , accumulator_ (Eigen::Matrix3f::Zero ()) 00266 , count_ (0) 00267 , epsilon_ (epsilon) 00268 { } 00269 00270 /// Add a color sample 00271 void 00272 add (const Color &c); 00273 /// Build the gaussian out of all the added color samples 00274 void 00275 fit (Gaussian& g, std::size_t total_count, bool compute_eigens = false) const; 00276 /// \return epsilon 00277 float 00278 getEpsilon () { return (epsilon_); } 00279 /** set epsilon which will be added to the covariance matrix diagonal which avoids singular 00280 * covariance matrix 00281 * \param[in] epsilon user defined epsilon 00282 */ 00283 void 00284 setEpsilon (float epsilon) { epsilon_ = epsilon; } 00285 00286 private: 00287 /// sum of r,g, and b 00288 Eigen::Vector3f sum_; 00289 /// matrix of products (i.e. r*r, r*g, r*b), some values are duplicated. 00290 Eigen::Matrix3f accumulator_; 00291 /// count of color samples added to the gaussian 00292 uint32_t count_; 00293 /// small value to add to covariance matrix diagonal to avoid singular values 00294 float epsilon_; 00295 }; 00296 00297 /** Build the initial GMMs using the Orchard and Bouman color clustering algorithm */ 00298 void 00299 buildGMMs (const Image &image, 00300 const std::vector<int>& indices, 00301 const std::vector<SegmentationValue> &hardSegmentation, 00302 std::vector<std::size_t> &components, 00303 GMM &background_GMM, GMM &foreground_GMM); 00304 /** Iteratively learn GMMs using GrabCut updating algorithm */ 00305 void 00306 learnGMMs (const Image& image, 00307 const std::vector<int>& indices, 00308 const std::vector<SegmentationValue>& hard_segmentation, 00309 std::vector<std::size_t>& components, 00310 GMM& background_GMM, GMM& foreground_GMM); 00311 } 00312 }; 00313 00314 /** \brief Implementation of the GrabCut segmentation in 00315 * "GrabCut — Interactive Foreground Extraction using Iterated Graph Cuts" by 00316 * Carsten Rother, Vladimir Kolmogorov and Andrew Blake. 00317 * 00318 * \author Justin Talbot, jtalbot@stanford.edu placed in Public Domain, 2010 00319 * \author Nizar Sallem port to PCL and adaptation of original code. 00320 * \ingroup segmentation 00321 */ 00322 template <typename PointT> 00323 class GrabCut : public pcl::PCLBase<PointT> 00324 { 00325 public: 00326 typedef typename pcl::search::Search<PointT> KdTree; 00327 typedef typename pcl::search::Search<PointT>::Ptr KdTreePtr; 00328 typedef typename PCLBase<PointT>::PointCloudConstPtr PointCloudConstPtr; 00329 typedef typename PCLBase<PointT>::PointCloudPtr PointCloudPtr; 00330 using PCLBase<PointT>::input_; 00331 using PCLBase<PointT>::indices_; 00332 using PCLBase<PointT>::fake_indices_; 00333 00334 /// Constructor 00335 GrabCut (uint32_t K = 5, float lambda = 50.f) 00336 : K_ (K) 00337 , lambda_ (lambda) 00338 , initialized_ (false) 00339 , nb_neighbours_ (9) 00340 {} 00341 /// Desctructor 00342 virtual ~GrabCut () {}; 00343 // /// Set input cloud 00344 void 00345 setInputCloud (const PointCloudConstPtr& cloud); 00346 /// Set background points, foreground points = points \ background points 00347 void 00348 setBackgroundPoints (const PointCloudConstPtr& background_points); 00349 /// Set background indices, foreground indices = indices \ background indices 00350 void 00351 setBackgroundPointsIndices (int x1, int y1, int x2, int y2); 00352 /// Set background indices, foreground indices = indices \ background indices 00353 void 00354 setBackgroundPointsIndices (const PointIndicesConstPtr& indices); 00355 /// Run Grabcut refinement on the hard segmentation 00356 virtual void 00357 refine (); 00358 /// \return the number of pixels that have changed from foreground to background or vice versa 00359 virtual int 00360 refineOnce (); 00361 /// \return lambda 00362 float 00363 getLambda () { return (lambda_); } 00364 /** Set lambda parameter to user given value. Suggested value by the authors is 50 00365 * \param[in] lambda 00366 */ 00367 void 00368 setLambda (float lambda) { lambda_ = lambda; } 00369 /// \return the number of components in the GMM 00370 uint32_t 00371 getK () { return (K_); } 00372 /** Set K parameter to user given value. Suggested value by the authors is 5 00373 * \param[in] K the number of components used in GMM 00374 */ 00375 void 00376 setK (uint32_t K) { K_ = K; } 00377 /** \brief Provide a pointer to the search object. 00378 * \param tree a pointer to the spatial search object. 00379 */ 00380 inline void 00381 setSearchMethod (const KdTreePtr &tree) { tree_ = tree; } 00382 /** \brief Get a pointer to the search method used. */ 00383 inline KdTreePtr 00384 getSearchMethod () { return (tree_); } 00385 /** \brief Allows to set the number of neighbours to find. 00386 * \param[in] number_of_neighbours new number of neighbours 00387 */ 00388 void 00389 setNumberOfNeighbours (int nb_neighbours) { nb_neighbours_ = nb_neighbours; } 00390 /** \brief Returns the number of neighbours to find. */ 00391 int 00392 getNumberOfNeighbours () const { return (nb_neighbours_); } 00393 /** \brief This method launches the segmentation algorithm and returns the clusters that were 00394 * obtained during the segmentation. The indices of points belonging to the object will be stored 00395 * in the cluster with index 1, other indices will be stored in the cluster with index 0. 00396 * \param[out] clusters clusters that were obtained. Each cluster is an array of point indices. 00397 */ 00398 void 00399 extract (std::vector<pcl::PointIndices>& clusters); 00400 00401 protected: 00402 // Storage for N-link weights, each pixel stores links to nb_neighbours 00403 struct NLinks 00404 { 00405 NLinks () : nb_links (0), indices (0), dists (0), weights (0) {} 00406 00407 int nb_links; 00408 std::vector<int> indices; 00409 std::vector<float> dists; 00410 std::vector<float> weights; 00411 }; 00412 bool 00413 initCompute (); 00414 typedef pcl::segmentation::grabcut::BoykovKolmogorov::vertex_descriptor vertex_descriptor; 00415 // /** Update hard segmentation after running GraphCut, \return the number of pixels that have 00416 // * changed from foreground to background or vice versa. 00417 // */ 00418 // int 00419 // updateHardSegmentation (); 00420 /// Compute beta from image 00421 void 00422 computeBeta (); 00423 /// Compute L parameter from given lambda 00424 void 00425 computeL (); 00426 /// Compute NLinks 00427 void 00428 computeNLinks (); 00429 /// Compute NLinks at a specific rectangular location 00430 float 00431 computeNLink (uint32_t x1, uint32_t y1, uint32_t x2, uint32_t y2); 00432 /// Edit Trimap 00433 void 00434 setTrimap (const PointIndicesConstPtr &indices, segmentation::grabcut::TrimapValue t); 00435 int 00436 updateHardSegmentation (); 00437 /// Fit Gaussian Multi Models 00438 virtual void 00439 fitGMMs (); 00440 /// Build the graph for GraphCut 00441 void 00442 initGraph (); 00443 /// Add an edge to the graph, graph must be oriented so we add the edge and its reverse 00444 void 00445 addEdge (vertex_descriptor v1, vertex_descriptor v2, float capacity, float rev_capacity); 00446 /// Set the weights of SOURCE --> v and v --> SINK 00447 void 00448 setTerminalWeights (vertex_descriptor v, float source_capacity, float sink_capacity); 00449 /// \return true if v is in source tree 00450 inline bool 00451 isSource (vertex_descriptor v) { return (graph_.inSourceTree (v)); } 00452 /// image width 00453 uint32_t width_; 00454 /// image height 00455 uint32_t height_; 00456 // Variables used in formulas from the paper. 00457 /// Number of GMM components 00458 uint32_t K_; 00459 /// lambda = 50. This value was suggested the GrabCut paper. 00460 float lambda_; 00461 /// beta = 1/2 * average of the squared color distances between all pairs of 8-neighboring pixels. 00462 float beta_; 00463 /// L = a large value to force a pixel to be foreground or background 00464 float L_; 00465 /// Pointer to the spatial search object. 00466 KdTreePtr tree_; 00467 /// Number of neighbours 00468 int nb_neighbours_; 00469 /// is segmentation initialized 00470 bool initialized_; 00471 /// Precomputed N-link weights 00472 std::vector<NLinks> n_links_; 00473 /// Converted input 00474 segmentation::grabcut::Image::Ptr image_; 00475 std::vector<segmentation::grabcut::TrimapValue> trimap_; 00476 std::vector<std::size_t> GMM_component_; 00477 std::vector<segmentation::grabcut::SegmentationValue> hard_segmentation_; 00478 // Not yet implemented (this would be interpreted as alpha) 00479 std::vector<float> soft_segmentation_; 00480 segmentation::grabcut::GMM background_GMM_, foreground_GMM_; 00481 // Graph part 00482 /// Graph for Graphcut 00483 pcl::segmentation::grabcut::BoykovKolmogorov graph_; 00484 /// Graph nodes 00485 std::vector<vertex_descriptor> graph_nodes_; 00486 }; 00487 } 00488 00489 #include <pcl/segmentation/impl/grabcut.hpp> 00490 00491 #endif