Point Cloud Library (PCL)  1.7.1
supervoxel_clustering.h
1 
2 /*
3  * Software License Agreement (BSD License)
4  *
5  * Point Cloud Library (PCL) - www.pointclouds.org
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  * Author : jpapon@gmail.com
37  * Email : jpapon@gmail.com
38  *
39  */
40 
41 #ifndef PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_H_
42 #define PCL_SEGMENTATION_SUPERVOXEL_CLUSTERING_H_
43 
44 #include <pcl/features/normal_3d.h>
45 #include <pcl/pcl_base.h>
46 #include <pcl/point_cloud.h>
47 #include <pcl/point_types.h>
48 #include <pcl/octree/octree.h>
49 #include <pcl/octree/octree_pointcloud_adjacency.h>
50 #include <pcl/search/search.h>
51 #include <pcl/segmentation/boost.h>
52 
53 
54 
55 //DEBUG TODO REMOVE
56 #include <pcl/common/time.h>
57 
58 
59 namespace pcl
60 {
61  /** \brief Supervoxel container class - stores a cluster extracted using supervoxel clustering
62  */
63  template <typename PointT>
64  class Supervoxel
65  {
66  public:
68  voxels_ (new pcl::PointCloud<PointT> ()),
69  normals_ (new pcl::PointCloud<Normal> ())
70  { }
71 
72  typedef boost::shared_ptr<Supervoxel<PointT> > Ptr;
73  typedef boost::shared_ptr<const Supervoxel<PointT> > ConstPtr;
74 
75  /** \brief Gets the centroid of the supervoxel
76  * \param[out] centroid_arg centroid of the supervoxel
77  */
78  void
80  {
81  centroid_arg = centroid_;
82  }
83 
84  /** \brief Gets the point normal for the supervoxel
85  * \param[out] normal_arg Point normal of the supervoxel
86  * \note This isn't an average, it is a normal computed using all of the voxels in the supervoxel as support
87  */
88  void
90  {
91  normal_arg.x = centroid_.x;
92  normal_arg.y = centroid_.y;
93  normal_arg.z = centroid_.z;
94  normal_arg.normal_x = normal_.normal_x;
95  normal_arg.normal_y = normal_.normal_y;
96  normal_arg.normal_z = normal_.normal_z;
97  normal_arg.curvature = normal_.curvature;
98  }
99 
100  /** \brief The normal calculated for the voxels contained in the supervoxel */
102  /** \brief The centroid of the supervoxel - average voxel */
104  /** \brief A Pointcloud of the voxels in the supervoxel */
106  /** \brief A Pointcloud of the normals for the points in the supervoxel */
108  };
109 
110  /** \brief Implements a supervoxel algorithm based on voxel structure, normals, and rgb values
111  * \note Supervoxels are oversegmented volumetric patches (usually surfaces)
112  * \note Usually, color isn't needed (and can be detrimental)- spatial structure is mainly used
113  * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
114  * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
115  * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
116  * \author Jeremie Papon (jpapon@gmail.com)
117  */
118  template <typename PointT>
119  class PCL_EXPORTS SupervoxelClustering : public pcl::PCLBase<PointT>
120  {
121  //Forward declaration of friended helper class
122  class SupervoxelHelper;
123  friend class SupervoxelHelper;
124  #define MAX_LABEL 16384
125  public:
126  /** \brief VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContainer
127  * \note It stores xyz, rgb, normal, distance, an index, and an owner.
128  */
129  class VoxelData
130  {
131  public:
133  xyz_ (0.0f, 0.0f, 0.0f),
134  rgb_ (0.0f, 0.0f, 0.0f),
135  normal_ (0.0f, 0.0f, 0.0f, 0.0f),
136  curvature_ (0.0f),
137  owner_ (0)
138  {}
139 
140  /** \brief Gets the data of in the form of a point
141  * \param[out] point_arg Will contain the point value of the voxeldata
142  */
143  void
144  getPoint (PointT &point_arg) const;
145 
146  /** \brief Gets the data of in the form of a normal
147  * \param[out] normal_arg Will contain the normal value of the voxeldata
148  */
149  void
150  getNormal (Normal &normal_arg) const;
151 
152  Eigen::Vector3f xyz_;
153  Eigen::Vector3f rgb_;
154  Eigen::Vector4f normal_;
155  float curvature_;
156  float distance_;
157  int idx_;
158  SupervoxelHelper* owner_;
159  };
160 
162  typedef std::vector <LeafContainerT*> LeafVectorT;
163 
169  typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
170 
174 
175  typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, uint32_t, float> VoxelAdjacencyList;
176  typedef VoxelAdjacencyList::vertex_descriptor VoxelID;
177  typedef VoxelAdjacencyList::edge_descriptor EdgeID;
178 
179 
180  public:
181 
182  /** \brief Constructor that sets default values for member variables.
183  * \param[in] voxel_resolution The resolution (in meters) of voxels used
184  * \param[in] seed_resolution The average size (in meters) of resulting supervoxels
185  * \param[in] use_single_camera_transform Set to true if point density in cloud falls off with distance from origin (such as with a cloud coming from one stationary camera), set false if input cloud is from multiple captures from multiple locations.
186  */
187  SupervoxelClustering (float voxel_resolution, float seed_resolution, bool use_single_camera_transform = true);
188 
189  /** \brief This destructor destroys the cloud, normals and search method used for
190  * finding neighbors. In other words it frees memory.
191  */
192  virtual
194 
195  /** \brief Set the resolution of the octree voxels */
196  void
197  setVoxelResolution (float resolution);
198 
199  /** \brief Get the resolution of the octree voxels */
200  float
201  getVoxelResolution () const;
202 
203  /** \brief Set the resolution of the octree seed voxels */
204  void
205  setSeedResolution (float seed_resolution);
206 
207  /** \brief Get the resolution of the octree seed voxels */
208  float
209  getSeedResolution () const;
210 
211  /** \brief Set the importance of color for supervoxels */
212  void
213  setColorImportance (float val);
214 
215  /** \brief Set the importance of spatial distance for supervoxels */
216  void
217  setSpatialImportance (float val);
218 
219  /** \brief Set the importance of scalar normal product for supervoxels */
220  void
221  setNormalImportance (float val);
222 
223  /** \brief This method launches the segmentation algorithm and returns the supervoxels that were
224  * obtained during the segmentation.
225  * \param[out] supervoxel_clusters A map of labels to pointers to supervoxel structures
226  */
227  virtual void
228  extract (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
229 
230  /** \brief This method sets the cloud to be supervoxelized
231  * \param[in] cloud The cloud to be supervoxelize
232  */
233  virtual void
234  setInputCloud (typename pcl::PointCloud<PointT>::ConstPtr cloud);
235 
236  /** \brief This method refines the calculated supervoxels - may only be called after extract
237  * \param[in] num_itr The number of iterations of refinement to be done (2 or 3 is usually sufficient)
238  * \param[out] supervoxel_clusters The resulting refined supervoxels
239  */
240  virtual void
241  refineSupervoxels (int num_itr, std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
242 
243  ////////////////////////////////////////////////////////////
244  /** \brief Returns an RGB colorized cloud showing superpixels
245  * Otherwise it returns an empty pointer.
246  * Points that belong to the same supervoxel have the same color.
247  * But this function doesn't guarantee that different segments will have different
248  * color(it's random). Points that are unlabeled will be black
249  */
251  getColoredCloud () const;
252 
253  /** \brief Returns a deep copy of the voxel centroid cloud */
255  getVoxelCentroidCloud () const;
256 
257  /** \brief Returns labeled cloud
258  * Points that belong to the same supervoxel have the same label.
259  * Labels for segments start from 1, unlabled points have label 0
260  */
262  getLabeledCloud () const;
263 
264  /** \brief Returns an RGB colorized voxelized cloud showing superpixels
265  * Otherwise it returns an empty pointer.
266  * Points that belong to the same supervoxel have the same color.
267  * But this function doesn't guarantee that different segments will have different
268  * color(it's random). Points that are unlabeled will be black
269  */
271  getColoredVoxelCloud () const;
272 
273  /** \brief Returns labeled voxelized cloud
274  * Points that belong to the same supervoxel have the same label.
275  * Labels for segments start from 1, unlabled points have label 0
276  */
278  getLabeledVoxelCloud () const;
279 
280  /** \brief Gets the adjacency list (Boost Graph library) which gives connections between supervoxels
281  * \param[out] adjacency_list_arg BGL graph where supervoxel labels are vertices, edges are touching relationships
282  */
283  void
284  getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const;
285 
286  /** \brief Get a multimap which gives supervoxel adjacency
287  * \param[out] label_adjacency Multi-Map which maps a supervoxel label to all adjacent supervoxel labels
288  */
289  void
290  getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const;
291 
292  /** \brief Static helper function which returns a pointcloud of normals for the input supervoxels
293  * \param[in] supervoxel_clusters Supervoxel cluster map coming from this class
294  * \returns Cloud of PointNormals of the supervoxels
295  *
296  */
298  makeSupervoxelNormalCloud (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
299  private:
300 
301  /** \brief This method simply checks if it is possible to execute the segmentation algorithm with
302  * the current settings. If it is possible then it returns true.
303  */
304  virtual bool
305  prepareForSegmentation ();
306 
307  /** \brief This selects points to use as initial supervoxel centroids
308  * \param[out] seed_points The selected points
309  */
310  void
311  selectInitialSupervoxelSeeds (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points);
312 
313  /** \brief This method creates the internal supervoxel helpers based on the provided seed points
314  * \param[in] seed_points The selected points
315  */
316  void
317  createSupervoxelHelpers (std::vector<PointT, Eigen::aligned_allocator<PointT> > &seed_points);
318 
319  /** \brief This performs the superpixel evolution */
320  void
321  expandSupervoxels (int depth);
322 
323  /** \brief This sets the data of the voxels in the tree */
324  void
325  computeVoxelData ();
326 
327  /** \brief Reseeds the supervoxels by finding the voxel closest to current centroid */
328  void
329  reseedSupervoxels ();
330 
331  /** \brief Constructs the map of supervoxel clusters from the internal supervoxel helpers */
332  void
333  makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
334 
335  /** \brief Stores the resolution used in the octree */
336  float resolution_;
337 
338  /** \brief Stores the resolution used to seed the superpixels */
339  float seed_resolution_;
340 
341  /** \brief Distance function used for comparing voxelDatas */
342  float
343  voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const;
344 
345  /** \brief Transform function used to normalize voxel density versus distance from camera */
346  void
347  transformFunction (PointT &p);
348 
349  /** \brief Contains a KDtree for the voxelized cloud */
350  typename pcl::search::KdTree<PointT>::Ptr voxel_kdtree_;
351 
352  /** \brief Octree Adjacency structure with leaves at voxel resolution */
353  typename OctreeAdjacencyT::Ptr adjacency_octree_;
354 
355  /** \brief Contains the Voxelized centroid Cloud */
356  typename PointCloudT::Ptr voxel_centroid_cloud_;
357 
358  /** \brief Importance of color in clustering */
359  float color_importance_;
360  /** \brief Importance of distance from seed center in clustering */
361  float spatial_importance_;
362  /** \brief Importance of similarity in normals for clustering */
363  float normal_importance_;
364 
365  /** \brief Stores the colors used for the superpixel labels*/
366  std::vector<uint32_t> label_colors_;
367 
368  /** \brief Internal storage class for supervoxels
369  * \note Stores pointers to leaves of clustering internal octree,
370  * \note so should not be used outside of clustering class
371  */
372  class SupervoxelHelper
373  {
374  public:
375  SupervoxelHelper (uint32_t label, SupervoxelClustering* parent_arg):
376  label_ (label),
377  parent_ (parent_arg)
378  { }
379 
380  void
381  addLeaf (LeafContainerT* leaf_arg);
382 
383  void
384  removeLeaf (LeafContainerT* leaf_arg);
385 
386  void
387  removeAllLeaves ();
388 
389  void
390  expand ();
391 
392  void
393  refineNormals ();
394 
395  void
396  updateCentroid ();
397 
398  void
399  getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const;
400 
401  void
402  getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const;
403 
404  typedef float (SupervoxelClustering::*DistFuncPtr)(const VoxelData &v1, const VoxelData &v2);
405 
406  uint32_t
407  getLabel () const
408  { return label_; }
409 
410  Eigen::Vector4f
411  getNormal () const
412  { return centroid_.normal_; }
413 
414  Eigen::Vector3f
415  getRGB () const
416  { return centroid_.rgb_; }
417 
418  Eigen::Vector3f
419  getXYZ () const
420  { return centroid_.xyz_;}
421 
422  void
423  getXYZ (float &x, float &y, float &z) const
424  { x=centroid_.xyz_[0]; y=centroid_.xyz_[1]; z=centroid_.xyz_[2]; }
425 
426  void
427  getRGB (uint32_t &rgba) const
428  {
429  rgba = static_cast<uint32_t>(centroid_.rgb_[0]) << 16 |
430  static_cast<uint32_t>(centroid_.rgb_[1]) << 8 |
431  static_cast<uint32_t>(centroid_.rgb_[2]);
432  }
433 
434  void
435  getNormal (pcl::Normal &normal_arg) const
436  {
437  normal_arg.normal_x = centroid_.normal_[0];
438  normal_arg.normal_y = centroid_.normal_[1];
439  normal_arg.normal_z = centroid_.normal_[2];
440  normal_arg.curvature = centroid_.curvature_;
441  }
442 
443  void
444  getNeighborLabels (std::set<uint32_t> &neighbor_labels) const;
445 
446  VoxelData
447  getCentroid () const
448  { return centroid_; }
449 
450 
451  size_t
452  size () const { return leaves_.size (); }
453  private:
454  //Stores leaves
455  std::set<LeafContainerT*> leaves_;
456  uint32_t label_;
457  VoxelData centroid_;
458  SupervoxelClustering* parent_;
459 
460 
461  };
462 
463  typedef boost::ptr_list<SupervoxelHelper> HelperListT;
464  HelperListT supervoxel_helpers_;
465 
466  //TODO DEBUG REMOVE
467  StopWatch timer_;
468  public:
469  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
470 
471 
472 
473  };
474 
475 }
476 
477 #ifdef PCL_NO_PRECOMPILE
478 #include <pcl/segmentation/impl/supervoxel_clustering.hpp>
479 #endif
480 
481 #endif