Point Cloud Library (PCL)  1.9.1-dev
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 #pragma once
42 
43 #include <boost/version.hpp>
44 
45 #include <pcl/features/normal_3d.h>
46 #include <pcl/pcl_base.h>
47 #include <pcl/point_cloud.h>
48 #include <pcl/point_types.h>
49 #include <pcl/octree/octree_search.h>
50 #include <pcl/octree/octree_pointcloud_adjacency.h>
51 #include <pcl/search/search.h>
52 #include <pcl/segmentation/boost.h>
53 
54 
55 
56 //DEBUG TODO REMOVE
57 #include <pcl/common/time.h>
58 
59 
60 namespace pcl
61 {
62  /** \brief Supervoxel container class - stores a cluster extracted using supervoxel clustering
63  */
64  template <typename PointT>
65  class Supervoxel
66  {
67  public:
69  voxels_ (new pcl::PointCloud<PointT> ()),
70  normals_ (new pcl::PointCloud<Normal> ())
71  { }
72 
73  typedef boost::shared_ptr<Supervoxel<PointT> > Ptr;
74  typedef boost::shared_ptr<const Supervoxel<PointT> > ConstPtr;
75 
76  /** \brief Gets the centroid of the supervoxel
77  * \param[out] centroid_arg centroid of the supervoxel
78  */
79  void
81  {
82  centroid_arg = centroid_;
83  }
84 
85  /** \brief Gets the point normal for the supervoxel
86  * \param[out] normal_arg Point normal of the supervoxel
87  * \note This isn't an average, it is a normal computed using all of the voxels in the supervoxel as support
88  */
89  void
91  {
92  normal_arg.x = centroid_.x;
93  normal_arg.y = centroid_.y;
94  normal_arg.z = centroid_.z;
95  normal_arg.normal_x = normal_.normal_x;
96  normal_arg.normal_y = normal_.normal_y;
97  normal_arg.normal_z = normal_.normal_z;
98  normal_arg.curvature = normal_.curvature;
99  }
100 
101  /** \brief The normal calculated for the voxels contained in the supervoxel */
103  /** \brief The centroid of the supervoxel - average voxel */
105  /** \brief A Pointcloud of the voxels in the supervoxel */
107  /** \brief A Pointcloud of the normals for the points in the supervoxel */
109 
110  public:
111  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
112  };
113 
114  /** \brief Implements a supervoxel algorithm based on voxel structure, normals, and rgb values
115  * \note Supervoxels are oversegmented volumetric patches (usually surfaces)
116  * \note Usually, color isn't needed (and can be detrimental)- spatial structure is mainly used
117  * - J. Papon, A. Abramov, M. Schoeler, F. Woergoetter
118  * Voxel Cloud Connectivity Segmentation - Supervoxels from PointClouds
119  * In Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR) 2013
120  * \ingroup segmentation
121  * \author Jeremie Papon (jpapon@gmail.com)
122  */
123  template <typename PointT>
124  class PCL_EXPORTS SupervoxelClustering : public pcl::PCLBase<PointT>
125  {
126  //Forward declaration of friended helper class
127  class SupervoxelHelper;
128  friend class SupervoxelHelper;
129  public:
130  /** \brief VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContainer
131  * \note It stores xyz, rgb, normal, distance, an index, and an owner.
132  */
133  class VoxelData
134  {
135  public:
137  xyz_ (0.0f, 0.0f, 0.0f),
138  rgb_ (0.0f, 0.0f, 0.0f),
139  normal_ (0.0f, 0.0f, 0.0f, 0.0f),
140  curvature_ (0.0f),
141  owner_ (nullptr)
142  {}
143 
144  /** \brief Gets the data of in the form of a point
145  * \param[out] point_arg Will contain the point value of the voxeldata
146  */
147  void
148  getPoint (PointT &point_arg) const;
149 
150  /** \brief Gets the data of in the form of a normal
151  * \param[out] normal_arg Will contain the normal value of the voxeldata
152  */
153  void
154  getNormal (Normal &normal_arg) const;
155 
156  Eigen::Vector3f xyz_;
157  Eigen::Vector3f rgb_;
158  Eigen::Vector4f normal_;
159  float curvature_;
160  float distance_;
161  int idx_;
162  SupervoxelHelper* owner_;
163 
164  public:
165  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
166  };
167 
169  typedef std::vector <LeafContainerT*> LeafVectorT;
170 
176  typedef boost::shared_ptr<std::vector<int> > IndicesPtr;
177 
181 
182  typedef boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, uint32_t, float> VoxelAdjacencyList;
183  typedef VoxelAdjacencyList::vertex_descriptor VoxelID;
184  typedef VoxelAdjacencyList::edge_descriptor EdgeID;
185 
186  public:
187 
188  /** \brief Constructor that sets default values for member variables.
189  * \param[in] voxel_resolution The resolution (in meters) of voxels used
190  * \param[in] seed_resolution The average size (in meters) of resulting supervoxels
191  */
192  SupervoxelClustering (float voxel_resolution, float seed_resolution);
193 
194  [[deprecated("constructor with flag for using the single camera transform is deprecated. Default behavior is now to use the transform for organized clouds, and not use it for unorganized. Use setUseSingleCameraTransform() to override the defaults.")]]
195  SupervoxelClustering (float voxel_resolution, float seed_resolution, bool) : SupervoxelClustering (voxel_resolution, seed_resolution) { }
196 
197  /** \brief This destructor destroys the cloud, normals and search method used for
198  * finding neighbors. In other words it frees memory.
199  */
200 
202 
203  /** \brief Set the resolution of the octree voxels */
204  void
205  setVoxelResolution (float resolution);
206 
207  /** \brief Get the resolution of the octree voxels */
208  float
209  getVoxelResolution () const;
210 
211  /** \brief Set the resolution of the octree seed voxels */
212  void
213  setSeedResolution (float seed_resolution);
214 
215  /** \brief Get the resolution of the octree seed voxels */
216  float
217  getSeedResolution () const;
218 
219  /** \brief Set the importance of color for supervoxels */
220  void
221  setColorImportance (float val);
222 
223  /** \brief Set the importance of spatial distance for supervoxels */
224  void
225  setSpatialImportance (float val);
226 
227  /** \brief Set the importance of scalar normal product for supervoxels */
228  void
229  setNormalImportance (float val);
230 
231  /** \brief Set whether or not to use the single camera transform
232  * \note By default it will be used for organized clouds, but not for unorganized - this parameter will override that behavior
233  * The single camera transform scales bin size so that it increases exponentially with depth (z dimension).
234  * This is done to account for the decreasing point density found with depth when using an RGB-D camera.
235  * Without the transform, beyond a certain depth adjacency of voxels breaks down unless the voxel size is set to a large value.
236  * Using the transform allows preserving detail up close, while allowing adjacency at distance.
237  * The specific transform used here is:
238  * x /= z; y /= z; z = ln(z);
239  * This transform is applied when calculating the octree bins in OctreePointCloudAdjacency
240  */
241  void
242  setUseSingleCameraTransform (bool val);
243 
244  /** \brief This method launches the segmentation algorithm and returns the supervoxels that were
245  * obtained during the segmentation.
246  * \param[out] supervoxel_clusters A map of labels to pointers to supervoxel structures
247  */
248  virtual void
249  extract (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
250 
251  /** \brief This method sets the cloud to be supervoxelized
252  * \param[in] cloud The cloud to be supervoxelize
253  */
254  void
255  setInputCloud (const typename pcl::PointCloud<PointT>::ConstPtr& cloud) override;
256 
257  /** \brief This method sets the normals to be used for supervoxels (should be same size as input cloud)
258  * \param[in] normal_cloud The input normals
259  */
260  virtual void
261  setNormalCloud (typename NormalCloudT::ConstPtr normal_cloud);
262 
263  /** \brief This method refines the calculated supervoxels - may only be called after extract
264  * \param[in] num_itr The number of iterations of refinement to be done (2 or 3 is usually sufficient)
265  * \param[out] supervoxel_clusters The resulting refined supervoxels
266  */
267  virtual void
268  refineSupervoxels (int num_itr, std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
269 
270  ////////////////////////////////////////////////////////////
271  /** \brief Returns an RGB colorized cloud showing superpixels
272  * Otherwise it returns an empty pointer.
273  * Points that belong to the same supervoxel have the same color.
274  * But this function doesn't guarantee that different segments will have different
275  * color(it's random). Points that are unlabeled will be black
276  * \note This will expand the label_colors_ vector so that it can accommodate all labels
277  */
278  [[deprecated("use getLabeledCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp")]]
281  {
283  }
284 
285  /** \brief Returns a deep copy of the voxel centroid cloud */
287  getVoxelCentroidCloud () const;
288 
289  /** \brief Returns labeled cloud
290  * Points that belong to the same supervoxel have the same label.
291  * Labels for segments start from 1, unlabled points have label 0
292  */
294  getLabeledCloud () const;
295 
296  /** \brief Returns an RGB colorized voxelized cloud showing superpixels
297  * Otherwise it returns an empty pointer.
298  * Points that belong to the same supervoxel have the same color.
299  * But this function doesn't guarantee that different segments will have different
300  * color(it's random). Points that are unlabeled will be black
301  * \note This will expand the label_colors_ vector so that it can accommodate all labels
302  */
303  [[deprecated("use getLabeledVoxelCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp")]]
306  {
308  }
309 
310  /** \brief Returns labeled voxelized cloud
311  * Points that belong to the same supervoxel have the same label.
312  * Labels for segments start from 1, unlabled points have label 0
313  */
315  getLabeledVoxelCloud () const;
316 
317  /** \brief Gets the adjacency list (Boost Graph library) which gives connections between supervoxels
318  * \param[out] adjacency_list_arg BGL graph where supervoxel labels are vertices, edges are touching relationships
319  */
320  void
321  getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const;
322 
323  /** \brief Get a multimap which gives supervoxel adjacency
324  * \param[out] label_adjacency Multi-Map which maps a supervoxel label to all adjacent supervoxel labels
325  */
326  void
327  getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const;
328 
329  /** \brief Static helper function which returns a pointcloud of normals for the input supervoxels
330  * \param[in] supervoxel_clusters Supervoxel cluster map coming from this class
331  * \returns Cloud of PointNormals of the supervoxels
332  *
333  */
335  makeSupervoxelNormalCloud (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
336 
337  /** \brief Returns the current maximum (highest) label */
338  int
339  getMaxLabel () const;
340 
341  private:
342  /** \brief This method simply checks if it is possible to execute the segmentation algorithm with
343  * the current settings. If it is possible then it returns true.
344  */
345  virtual bool
346  prepareForSegmentation ();
347 
348  /** \brief This selects points to use as initial supervoxel centroids
349  * \param[out] seed_indices The selected leaf indices
350  */
351  void
352  selectInitialSupervoxelSeeds (std::vector<int> &seed_indices);
353 
354  /** \brief This method creates the internal supervoxel helpers based on the provided seed points
355  * \param[in] seed_indices Indices of the leaves to use as seeds
356  */
357  void
358  createSupervoxelHelpers (std::vector<int> &seed_indices);
359 
360  /** \brief This performs the superpixel evolution */
361  void
362  expandSupervoxels (int depth);
363 
364  /** \brief This sets the data of the voxels in the tree */
365  void
366  computeVoxelData ();
367 
368  /** \brief Reseeds the supervoxels by finding the voxel closest to current centroid */
369  void
370  reseedSupervoxels ();
371 
372  /** \brief Constructs the map of supervoxel clusters from the internal supervoxel helpers */
373  void
374  makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
375 
376  /** \brief Stores the resolution used in the octree */
377  float resolution_;
378 
379  /** \brief Stores the resolution used to seed the superpixels */
380  float seed_resolution_;
381 
382  /** \brief Distance function used for comparing voxelDatas */
383  float
384  voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const;
385 
386  /** \brief Transform function used to normalize voxel density versus distance from camera */
387  void
388  transformFunction (PointT &p);
389 
390  /** \brief Contains a KDtree for the voxelized cloud */
391  typename pcl::search::KdTree<PointT>::Ptr voxel_kdtree_;
392 
393  /** \brief Octree Adjacency structure with leaves at voxel resolution */
394  typename OctreeAdjacencyT::Ptr adjacency_octree_;
395 
396  /** \brief Contains the Voxelized centroid Cloud */
397  typename PointCloudT::Ptr voxel_centroid_cloud_;
398 
399  /** \brief Contains the Voxelized centroid Cloud */
400  typename NormalCloudT::ConstPtr input_normals_;
401 
402  /** \brief Importance of color in clustering */
403  float color_importance_;
404  /** \brief Importance of distance from seed center in clustering */
405  float spatial_importance_;
406  /** \brief Importance of similarity in normals for clustering */
407  float normal_importance_;
408 
409  /** \brief Whether or not to use the transform compressing depth in Z
410  * This is only checked if it has been manually set by the user.
411  * The default behavior is to use the transform for organized, and not for unorganized.
412  */
413  bool use_single_camera_transform_;
414  /** \brief Whether to use default transform behavior or not */
415  bool use_default_transform_behaviour_;
416 
417  /** \brief Internal storage class for supervoxels
418  * \note Stores pointers to leaves of clustering internal octree,
419  * \note so should not be used outside of clustering class
420  */
421  class SupervoxelHelper
422  {
423  public:
424  /** \brief Comparator for LeafContainerT pointers - used for sorting set of leaves
425  * \note Compares by index in the overall leaf_vector. Order isn't important, so long as it is fixed.
426  */
428  {
429  bool operator() (LeafContainerT* const &left, LeafContainerT* const &right) const
430  {
431  const VoxelData& leaf_data_left = left->getData ();
432  const VoxelData& leaf_data_right = right->getData ();
433  return leaf_data_left.idx_ < leaf_data_right.idx_;
434  }
435  };
436  typedef std::set<LeafContainerT*, typename SupervoxelHelper::compareLeaves> LeafSetT;
437  typedef typename LeafSetT::iterator iterator;
438  typedef typename LeafSetT::const_iterator const_iterator;
439 
440  SupervoxelHelper (uint32_t label, SupervoxelClustering* parent_arg):
441  label_ (label),
442  parent_ (parent_arg)
443  { }
444 
445  void
446  addLeaf (LeafContainerT* leaf_arg);
447 
448  void
449  removeLeaf (LeafContainerT* leaf_arg);
450 
451  void
452  removeAllLeaves ();
453 
454  void
455  expand ();
456 
457  void
458  refineNormals ();
459 
460  void
461  updateCentroid ();
462 
463  void
464  getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const;
465 
466  void
467  getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const;
468 
469  typedef float (SupervoxelClustering::*DistFuncPtr)(const VoxelData &v1, const VoxelData &v2);
470 
471  uint32_t
472  getLabel () const
473  { return label_; }
474 
475  Eigen::Vector4f
476  getNormal () const
477  { return centroid_.normal_; }
478 
479  Eigen::Vector3f
480  getRGB () const
481  { return centroid_.rgb_; }
482 
483  Eigen::Vector3f
484  getXYZ () const
485  { return centroid_.xyz_;}
486 
487  void
488  getXYZ (float &x, float &y, float &z) const
489  { x=centroid_.xyz_[0]; y=centroid_.xyz_[1]; z=centroid_.xyz_[2]; }
490 
491  void
492  getRGB (uint32_t &rgba) const
493  {
494  rgba = static_cast<uint32_t>(centroid_.rgb_[0]) << 16 |
495  static_cast<uint32_t>(centroid_.rgb_[1]) << 8 |
496  static_cast<uint32_t>(centroid_.rgb_[2]);
497  }
498 
499  void
500  getNormal (pcl::Normal &normal_arg) const
501  {
502  normal_arg.normal_x = centroid_.normal_[0];
503  normal_arg.normal_y = centroid_.normal_[1];
504  normal_arg.normal_z = centroid_.normal_[2];
505  normal_arg.curvature = centroid_.curvature_;
506  }
507 
508  void
509  getNeighborLabels (std::set<uint32_t> &neighbor_labels) const;
510 
511  VoxelData
512  getCentroid () const
513  { return centroid_; }
514 
515  size_t
516  size () const { return leaves_.size (); }
517  private:
518  //Stores leaves
519  LeafSetT leaves_;
520  uint32_t label_;
522  SupervoxelClustering* parent_;
523  public:
524  //Type VoxelData may have fixed-size Eigen objects inside
525  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
526  };
527 
528  //Make boost::ptr_list can access the private class SupervoxelHelper
529 #if BOOST_VERSION >= 107000
530  friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering<PointT>::SupervoxelHelper *) BOOST_NOEXCEPT;
531 #else
532  friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering<PointT>::SupervoxelHelper *);
533 #endif
534 
535  typedef boost::ptr_list<SupervoxelHelper> HelperListT;
536  HelperListT supervoxel_helpers_;
537 
538  //TODO DEBUG REMOVE
539  StopWatch timer_;
540  public:
541  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
542 
543  };
544 
545 }
546 
547 #ifdef PCL_NO_PRECOMPILE
548 #include <pcl/segmentation/impl/supervoxel_clustering.hpp>
549 #endif
A point structure representing normal coordinates and the surface curvature estimate.
pcl::octree::OctreePointCloudAdjacency< PointT, LeafContainerT > OctreeAdjacencyT
pcl::PointCloud< PointT >::Ptr voxels_
A Pointcloud of the voxels in the supervoxel.
Octree adjacency leaf container class- stores a list of pointers to neighbors, number of points added...
pcl::PointXYZRGBA centroid_
The centroid of the supervoxel - average voxel.
Octree pointcloud voxel class which maintains adjacency information for its voxels.
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:44
void getCentroidPointNormal(PointNormal &normal_arg)
Gets the point normal for the supervoxel.
pcl::Normal normal_
The normal calculated for the voxels contained in the supervoxel.
VoxelAdjacencyList::vertex_descriptor VoxelID
pcl::PointCloud< PointT > PointCloudT
boost::shared_ptr< std::vector< int > > IndicesPtr
A point structure representing Euclidean xyz coordinates, and the RGBA color.
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getColoredVoxelCloud() const
Returns an RGB colorized voxelized cloud showing superpixels Otherwise it returns an empty pointer...
boost::shared_ptr< const Supervoxel< PointT > > ConstPtr
Supervoxel container class - stores a cluster extracted using supervoxel clustering.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:427
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:78
PCL base class.
Definition: pcl_base.h:68
pcl::octree::OctreePointCloudSearch< PointT > OctreeSearchT
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
std::vector< LeafContainerT * > LeafVectorT
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:428
pcl::search::KdTree< PointT > KdTreeT
void getCentroidPoint(PointXYZRGBA &centroid_arg)
Gets the centroid of the supervoxel.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Implements a supervoxel algorithm based on voxel structure, normals, and rgb values.
pcl::PointCloud< Normal > NormalCloudT
Comparator for LeafContainerT pointers - used for sorting set of leaves.
A point structure representing Euclidean xyz coordinates, together with normal coordinates and the su...
DataT & getData()
Returns a reference to the data member to access it without copying.
boost::shared_ptr< OctreeAdjacencyT > Ptr
Octree pointcloud search class
Definition: octree_search.h:56
VoxelAdjacencyList::edge_descriptor EdgeID
Simple stopwatch.
Definition: time.h:58
SupervoxelClustering(float voxel_resolution, float seed_resolution, bool)
pcl::octree::OctreePointCloudAdjacencyContainer< PointT, VoxelData > LeafContainerT
pcl::PointCloud< Normal >::Ptr normals_
A Pointcloud of the normals for the points in the supervoxel.
A point structure representing Euclidean xyz coordinates, and the RGB color.
boost::shared_ptr< Supervoxel< PointT > > Ptr
boost::adjacency_list< boost::setS, boost::setS, boost::undirectedS, uint32_t, float > VoxelAdjacencyList
pcl::PointCloud< PointXYZRGBA >::Ptr getColoredCloud() const
Returns an RGB colorized cloud showing superpixels Otherwise it returns an empty pointer.