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