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