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/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 
74  using Ptr = boost::shared_ptr<Supervoxel<PointT> >;
75  using ConstPtr = boost::shared_ptr<const Supervoxel<PointT> >;
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 
179  using IndicesPtr = boost::shared_ptr<std::vector<int> >;
180 
184 
185  using VoxelAdjacencyList = boost::adjacency_list<boost::setS, boost::setS, boost::undirectedS, 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<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<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 an RGB colorized voxelized cloud showing superpixels
300  * Otherwise it returns an empty pointer.
301  * Points that belong to the same supervoxel have the same color.
302  * But this function doesn't guarantee that different segments will have different
303  * color(it's random). Points that are unlabeled will be black
304  * \note This will expand the label_colors_ vector so that it can accommodate all labels
305  */
306  [[deprecated("use getLabeledVoxelCloud() instead. An example of how to display and save with colorized labels can be found in examples/segmentation/example_supervoxels.cpp")]]
309  {
311  }
312 
313  /** \brief Returns labeled voxelized cloud
314  * Points that belong to the same supervoxel have the same label.
315  * Labels for segments start from 1, unlabled points have label 0
316  */
318  getLabeledVoxelCloud () const;
319 
320  /** \brief Gets the adjacency list (Boost Graph library) which gives connections between supervoxels
321  * \param[out] adjacency_list_arg BGL graph where supervoxel labels are vertices, edges are touching relationships
322  */
323  void
324  getSupervoxelAdjacencyList (VoxelAdjacencyList &adjacency_list_arg) const;
325 
326  /** \brief Get a multimap which gives supervoxel adjacency
327  * \param[out] label_adjacency Multi-Map which maps a supervoxel label to all adjacent supervoxel labels
328  */
329  void
330  getSupervoxelAdjacency (std::multimap<uint32_t, uint32_t> &label_adjacency) const;
331 
332  /** \brief Static helper function which returns a pointcloud of normals for the input supervoxels
333  * \param[in] supervoxel_clusters Supervoxel cluster map coming from this class
334  * \returns Cloud of PointNormals of the supervoxels
335  *
336  */
338  makeSupervoxelNormalCloud (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
339 
340  /** \brief Returns the current maximum (highest) label */
341  int
342  getMaxLabel () const;
343 
344  private:
345  /** \brief This method simply checks if it is possible to execute the segmentation algorithm with
346  * the current settings. If it is possible then it returns true.
347  */
348  virtual bool
349  prepareForSegmentation ();
350 
351  /** \brief This selects points to use as initial supervoxel centroids
352  * \param[out] seed_indices The selected leaf indices
353  */
354  void
355  selectInitialSupervoxelSeeds (std::vector<int> &seed_indices);
356 
357  /** \brief This method creates the internal supervoxel helpers based on the provided seed points
358  * \param[in] seed_indices Indices of the leaves to use as seeds
359  */
360  void
361  createSupervoxelHelpers (std::vector<int> &seed_indices);
362 
363  /** \brief This performs the superpixel evolution */
364  void
365  expandSupervoxels (int depth);
366 
367  /** \brief This sets the data of the voxels in the tree */
368  void
369  computeVoxelData ();
370 
371  /** \brief Reseeds the supervoxels by finding the voxel closest to current centroid */
372  void
373  reseedSupervoxels ();
374 
375  /** \brief Constructs the map of supervoxel clusters from the internal supervoxel helpers */
376  void
377  makeSupervoxels (std::map<uint32_t,typename Supervoxel<PointT>::Ptr > &supervoxel_clusters);
378 
379  /** \brief Stores the resolution used in the octree */
380  float resolution_;
381 
382  /** \brief Stores the resolution used to seed the superpixels */
383  float seed_resolution_;
384 
385  /** \brief Distance function used for comparing voxelDatas */
386  float
387  voxelDataDistance (const VoxelData &v1, const VoxelData &v2) const;
388 
389  /** \brief Transform function used to normalize voxel density versus distance from camera */
390  void
391  transformFunction (PointT &p);
392 
393  /** \brief Contains a KDtree for the voxelized cloud */
394  typename pcl::search::KdTree<PointT>::Ptr voxel_kdtree_;
395 
396  /** \brief Octree Adjacency structure with leaves at voxel resolution */
397  typename OctreeAdjacencyT::Ptr adjacency_octree_;
398 
399  /** \brief Contains the Voxelized centroid Cloud */
400  typename PointCloudT::Ptr voxel_centroid_cloud_;
401 
402  /** \brief Contains the Voxelized centroid Cloud */
403  typename NormalCloudT::ConstPtr input_normals_;
404 
405  /** \brief Importance of color in clustering */
406  float color_importance_;
407  /** \brief Importance of distance from seed center in clustering */
408  float spatial_importance_;
409  /** \brief Importance of similarity in normals for clustering */
410  float normal_importance_;
411 
412  /** \brief Whether or not to use the transform compressing depth in Z
413  * This is only checked if it has been manually set by the user.
414  * The default behavior is to use the transform for organized, and not for unorganized.
415  */
416  bool use_single_camera_transform_;
417  /** \brief Whether to use default transform behavior or not */
418  bool use_default_transform_behaviour_;
419 
420  /** \brief Internal storage class for supervoxels
421  * \note Stores pointers to leaves of clustering internal octree,
422  * \note so should not be used outside of clustering class
423  */
424  class SupervoxelHelper
425  {
426  public:
427  /** \brief Comparator for LeafContainerT pointers - used for sorting set of leaves
428  * \note Compares by index in the overall leaf_vector. Order isn't important, so long as it is fixed.
429  */
431  {
432  bool operator() (LeafContainerT* const &left, LeafContainerT* const &right) const
433  {
434  const VoxelData& leaf_data_left = left->getData ();
435  const VoxelData& leaf_data_right = right->getData ();
436  return leaf_data_left.idx_ < leaf_data_right.idx_;
437  }
438  };
439  using LeafSetT = std::set<LeafContainerT*, typename SupervoxelHelper::compareLeaves>;
440  using iterator = typename LeafSetT::iterator;
441  using const_iterator = typename LeafSetT::const_iterator;
442 
443  SupervoxelHelper (uint32_t label, SupervoxelClustering* parent_arg):
444  label_ (label),
445  parent_ (parent_arg)
446  { }
447 
448  void
449  addLeaf (LeafContainerT* leaf_arg);
450 
451  void
452  removeLeaf (LeafContainerT* leaf_arg);
453 
454  void
455  removeAllLeaves ();
456 
457  void
458  expand ();
459 
460  void
461  refineNormals ();
462 
463  void
464  updateCentroid ();
465 
466  void
467  getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const;
468 
469  void
470  getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const;
471 
472  using DistFuncPtr = float (SupervoxelClustering<PointT>::*)(const VoxelData &, const VoxelData &);
473 
474  uint32_t
475  getLabel () const
476  { return label_; }
477 
478  Eigen::Vector4f
479  getNormal () const
480  { return centroid_.normal_; }
481 
482  Eigen::Vector3f
483  getRGB () const
484  { return centroid_.rgb_; }
485 
486  Eigen::Vector3f
487  getXYZ () const
488  { return centroid_.xyz_;}
489 
490  void
491  getXYZ (float &x, float &y, float &z) const
492  { x=centroid_.xyz_[0]; y=centroid_.xyz_[1]; z=centroid_.xyz_[2]; }
493 
494  void
495  getRGB (uint32_t &rgba) const
496  {
497  rgba = static_cast<uint32_t>(centroid_.rgb_[0]) << 16 |
498  static_cast<uint32_t>(centroid_.rgb_[1]) << 8 |
499  static_cast<uint32_t>(centroid_.rgb_[2]);
500  }
501 
502  void
503  getNormal (pcl::Normal &normal_arg) const
504  {
505  normal_arg.normal_x = centroid_.normal_[0];
506  normal_arg.normal_y = centroid_.normal_[1];
507  normal_arg.normal_z = centroid_.normal_[2];
508  normal_arg.curvature = centroid_.curvature_;
509  }
510 
511  void
512  getNeighborLabels (std::set<uint32_t> &neighbor_labels) const;
513 
514  VoxelData
515  getCentroid () const
516  { return centroid_; }
517 
518  size_t
519  size () const { return leaves_.size (); }
520  private:
521  //Stores leaves
522  LeafSetT leaves_;
523  uint32_t label_;
525  SupervoxelClustering* parent_;
526  public:
527  //Type VoxelData may have fixed-size Eigen objects inside
529  };
530 
531  //Make boost::ptr_list can access the private class SupervoxelHelper
532 #if BOOST_VERSION >= 107000
533  friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering<PointT>::SupervoxelHelper *) BOOST_NOEXCEPT;
534 #else
535  friend void boost::checked_delete<> (const typename pcl::SupervoxelClustering<PointT>::SupervoxelHelper *);
536 #endif
537 
538  using HelperListT = boost::ptr_list<SupervoxelHelper>;
539  HelperListT supervoxel_helpers_;
540 
541  //TODO DEBUG REMOVE
542  StopWatch timer_;
543  public:
545  };
546 
547 }
548 
549 #ifdef PCL_NO_PRECOMPILE
550 #include <pcl/segmentation/impl/supervoxel_clustering.hpp>
551 #endif
A point structure representing normal coordinates and the surface curvature estimate.
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.
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.
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:344
A point structure representing Euclidean xyz coordinates, and the RGBA color.
Supervoxel container class - stores a cluster extracted using supervoxel clustering.
pcl::PointCloud< pcl::PointXYZRGBA >::Ptr getColoredVoxelCloud() const
Returns an RGB colorized voxelized cloud showing superpixels Otherwise it returns an empty pointer...
boost::shared_ptr< std::vector< int > > IndicesPtr
Defines all the PCL implemented PointT point type structures.
std::vector< LeafContainerT * > LeafVectorT
PCL base class.
Definition: pcl_base.h:69
boost::shared_ptr< KdTree< PointT, Tree > > Ptr
Definition: kdtree.h:78
VoxelData is a structure used for storing data within a pcl::octree::OctreePointCloudAdjacencyContain...
boost::shared_ptr< Supervoxel< PointT > > Ptr
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:444
void getCentroidPoint(PointXYZRGBA &centroid_arg)
Gets the centroid of the supervoxel.
boost::shared_ptr< OctreeAdjacencyT > 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
boost::shared_ptr< const PointCloud< PointT > > ConstPtr
Definition: point_cloud.h:445
Simple stopwatch.
Definition: time.h:58
SupervoxelClustering(float voxel_resolution, float seed_resolution, bool)
boost::shared_ptr< const Supervoxel< PointT > > ConstPtr
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::adjacency_list< boost::setS, boost::setS, boost::undirectedS, uint32_t, float > VoxelAdjacencyList
Define methods for measuring time spent in code blocks.
#define PCL_EXPORTS
Definition: pcl_macros.h:226
Defines all the PCL and non-PCL macros used.